def basis_error(self, current, target): ''' return the value of basis_position_error ''' ers = "Target has not been set" assert target != None, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers) if self.settings.representation == 'Cartesian Coordinates': err = current - target f = err**self.settings.power elif self.settings.representation == 'Cylindrical_Coordinates': p = 3 assert len(self.power) == p f = numpy.zeros((p)) assert False elif self.settings.representation == 'Spherical_Coordinates': p = 3 assert len(self.power) == p f = numpy.zeros((p)) assert False else: ers = self.settings.representation + " is an invalid value for property self.settings.representation" assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers) return f
def error_jacobian(self): if self.err_jac == None: ''' Calculates and updates the error jacobian of the endeffector according to the actual and target poses of the task_points and task_frames and their corresponding metrics ''' cnt = 0 #Creation of the error jacobian matrix" self.err_jac = numpy.zeros((self.mp + self.mo, self.config_settings.DOF)) H = self.transfer_matrices() #For task_points for tp in self.task_point: ers = "Target has not been set for some of the taskpoints" assert tp.rd != None, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers) # Calculating error jacobian for each task_point tp_ej = tp.error_jacobian(H, self.ajac) tp_er = tp.error.value(tp.position(H), tp.rd) #Arranging error jacobian for position for k in range(0,len(tp_er)): for j in range(0, self.config_settings.DOF): self.err_jac[cnt,j] = self.jmc_c[j]*tp_ej[k,j] cnt = cnt + 1 #For task_frames for tf in self.task_frame: ers = "Target has not been set for some of the taskframes" assert tf.rd != None, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers) # Calculating error jacobian for each reference_orientation tf_ej = tf.error_jacobian(self.ajac) tf_er = tf.error.value(tf.orientation(H), tf.rd) #Arranging error jacobian for orientation for k in range(0,len(tf_er)): for j in range(0, self.config_settings.DOF): self.err_jac[cnt,j] = self.jmc_c[j]*tf_ej[k,j] cnt = cnt + 1 return self.err_jac
def create(self): print "Workspace Creation Started. Creation Method: ",self.ws_settings.creation_method if self.ws_settings.creation_method == 'JSG': self.create_JSG() elif self.ws_settings.creation_method == 'RND': self.create_random() elif self.ws_settings.creation_method == 'PCL': if not self.ws_settings.predefined_config_list == None: i = 0 p = len(self.ws_settings.predefined_config_list) for q in self.ws_settings.predefined_config_list: print 'Creating Workspace .... ' + str(i) + ' out of: ' + str (p) qf = self.free_config(q) assert self.set_config(qf) self.config_list.append(qf) ef_pose = self.pose_to_tuple('actual', self.ws_settings.representation_of_orientation) self.pose_list.append(ef_pose) self.ws_settings.number_of_configs = len(self.config_list) pose_set = set(self.pose_list) config_tuple_list = [ tuple(c) for c in self.config_list ] config_set = set(config_tuple_list) self.pose_tree = kdtree.KDTree.construct_from_data(list(pose_set)) self.config_tree = kdtree.KDTree.construct_from_data(list(config_set)) else: assert False, genpy.err_str(__name__, self.__class__.__name__, 'create', 'predefined_config_list has not been set. Workspace can NOT be created !') else: assert False, genpy.err_str(__name__, self.__class__.__name__, 'create', self.ws_settings.creation_method+ ' is not a valid value for creation_method')
def basis_error(self, current, target) : ''' return the value of basis_position_error ''' ers = "Target has not been set" assert target != None, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers) if self.settings.representation == 'Cartesian Coordinates': err = current - target f = err**self.settings.power elif self.settings.representation == 'Cylindrical_Coordinates': p = 3 assert len(self.power) == p f = numpy.zeros((p)) assert False elif self.settings.representation == 'Spherical_Coordinates': p = 3 assert len(self.power) == p f = numpy.zeros((p)) assert False else: ers = self.settings.representation + " is an invalid value for property self.settings.representation" assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers) return f
def unit_quaternion(TRM): ''' Returns a vector (4 X 1) containing elements of the unit quaternion corresponding to transformation or rotation matrix TRM. (TRM can be the 4*4 transformation matrix or 3*3 rotation matrix) the first element of the output vector contains the scalar part of the unit quaternion and the last three elements represent the vectorial part ''' assert gen.equal(numpy.linalg.det(TRM[0:3, 0:3]), 1.0), genpy.err_str( __name__, self.__class__.__name__, sys._getframe().f_code.co_name, "Given TRM is not a rotation matrix.") uqn = numpy.zeros((4)) uqn[0] = 1.00 [u, v, w] = permutation_uvw(TRM) p = 1.0 + TRM[u, u] - TRM[v, v] - TRM[w, w] if (p > gen.epsilon): p = math.sqrt(p) uqn[u + 1] = p / 2.0 uqn[v + 1] = (TRM[v, u] + TRM[u, v]) / (2 * p) uqn[w + 1] = (TRM[w, u] + TRM[u, w]) / (2 * p) uqn[0] = (TRM[w, v] - TRM[v, w]) / (2 * p) if uqn[0] < 0: uqn = -uqn assert gen.equal(numpy.linalg.norm(uqn), 1.0), "Impossible !" return uqn
def update_virtual_jacobian_multipliers(self): ''' joint_limit_multipliers are coefficients of forward and inverse mapping to/from limited to/from unlimited jointspace. i.e: jmc_a is the derivative of q to q_star (Refer also to the comments of method: initialize in this class) This method calculates and updates the values of one of the joint limit multipliers: "jmc_c" which is called "joint_limit_jacobian_multipliers" Each column of the error jacobian is multiplied by the corresponding element of vector "jmc_c": Jstar = Je * diag(jmc_c) Since the coefficient "jmc_c" for a joint, depends on the current value of that joint, it should be updated everytime "q" changes The other multipliers: "jmc_a", "jmc_b", "jmc_f" and "jmc_g" do not depend on the joints, therefore do not need to be updated ''' func_name = "update_virtual_jacobian_multipliers" for i in range(0,self.config_settings.DOF): if self.config_settings.joint_handling[i] == 'NM': self.jmc_c[i] = 1.0 elif self.config_settings.joint_handling[i] == 'LM': self.jmc_c[i] = 1.0 / self.jmc_a[i] elif self.config_settings.joint_handling[i] == 'TM': self.jmc_c[i] = - math.sin(self.qvr[i])/self.jmc_a[i] elif self.config_settings.joint_handling[i] == 'TGM': t = self.jmc_f[i] * math.cos(self.qvr[i]) + self.jmc_g[i] self.jmc_c[i] = - 2.0 * math.sin(self.qvr[i])/(self.jmc_a[i]*(1 + t**2)) else: assert False, genpy.err_str(__name__, self.__class__.__name__, 'update_virtual_jacobian_multipliers', self.config_settings.joint_handling[i] + " is not a valid value for joint_handling")
def update(self, current, target) : ''' Calculates the error vector between the actual and desired orientations of the corresponding taskframe ''' be = self.basis_error(current, target) assert self.settings.weight.shape[1] == len(be) self.current_value = numpy.dot(self.settings.weight, be) + self.settings.offset if self.settings.precision_base == 'Axis Angle': self.is_in_target = True for k in range(0,3): if self.settings.required_identical_coordinate[k]: cos_teta = numpy.dot(target.frame_axis(k), current.frame_axis(k)) if (cos_teta < 1.00000) and (cos_teta > -1.00000): deviation = abs((180.0/math.pi)*math.acos(cos_teta)) self.is_in_target = self.is_in_target and (deviation < self.settings.precision) # print "deviation = ", k, deviation, self.settings.precision, self.is_in_target self.is_in_target = self.is_in_target and (cos_teta > -1.00000) # print self.is_in_target elif self.settings.precision_base == 'Error Function': self.is_in_target = (numpy.linalg.norm(self.current_value) < self.settings.precision) else: assert False, genpy.err_str(__name__, self.__class__.__name__,'update', self.settings.precision_base + ' is not a valid value for precision_base')
def set_config(self, qd, set_virtual = True): len_qd = len(qd) assert len_qd == self.config_settings.DOF, genpy.err_str(__name__, self.__class__.__name__,'set_config','Length of given config vector is '+str(len_qd)+' which does not match the settings DOF('+str(self.config_settings.DOF)+')') permission = not self.config_settings.joint_limits_respected if not permission: permission = self.joints_in_range(qd) if permission: j = 0 for jj in range(0,self.config_settings.njoint): if self.config_settings.free[jj]: if self.config_settings.limited[j] and (not self.config_settings.prismatic[jj]): self.q[jj] = trig.angle_standard_range(qd[j]) else: self.q[jj] = qd[j] j = j + 1 self.mapto_virtual() self.update_virtual_jacobian_multipliers() return True else: print "Warning from configuration.set_config(): Given joints are not in their feasible range" for i in range(self.config_settings.DOF): if not self.joint_in_range(i, qd[i]): print print "Joint Number: ", i print "Lower Limit :", self.ql[i] print "Desired Value:", qd[i] print "Upper Limit :", self.qh[i] return False
def __init__(self, M=np.eye(3), center=Point_3D(pos=np.zeros(3))): ''' For example to have a non-rotated ellipsoid centered at the origin with equation: x^2/a^2 + y^2/b^2 + z^2/c^2 = 1 Arguments M and center must be selected as: [ a^(-2) 0 0 ] M = [ 0 b^(-2) 0 ] [ 0 0 c^(-2) ] center = (0, 0, 0) ''' # Checking arguments: func_name = sys._getframe().f_code.co_name genpy.check_type(M, [np.ndarray], __name__, self.__class__.__name__, func_name, 'M', shape=(3, 3)) (landa, V) = np.linalg.eig(M) # V is a rotation that transforms from principal coordinate system to the actual coordinate system # if x is a vector in principal CS and x' is its representation in actual CS: x' = V*x and x = V^T*x' assert vm.positive(landa, non_zero=True) and gen.equal( np.linalg.norm(V.imag), 0.0), genpy.err_str( __name__, self.__class__.__name__, func_name, 'Matrix M must have real and positive eigen values') # self.M = M self.center = center self.a = math.sqrt(1.0 / landa[0]) self.b = math.sqrt(1.0 / landa[1]) self.c = math.sqrt(1.0 / landa[2]) self.R = V.real
def mapto_virtual(self): qf = self.free_config(self.q) self.qvr = np.zeros((self.config_settings.DOF)) for i in range(self.config_settings.DOF): if (not self.config_settings.limited) or (self.config_settings.joint_handling[i] == 'NM'): self.qvr[i] = qf[i] elif self.config_settings.joint_handling[i] == 'LM': assert self.joint_in_range(i, qf[i]), "Joint " + str(i) + " = " + str(qf[i]) + " is not in feasible range: (" + str(self.config_settings.ql[i]) + ',' + str(self.config_settings.qh[i]) + ')' self.qvr[i] = self.jmc_a[i] * qf[i] + self.jmc_b[i] elif self.config_settings.joint_handling[i] == 'TM': ''' ql = self.config_settings.ql[i] qh = self.config_settings.qh[i] print "i: ", i, "ql: ", ql," q: ", qf[i],"qh: ", qh, " g: ", self.jmc_g[i], " f: ", self.jmc_f[i], " c: ", (qf[i] - self.jmc_g[i]) / self.jmc_f[i] ''' assert self.joint_in_range(i, qf[i]), "Joint " + str(i) + " = " + str(qf[i]) + " is not in feasible range: (" + str(self.config_settings.ql[i]) + ',' + str(self.config_settings.qh[i]) + ')' self.qvr[i] = trig.arccos((qf[i] - self.jmc_g[i]) / self.jmc_f[i]) elif self.config_settings.joint_handling[i] == 'TGM': assert self.joint_in_range(i, qf[i]), "Joint " + str(i) + " = " + str(qf[i]) + " is not in feasible range: (" + str(self.config_settings.ql[i]) + ',' + str(self.config_settings.qh[i]) + ')' self.qvr[i] = trig.arccos((math.tan(0.5*qf[i]) - self.jmc_g[i]) / self.jmc_f[i]) else: print 'Wrong Joint Handling (Free Joint ' + str(i) + '): ' + self.config_settings.joint_handling[i] assert False, genpy.err_str(__name__, self.__class__.__name__, 'q_to_q', self.config_settings.joint_handling[i] + " is not a valid value for joint_handling")
def set_acceleration(self, ori_dd, representation=None): self.clear_acceleration() if representation == 'vector': self.pdd = oridd else: assert False, genpy.err_str( __name__, self.__class__.__name__, '__getitem__', representation + ' is not a valid value for representation')
def set_acceleration(self, value, representation): self.clear_acceleration() if representation == 'cartesian': self.xdd = value elif representation == 'polar': self.pdd = value else: assert False, genpy.err_str(__name__ , self.__class__.__name__ , 'set_acceleration', representation + ' is not a valid value for representation')
def interpolate_smart(self, points): n = len(points) m = 0 for p in points: m = m + p.count_constraints() self.degree = m - 1 A = numpy.zeros((m, m)) i = 0 u = numpy.array([]) for p in points: if p.x != None: A[i, 0] = 1 for j in range(1, m): A[i, j] = A[i, j - 1] * p.t i = i + 1 u = numpy.append(u, p.x) if p.v != None: A[i, 0] = 0.0 A[i, 1] = 1.0 for j in range(2, m): A[i, j] = j * (p.t**(j - 1)) i = i + 1 u = numpy.append(u, p.v) if p.a != None: A[i, 0:2] = 0.0 A[i, 2] = 2.0 for j in range(3, m): A[i, j] = j * (j - 1) * (p.t**(j - 2)) i = i + 1 u = numpy.append(u, p.a) try: self.coeff = numpy.dot(numpy.linalg.inv(A), u) except: print genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, 'The matrix of coefficients is singular') print "Matrix of coefficients:" print A print print "Determinent = ", numpy.linalg.det(A) print assert False
def goto_target(self): ''' ''' if self.ik_settings.generate_log: self.run_log.clear() if self.ik_settings.include_current_config: self.initial_config = self.free_config(self.q) if self.in_target(): return True if self.ik_settings.run_mode == 'binary_run': self.run_binary(True) elif self.ik_settings.run_mode == 'normal_run': if self.ik_settings.algorithm in ["JT","JPI","DLS(CDF)", "JI"]: self.run() elif self.ik_settings.algorithm == "DLS(ADF)": self.run_dls_vdf() else: assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, self.ik_settings.algorithm + " is not a valid value for algorithm") self.start_node = 0 p = len(self.initial_config_list) while (not self.in_target()) and (self.start_node < p): assert self.set_config(self.initial_config_list[self.start_node]) self.initial_config = self.free_config(self.q) # Just for keeping the information of the initial status (Initial status will be shown later in the results log file) # self.forward_update() if self.ik_settings.run_mode == 'binary_run': self.run_binary(True) elif self.ik_settings.run_mode == 'normal_run': if self.ik_settings.algorithm in ["JT","JPI","DLS(CDF)", "JI"]: self.run() elif self.ik_settings.algorithm == "DLS(ADF)": self.run_dls_vdf() else: assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, self.ik_settings.algorithm + ' is not a valid value for algorithm') self.start_node += 1 return self.in_target()
def goto_target(self): ''' ''' if self.ik_settings.generate_log: self.run_log.clear() if self.ik_settings.include_current_config: self.initial_config = self.free_config(self.q) if self.in_target(): return True if self.ik_settings.run_mode == 'binary_run': self.run_binary(True) elif self.ik_settings.run_mode == 'normal_run': if self.ik_settings.algorithm in ["JT","JPI","DLS(CDF)", "JI", "DLS(MADF)"]: self.run() elif self.ik_settings.algorithm == "DLS(ADF)": self.run_dls_vdf() else: assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, self.ik_settings.algorithm + " is not a valid value for algorithm") self.start_node = 0 p = len(self.initial_config_list) while (not self.in_target()) and (self.start_node < p): assert self.set_config(self.initial_config_list[self.start_node]) self.initial_config = self.free_config(self.q) # Just for keeping the information of the initial status (Initial status will be shown later in the results log file) # self.forward_update() if self.ik_settings.run_mode == 'binary_run': self.run_binary(True) elif self.ik_settings.run_mode == 'normal_run': if self.ik_settings.algorithm in ["JT","JPI","DLS(CDF)", "JI", "DLS(MADF)"]: self.run() elif self.ik_settings.algorithm == "DLS(ADF)": self.run_dls_vdf() else: assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, self.ik_settings.algorithm + ' is not a valid value for algorithm') self.start_node += 1 return self.in_target()
def interpolate_smart(self, points): n = len(points) m = 0 for p in points: m = m + p.count_constraints() self.degree = m - 1 A = numpy.zeros((m,m)) i = 0 u = numpy.array([]) for p in points: if p.x != None: A[i, 0] = 1 for j in range(1, m): A[i, j] = A[i, j-1]*p.t i = i + 1 u = numpy.append(u, p.x) if p.v != None: A[i, 0] = 0.0 A[i, 1] = 1.0 for j in range(2, m): A[i, j] = j*(p.t**(j-1)) i = i + 1 u = numpy.append(u, p.v) if p.a != None: A[i, 0:2] = 0.0 A[i, 2] = 2.0 for j in range(3, m): A[i, j] = j*(j-1)*(p.t**(j-2)) i = i + 1 u = numpy.append(u, p.a) try: self.coeff = numpy.dot(numpy.linalg.inv(A), u) except: print genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, 'The matrix of coefficients is singular') print "Matrix of coefficients:" print A print print "Determinent = ", numpy.linalg.det(A) print assert False
def set_velocity(self, value, representation): self.clear_velocity() if representation == 'cartesian': self.pd = value elif representation == 'spherical': self.sd = value elif representation == 'cylindrical': self.cd = value else: assert False, genpy.err_str(__name__ , self.__class__.__name__ , 'set_velocity', representation + ' is not a valid value for representation')
def set_acceleration(self, value, representation): self.clear_acceleration() if representation == 'cartesian': self.xdd = value elif representation == 'polar': self.pdd = value else: assert False, genpy.err_str( __name__, self.__class__.__name__, 'set_acceleration', representation + ' is not a valid value for representation')
def set_acceleration(self, value, representation = None): self.clear_acceleration() if representation == None: representation = self.representation if representation == 'vector': self.pdd = value elif representation == 'matrix': self.Rdd = value else: assert False, genpy.err_str(__name__ , self.__class__.__name__ , 'set_acceleration', representation + ' is not a valid value for representation')
def optimum_stepsize(self, direction): if self.ik_settings.algorithm in ['JPI','JI']: k = 1.0 elif self.ik_settings.algorithm in ['JT', 'DLS(CDF)', 'DLS(ADF)', 'DLS(MADF)']: J_delta_q = np.dot(self.error_jacobian(), direction) k = - np.sum(J_delta_q*J_delta_q)/np.sum(J_delta_q*self.pose_error()) else: assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, self.ik_settings.algorithm + " is an unknown value for algorithm") # assert k >= 0.0 return k
def optimum_stepsize(self, direction): if self.ik_settings.algorithm in ['JPI','JI']: k = 1.0 elif self.ik_settings.algorithm in ['JT', 'DLS(CDF)', 'DLS(ADF)', 'DLS(MADF)']: J_delta_q = np.dot(self.error_jacobian(), direction) k = - np.sum(J_delta_q*J_delta_q)/np.sum(J_delta_q*self.pose_error()) else: assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, self.ik_settings.algorithm + " is an unknown value for algorithm") # assert k >= 0.0 return k
def set_acceleration(self, value, representation=None): self.clear_acceleration() if representation == None: representation = self.representation if representation == 'vector': self.pdd = value elif representation == 'matrix': self.Rdd = value else: assert False, genpy.err_str( __name__, self.__class__.__name__, 'set_acceleration', representation + ' is not a valid value for representation')
def set_velocity(self, value, representation): self.clear_velocity() if representation == 'cartesian': self.pd = value elif representation == 'spherical': self.sd = value elif representation == 'cylindrical': self.cd = value else: assert False, genpy.err_str( __name__, self.__class__.__name__, 'set_velocity', representation + ' is not a valid value for representation')
def __setitem__(self, representation, value): self.clear() if representation == 'cartesian': self.x = value elif representation == 'polar': ''' p[0] = r p[1] = theta ''' self.p = value else: assert False, genpy.err_str(__name__ , self.__class__.__name__ , '__setitem__', representation + ' is not a valid value for representation')
def pose_error(self): if self.current_pose_error == None: ''' Calculates and updates the current values of position and orientation errors between the actual and target poses of task_points and task_frames Also updates the values of "pose_error" and "error_norm" ''' #Creation of the vector of errors cnt = 0 p = self.pose() # This should change self.num_task_constraints() self.current_pose_error = numpy.zeros((self.mp + self.mo)) #For task_points self.all_task_points_in_target = True H = self.transfer_matrices() for tp in self.task_point: ers = "Target has not been set for some of the taskpoints" assert tp.rd != None, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers) # Calculating position error for each task_point self.all_task_points_in_target = self.all_task_points_in_target and tp.error.in_target(tp.position(H), tp.rd) #Arranging the vector of errors for position tp_err = tp.error.value(tp.ra, tp.rd) for k in range(0,len(tp_err)): self.current_pose_error[cnt] = tp_err[k] cnt = cnt + 1 #For task_frames self.all_task_frames_in_target = True for tf in self.task_frame: ers = "Target has not been set for some of the taskframes" assert tf.rd != None, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers) # Calculating orientation error for each task_frame self.all_task_frames_in_target = self.all_task_frames_in_target and tf.error.in_target(tf.orientation(H), tf.rd) #Arranging the vector of errors for orientation" tf_err = tf.error.value(tf.ra, tf.rd) for k in range(0,len(tf_err)): self.current_pose_error[cnt] = tf_err[k] cnt = cnt + 1 self.is_in_target = self.all_task_points_in_target and self.all_task_frames_in_target return self.current_pose_error
def parameter_value(self, parameter): if parameter == 'AS': return self.application_scenario elif parameter == 'AL': return self.ik_settings.algorithm elif parameter == 'MAN': return self.geo_settings.name elif parameter == 'KC': return str(self.NKC()) elif parameter == 'MNI': return str(self.ik_settings.number_of_steps) elif parameter == 'PP': return str(1000*self.end_settings.precision_for_position) elif parameter == 'PO': return str(self.end_settings.precision_for_rotation) elif parameter == 'PM': if self.num_position_ref == 0: return 'NA' else: return self.position_constraint elif parameter == 'OM': if self.num_orientation_ref == 0: return 'NA' else: return self.orientation_constraint elif parameter == 'JM': if 'TM' in self.config_settings.joint_handling: return 'TM' elif 'LM' in self.config_settings.joint_handling: return 'LM' else: return 'NM' # return genpy.most_common(self.config_settings.joint_handling) elif parameter == 'DF': if self.ik_settings.algorithm[0:3] == 'DLS': return str(self.ik_settings.initial_damping_factor) else: return 'NA' elif parameter == 'DFG': if self.ik_settings.algorithm == 'DLS(ADF)': return str(self.ik_settings.df_gain) elif self.ik_settings.algorithm == 'DLS(MADF)': return str(self.ik_settings.manipulability_threshold) else: return 'NA' elif parameter == 'WCTP': return self.workspace_settings_tp.gen_key() elif parameter == 'WCIC': return self.workspace_settings_ic.gen_key() else: assert False, genpy.err_str(__name__, self.__class__.__name__, 'parameter_value', parameter + ' is an Unknown Setting Parameter')
def nearest_configs(self, ik): if self.ws_settings.search_criteria == 'NTP': # (key: NTP) Initial Configuration: Nearest to Target Pose nearest_config_list = self.configs_nearest_to_pose(ik) elif self.ws_settings.search_criteria == 'NCC': # (key: NCC) Initial Configuration: Nearest to Current Configuration nearest_config_list = self.configs_nearest_to(ik.free_config(ik.q)) elif self.ws_settings.search_criteria == 'CLO': nearest_config_list = [] for i in range(self.ws_settings.number_of_search_items): nearest_config_list.append(self.config_list[i]) else : assert False, genpy.err_str(__name__, self.__class__.__name__, 'nearest_configs', self.ws_settings.search_criteria + ' is not a valid value for search_criteria') return nearest_config_list
def set_config_virtual(self, qvrd): self.qvr = qvrd qd = self.mapfrom_virtual(qvrd) permission = not self.config_settings.joint_limits_respected if not permission: permission = self.joints_in_range(qd) if permission: self.q = self.free_config_inv(qd) self.update_virtual_jacobian_multipliers() return True else: assert False, genpy.err_str(__name__, self.__class__.__name__, 'set_config_virtual', 'The joint values computed from qvr are out of range while joint limits are respected.') return False
def __setitem__(self, representation, value): self.clear() if representation == 'matrix': self.R = value elif representation == 'quaternion': self.Q = value elif representation == 'angle_axis': self.phi = value[0] self.u = value[1:4] elif representation == 'vector': self.p = value else: assert False, genpy.err_str(__name__ , self.__class__.__name__ , '__setitem__', representation + ' is not a valid value for representation')
def __setitem__(self, representation, value): self.clear() if representation == 'cartesian': self.x = value elif representation == 'polar': ''' p[0] = r p[1] = theta ''' self.p = value else: assert False, genpy.err_str( __name__, self.__class__.__name__, '__setitem__', representation + ' is not a valid value for representation')
def __getitem__(self, representation): if representation == 'cartesian': return self.cartesian() elif representation == 'cartesian_velocity': return self.cartesian_velocity() elif representation == 'cartesian_acceleration': return self.cartesian_acceleration() elif representation == 'polar': return self.polar() elif representation == 'polar_velocity': return self.polar_velocity() elif representation == 'polar_acceleration': return self.polar_acceleration() else: assert False, genpy.err_str(__name__ , self.__class__.__name__ , '__getitem__', representation + ' is not a valid value for representation')
def __setitem__(self, representation, value): self.clear() if representation == 'matrix': self.R = value elif representation == 'quaternion': self.Q = value elif representation == 'angle_axis': self.phi = value[0] self.u = value[1:4] elif representation == 'vector': self.p = value else: assert False, genpy.err_str( __name__, self.__class__.__name__, '__setitem__', representation + ' is not a valid value for representation')
def __getitem__(self, representation): if representation == 'cartesian': return self.cartesian() elif representation == 'cartesian_velocity': return self.cartesian_velocity() elif representation == 'cartesian_acceleration': return self.cartesian_acceleration() elif representation == 'polar': return self.polar() elif representation == 'polar_velocity': return self.polar_velocity() elif representation == 'polar_acceleration': return self.polar_acceleration() else: assert False, genpy.err_str( __name__, self.__class__.__name__, '__getitem__', representation + ' is not a valid value for representation')
def objective_function_gradient(self, k = 1.0): dof = self.config_settings.DOF qh = self.qh ql = self.ql u = np.zeros(dof) qs = trig.angles_standard_range(self.qvr) for i in range(0, self.config_settings.DOF): if self.config_settings.joint_handling[i] == 'TM': u[i] = - k*qs[i]/(dof*((2*math.pi)**2)) elif self.config_settings.joint_handling[i] == 'NM': if self.config_settings.limited[i]: mean = 0.5*(self.qh[i] + self.ql[i]) u[i] = - k*(qs[i] - mean)/(dof*((self.qh[i] - self.ql[i])**2)) # print "qs[i] = ", qs[i], " mean = ", mean, " qh-ql= ", self.qh[i] - self.ql[i], " u = ", u[i] else: assert False, genpy.err_str(__name__, self.__class__.__name__, 'objective_function_gradient', self.config_settings.joint_handling[i] + " is not a valid value for joint_handling") return u
def __getitem__(self, representation): if representation == 'matrix': return self.matrix() elif representation == 'quaternion': return self.quaternion() elif representation == 'angle_axis': return self.angle_axis() elif representation == 'angle': return self.angle() elif representation == 'axis': return self.axis() elif representation == 'vector': return self.vector() elif representation == 'trace': return np.trace(self.matrix()) elif representation == 'diag': return np.diag(self.matrix()) elif representation == 'angular_spherical': return self.spherical() elif representation == 'matrix_velocity': return self.matrix_velocity() elif representation == 'quaternion_velocity': return self.quaternion_velocity() elif representation == 'angle_axis_velocity': u = self.axis_velocity() return np.array([self.angle_velocity(), u[0], u[1], u[2]]) elif representation == 'angle_velocity': return self.angle_velocity() elif representation == 'axis_velocity': return self.axis_velocity() elif representation == 'vector_velocity': return self.vector_velocity() elif representation == 'vector_acceleration': return self.vector_acceleration() elif representation == 'trace_velocity': return np.trace(self.matrix_velocity()) elif representation == 'diag_velocity': return np.diag(self.matrix_velocity()) else: assert False, genpy.err_str( __name__, self.__class__.__name__, '__getitem__', representation + ' is not a valid value for representation')
def value(self, figure): if figure == 'Time': return self.current_time elif figure == 'Duration': return self.time_elapsed elif figure == 'Phase': return self.phase elif figure == 'Manipulability': return self.manipulability elif figure == 'Damping Factor': return self.damping_factor elif figure == 'In Range': return self.joints_in_range elif figure == 'Target Reached': return self.is_in_target elif figure == 'Error Norm': return self.error_norm else: ers = figure + ' is not a valid value for argument figure' assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers)
def objective_function_value(self, k = 1.0): dof = self.config_settings.DOF qs = trig.angles_standard_range(self.qvr) qh = self.qh ql = self.ql f = 0.0 for i in range(0, self.config_settings.DOF): df = 0.0 if self.config_settings.joint_handling[i] == 'TM': f += - k*qs[i]/(dof*((2*math.pi)**2)) elif self.config_settings.joint_handling[i] == 'NM': if self.config_settings.limited[i]: mean = 0.5*(self.qh[i] + self.ql[i]) df = k*((qs[i] - mean)**2)/(dof*((self.qh[i] - self.ql[i])**2)) # print "qs[i] = ", qs[i], " mean = ", mean, " qh-ql= ", self.qh[i] - self.ql[i], " df = ", df else: assert False, genpy.err_str(__name__, self.__class__.__name__, 'objective_function_value', self.config_settings.joint_handling[i] + " is not a valid value for joint_handling") f += df return f
def mapfrom_virtual(self, qs): qq = np.zeros((self.config_settings.DOF)) for i in range(self.config_settings.DOF): if self.config_settings.limited[i]: if self.config_settings.joint_handling[i] == 'NM': qq[i] = trig.angle_standard_range( qs[i] ) elif self.config_settings.joint_handling[i] == 'LM': qs[i] = trig.angle_standard_range( qs[i] ) qq[i] = (qs[i] - self.jmc_b[i]) / self.jmc_a[i] elif self.config_settings.joint_handling[i] == 'TM': qq[i] = self.jmc_f[i] * math.cos(qs[i]) + self.jmc_g[i] elif self.config_settings.joint_handling[i] == 'TGM': qq[i] = 2*math.atan(self.jmc_f[i] * math.cos(qs[i]) + self.jmc_g[i]) else: assert False, genpy.err_str(__name__, self.__class__.__name__, 'mapfrom_virtual', self.config_settings.joint_handling[i] + " is not a valid value for joint_handling") else: qq[i] = qs[i] # return self.free_config_inv(qq) return qq
def __setitem__(self, representation, value): self.clear() if representation == 'cartesian': self.p = value elif representation == 'spherical': ''' s[0] = ro s[1] = theta s[2] = phi ''' self.s = value elif representation == 'cylindrical': ''' c[0] = r c[1] = theta c[2] = z ''' self.c = value else: assert False, genpy.err_str(__name__ , self.__class__.__name__ , '__setitem__', representation + ' is not a valid value for representation')
def value(self, figure): if figure == 'Time': return self.current_time elif figure == 'Duration': return self.time_elapsed elif figure == 'Phase': return self.phase elif figure == 'Manipulability': return self.manipulability elif figure == 'Damping Factor': return self.damping_factor elif figure == 'In Range': return self.joints_in_range elif figure == 'Target Reached': return self.is_in_target elif figure == 'Error Norm': return self.error_norm else: ers = figure + ' is not a valid value for argument figure' assert False, genpy.err_str(__name__, self.__class__.__name__, sys._getframe().f_code.co_name, ers)
def __getitem__(self, representation): if representation == 'matrix': return self.matrix() elif representation == 'quaternion': return self.quaternion() elif representation == 'angle_axis': return self.angle_axis() elif representation == 'angle': return self.angle() elif representation == 'axis': return self.axis() elif representation == 'vector': return self.vector() elif representation == 'trace': return np.trace(self.matrix()) elif representation == 'diag': return np.diag(self.matrix()) elif representation == 'angular_spherical': return self.spherical() elif representation == 'matrix_velocity': return self.matrix_velocity() elif representation == 'quaternion_velocity': return self.quaternion_velocity() elif representation == 'angle_axis_velocity': u = self.axis_velocity() return np.array([self.angle_velocity(), u[0], u[1], u[2]]) elif representation == 'angle_velocity': return self.angle_velocity() elif representation == 'axis_velocity': return self.axis_velocity() elif representation == 'vector_velocity': return self.vector_velocity() elif representation == 'vector_acceleration': return self.vector_acceleration() elif representation == 'trace_velocity': return np.trace(self.matrix_velocity()) elif representation == 'diag_velocity': return np.diag(self.matrix_velocity()) else: assert False, genpy.err_str(__name__ , self.__class__.__name__ , '__getitem__', representation + ' is not a valid value for representation')
def __setitem__(self, representation, value): self.clear() if representation == 'cartesian': self.p = value elif representation == 'spherical': ''' s[0] = ro s[1] = theta s[2] = phi ''' self.s = value elif representation == 'cylindrical': ''' c[0] = r c[1] = theta c[2] = z ''' self.c = value else: assert False, genpy.err_str( __name__, self.__class__.__name__, '__setitem__', representation + ' is not a valid value for representation')
def update(self, current, target): ''' Calculates the error vector between the actual and desired orientations of the corresponding taskframe ''' be = self.basis_error(current, target) assert self.settings.weight.shape[1] == len(be) self.current_value = numpy.dot(self.settings.weight, be) + self.settings.offset if self.settings.precision_base == 'Axis Angle': self.is_in_target = True for k in range(0, 3): if self.settings.required_identical_coordinate[k]: cos_teta = numpy.dot(target.frame_axis(k), current.frame_axis(k)) if (cos_teta < 1.00000) and (cos_teta > -1.00000): deviation = abs( (180.0 / math.pi) * math.acos(cos_teta)) self.is_in_target = self.is_in_target and ( deviation < self.settings.precision) # print "deviation = ", k, deviation, self.settings.precision, self.is_in_target self.is_in_target = self.is_in_target and (cos_teta > -1.00000) # print self.is_in_target elif self.settings.precision_base == 'Error Function': self.is_in_target = (numpy.linalg.norm(self.current_value) < self.settings.precision) else: assert False, genpy.err_str( __name__, self.__class__.__name__, 'update', self.settings.precision_base + ' is not a valid value for precision_base')
def __init__(self, M = np.eye(3), center = Point_3D(pos = np.zeros(3))): ''' For example to have a non-rotated ellipsoid centered at the origin with equation: x^2/a^2 + y^2/b^2 + z^2/c^2 = 1 Arguments M and center must be selected as: [ a^(-2) 0 0 ] M = [ 0 b^(-2) 0 ] [ 0 0 c^(-2) ] center = (0, 0, 0) ''' # Checking arguments: func_name = sys._getframe().f_code.co_name genpy.check_type(M, [np.ndarray], __name__, self.__class__.__name__, func_name, 'M', shape = (3,3)) (landa, V) = np.linalg.eig(M) # V is a rotation that transforms from principal coordinate system to the actual coordinate system # if x is a vector in principal CS and x' is its representation in actual CS: x' = V*x and x = V^T*x' assert vm.positive(landa, non_zero = True) and gen.equal(np.linalg.norm(V.imag) , 0.0), genpy.err_str(__name__, self.__class__.__name__, func_name, 'Matrix M must have real and positive eigen values') # self.M = M self.center = center self.a = math.sqrt(1.0/landa[0]) self.b = math.sqrt(1.0/landa[1]) self.c = math.sqrt(1.0/landa[2]) self.R = V.real
def basis_error(self, current, target) : # def basis_orientation_error(self, current, target): rpn = self.settings.representation if rpn in ['vector', 'ReRoAn + ReOrVe']: current.set_parametrization( self.settings.parametrization ) target.set_parametrization( self.settings.parametrization ) ''' ''' if self.settings.metric_type == 'relative': Oe = current/target e = Oe[rpn] elif self.settings.metric_type == 'differential': e = current[rpn] - target[rpn] elif self.settings.metric_type == 'special': if self.settings.representation == 'AxInPr': p = 3 assert len(self.settings.power) == p f = numpy.zeros((p)) for k in range (0,p): if self.settings.power[k] == 0: err = 0 f[k] = 1 else: err = numpy.dot(target.frame_axis(k), current.frame_axis(k)) if self.settings.power[k] == 1 : f[k] = err else: f[k] = err**self.settings.power[k] elif self.settings.representation == 'DiNoQu': p = 3 assert len(self.settings.power) == p f = numpy.zeros((p)) qn = current['quaternion'] qnd = target['quaternion'] for k in range (0,3): z = - qnd[0]*qn[k+1] + qn[0]*qnd[k+1] f[k] = z**self.settings.power[k] elif self.settings.representation == 'AxInPr + DiNoQu': p = 6 assert len(self.settings.power) == p f = numpy.zeros((p)) for k in range (0,3): if self.settings.power[k] == 0: err = 0 f[k] = 1 else: err = numpy.dot(target.frame_axis(k), current.frame_axis(k)) if self.settings.power[k] == 1 : f[k] = err else: f[k] = err**self.settings.power[k] qn = current['quaternion'] qnd = target['quaternion'] for k in range (3,6): z = - qnd[0]*qn[k-2] + qn[0]*qnd[k-2] f[k] = z**self.settings.power[k] elif self.settings.representation == 'ReRoAn + ReOrVe': p = 4 assert len(self.settings.power) == p e = numpy.zeros((p)) Oe = current/target e[0] = Oe['angle'] e[1:4] = Oe['vector'] f = e**self.settings.power else: assert False return f else: assert False, genpy.err_str(__name__, self.__class__.__name__, "basis_error", self.settings.metric_type + " is an invalid metric type") if self.settings.representation == 'matrix': e = e.flatten() f = e**self.settings.power return f
def __init__(self, representation = "Cartesian Coordinates", metric_type = "differential"): assert (representation in representations_of_position) or (representation in representations_of_orientation) or (representation in special_metric_representations), genpy.err_str(__name__,self.__class__.__name__,'__init__', representation+' is an invalid value for representation') self.metric_type = metric_type self.representation = representation self.parametrization = 'identity' ''' Property "power" determines the power of elements of basis error in the final vector of errors: Please refer to the comments of property: "W" in this file. 0: Not considered for the corresponding coordinate 1: rd - ra 2: (rd - ra)^2 3: (rd - ra)^3 . . . where: "ra" is the actual position "rd" is the desired position and r can be replaced by: X, Y or Z ''' self.power = numpy.array([1, 1, 1]) ''' property "W" is a 3 X 3 Weighting matrix. This matrix will be multiplied by the basis error and basis jacobian. Matrix W is by default the identity matrix defining 3 constraints as: Xd - X = 0 Yd - Y = 0 Zd - Z = 0 Example 1: "power" array = [1, 1, 1] and "W" is a (3 X 3) matrix X Y Z 1st Row of "W": 1 0 0 (Xd - Xa) = 0 2nd Row of "W": 0 1 0 (Yd - Ya) = 0 3rd Row of "W": 0 0 1 (Zd - Za) = 0 Example 2: "power" array = [2, 2, 1] and "W" is a (2 X 3) matrix X Y Z 1st Row of "W": 1 1 0 (Xd - Xa)^2 + (Yd - Ya)^2 = 0 2nd Row of "W": 0 0 1 (Zd - Za) = 0 Example 3: (When only two coordinations "X" and "Z" are important to be identical) In this example, it is important to also set the property "required_identical_coordinate" to: [True, False, True] Please refer to the of comments for property "required_identical_coordinate". "power" array = [1, 0, 1] and "W" is a (2 X 3) matrix X Y Z 1st Row of "W": 1 0 0 (Xd - Xa) = 0 2nd Row of "W": 0 0 1 (Zd - Za) = 0 the same for the orientation ... ''' self.weight = numpy.eye(3) self.offset = numpy.zeros((3)) # for orientation: if self.representation in ['diag', 'AxInPr']: self.offset = numpy.array([-1.0, -1.0, -1.0]) elif self.representation in ['trace', 'angle']: self.weight = numpy.array([[1]]) self.power = numpy.array([1]) if self.representation == 'trace': self.offset = numpy.array([-3.0]) else: self.offset = numpy.array([0.0]) else: self.offset = numpy.zeros(3) ''' "self.required_identical_coordinate" is an array of booleans and indicates which coordinates (x, y ,z) should be considered as a criteria in determining the confirmity of the actual and desired positions and orientations. For example if "required_identical_coordinate" = [True, False, True] the actual and desired positions are considered as identical only when their "x" and "z" coordinates are equal. (the "y" coordinate will be neglected) Please refer to "example 3" of the comments provided for property: "W" in this file. ''' self.required_identical_coordinate = [True, True, True] ''' set "precision" or termination criteria by default as: 2 cm. It means actual and desired positions are considered "identical" if for each position coordinate (x, y, z), the absolute value of the difference of current and desired, does not exceed: 2 cm. ''' if representation in representations_of_position: self.precision = 0.01 self.precision_base = "Coordinate Difference" else: self.precision = 1.0 self.precision_base = "Axis Angle"
def set_acceleration(self, ori_dd, representation = None): self.clear_acceleration() if representation == 'vector': self.pdd = oridd else: assert False, genpy.err_str(__name__ , self.__class__.__name__ , '__getitem__', representation + ' is not a valid value for representation')
def initialize(self): ''' Everything regarding joint parameters needed to be done before running kinematic calculations This function must be called whenever you change joint limits or change type of a joint. ''' func_name = "initialize()" # Counting free joints in order to detremine the degree of freedom: "DOF" self.config_settings.DOF = 0 # Important: Here you do not consider prismatic joints. Do it in the future for i in range(0,self.config_settings.njoint): # First bring revolute joint limits in the range (- pi , pi) if (not self.config_settings.prismatic[i]): self.config_settings.ql[i] = trig.angle_standard_range( self.config_settings.ql[i] ) self.config_settings.qh[i] = trig.angle_standard_range( self.config_settings.qh[i] ) if self.config_settings.free[i]: self.config_settings.DOF += 1 self.ql = self.free_config(self.config_settings.ql) self.qh = self.free_config(self.config_settings.qh) # check all the joints are already in their desired range ''' for i in range(self.DOF): if not self.config_settings.joint_handling[i] == 'No Mapping': assert self.joints_in_range() #check only for joint i ''' ''' The following code defines the joint limit multipliers jmc_a and jmc_b are auxiliary coefficients which are used for conversion from "q" to "qvr" space (Please refer to the joint limits section of the documentation associated with this code) To make sure that all joints are in their desired range identified as: (self.ql, self.qh), the real jointspace is mapped into an unlimited jointspace. "q " represent the real joint values which are in a limited jointpace "qvr" represent the mapped joint values in unlimited jointspace The following code creates and defines "Joint Limit Multipliers". These are coefficients for this jointspace mapping - (jmc: Jointspace Mapping Coefficients) in linear mapping: q = a * qvr + b qs = (q - b) / a in trigonometric mapping: q = f * cos(qvr) + g qvr = arccos [ (q - g) / f ] a, b, f and g are calculated in such a way that the real joint values which are in their feasible range are mapped into an equivalent qvr in an unlimited space (-pi, +pi) "jmc_c" is multiplied by the columns of the error jacobian matrix ''' self.jmc_a = np.zeros((self.config_settings.DOF)) self.jmc_b = np.zeros((self.config_settings.DOF)) self.jmc_c = np.zeros((self.config_settings.DOF)) self.jmc_f = np.zeros((self.config_settings.DOF)) self.jmc_g = np.zeros((self.config_settings.DOF)) self.jmc_f = 0.5*(self.qh - self.ql) self.jmc_g = 0.5*(self.qh + self.ql) for i in range(0, self.config_settings.DOF): if self.config_settings.joint_handling[i] == 'NM': self.jmc_a[i] = 1.00000 self.jmc_b[i] = 0.00000 elif self.config_settings.joint_handling[i] == 'LM': self.jmc_a[i] = 2*math.pi/(self.qh[i] - self.ql[i]) self.jmc_b[i] = math.pi * (self.qh[i] + self.ql[i])/(self.ql[i] - self.qh[i]) elif self.config_settings.joint_handling[i] == 'TM': self.jmc_a[i] = 2.0/(self.qh[i] - self.ql[i]) self.jmc_b[i] = (self.qh[i] + self.ql[i])/(self.ql[i] - self.qh[i]) elif self.config_settings.joint_handling[i] == 'TGM': th = math.tan(0.5*self.qh[i]) tl = math.tan(0.5*self.ql[i]) self.jmc_a[i] = 2/(th - tl) self.jmc_b[i] = (tl + th)/(tl - th) self.jmc_f[i] = 0.5*(th - tl) self.jmc_g[i] = 0.5*(th + tl) else: assert False, genpy.err_str(__name__, self.__class__.__name__, 'initialize', self.config_settings.joint_handling[i] + " is not a valid value for joint_handling") # map q to qvr for the first time: self.mapto_virtual() # assign the value of jmc_c the jacobian multiplier: self.update_virtual_jacobian_multipliers()