def solve_system(code,A,B,x,y,z): ''' solves a trigonometric system of equations x = cos(t0)*(A*sin(t1) + B*cos(t1)) (1) y = sin(t0)*(A*sin(t1) + B*cos(t1)) (2) z = -B*sin(t1) + A*cos(t1) (3) for two angles t0 and t1 and returns a set of paired solutions as tulips (t0,t1) The system is over constrained so a solution may not exist. In thic case, an empty set is returned. ''' solution_set = [] t0 = [] t0.append(math.atan2(y, x)) t0.append(t0[0] + math.pi) t1 = solve_equation(1, [A, - B, - z]) for i in range(0,len(t0)): for j in range(0,len(t1)): q = A*math.sin(t1[j]) + B*math.cos(t1[j]) p = A*math.cos(t1[j]) - B*math.sin(t1[j]) c0 = math.cos(t0[i]) s0 = math.sin(t0[i]) if (gen.equal(x , c0*q) and gen.equal(y , s0*q) and gen.equal(z , p)): solution_set.append((t0[i],t1[j])) return solution_set
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 solve_system(code, A, B, x, y, z): ''' solves a trigonometric system of equations x = cos(t0)*(A*sin(t1) + B*cos(t1)) (1) y = sin(t0)*(A*sin(t1) + B*cos(t1)) (2) z = -B*sin(t1) + A*cos(t1) (3) for two angles t0 and t1 and returns a set of paired solutions as tulips (t0,t1) The system is over constrained so a solution may not exist. In thic case, an empty set is returned. ''' solution_set = [] t0 = [] t0.append(math.atan2(y, x)) t0.append(t0[0] + math.pi) t1 = solve_equation(1, [A, -B, -z]) for i in range(0, len(t0)): for j in range(0, len(t1)): q = A * math.sin(t1[j]) + B * math.cos(t1[j]) p = A * math.cos(t1[j]) - B * math.sin(t1[j]) c0 = math.cos(t0[i]) s0 = math.sin(t0[i]) if (gen.equal(x, c0 * q) and gen.equal(y, s0 * q) and gen.equal(z, p)): solution_set.append((t0[i], t1[j])) return solution_set
def writing_posture(self): if self.larm_startpoint == None: print "Left Arm is not calibrated!" else: pos = self.larm_startpoint if (self.larm_endpoint_width == None) or (self.larm_endpoint_height == None): ori = ps.rot.point_forward_orientation else: z = self.larm_endpoint_height - self.larm_startpoint self.box_height_larm = np.linalg.norm(z) z = z / self.box_height_larm y = self.larm_endpoint_width - self.larm_startpoint if y[1] < 0: y = y / self.box_width_larm else: y = -y / self.box_width_larm self.box_width_larm = np.linalg.norm(y) y = -y / self.box_width_larm x = np.cross(y, z) x = x / np.linalg.norm(x) # Correct z: z = np.cross(x, y) ori = np.append(np.append([y], [z], axis=0), [x], axis=0).T assert gen.equal(np.linalg.det(ori), 1.0) n = ori[:, 2] self.larm.set_target(pos - self.depth * n, ori) if self.rarm_startpoint == None: print "Right Arm is not calibrated!" else: pos = self.rarm_startpoint if (self.rarm_endpoint_width == None) or (self.rarm_endpoint_height == None): ori = ps.rot.point_forward_orientation else: z = self.rarm_endpoint_height - self.rarm_startpoint self.box_height_rarm = np.linalg.norm(z) z = z / self.box_height_rarm y = self.rarm_endpoint_width - self.rarm_startpoint self.box_width_rarm = np.linalg.norm(y) if y[1] < 0: y = -y / self.box_width_rarm else: y = y / self.box_width_rarm x = np.cross(y, z) x = x / np.linalg.norm(x) # Correct z: z = np.cross(x, y) ori = np.append(np.append([y], [z], axis=0), [x], axis=0).T assert gen.equal(np.linalg.det(ori), 1.0) n = ori[:, 2] self.rarm.set_target(pos - self.depth * n, ori) self.larm_target(wait=False) self.rarm_target(wait=False) pint.wait_until_finished(target_list=['rarm', 'larm']) self.sync_object() '''
def writing_posture(self): if self.larm_startpoint == None: print "Left Arm is not calibrated!" else: pos = self.larm_startpoint if (self.larm_endpoint_width == None) or (self.larm_endpoint_height == None): ori = ps.rot.point_forward_orientation else: z = self.larm_endpoint_height - self.larm_startpoint self.box_height_larm = np.linalg.norm(z) z = z/self.box_height_larm y = self.larm_endpoint_width - self.larm_startpoint if y[1] < 0: y = y/self.box_width_larm else: y = - y/self.box_width_larm self.box_width_larm = np.linalg.norm(y) y = - y/self.box_width_larm x = np.cross(y, z) x = x/np.linalg.norm(x) # Correct z: z = np.cross(x, y) ori = np.append(np.append([y],[z],axis = 0), [x], axis = 0).T assert gen.equal(np.linalg.det(ori), 1.0) n = ori[:,2] self.larm.set_target(pos - self.depth*n, ori) if self.rarm_startpoint == None: print "Right Arm is not calibrated!" else: pos = self.rarm_startpoint if (self.rarm_endpoint_width == None) or (self.rarm_endpoint_height == None): ori = ps.rot.point_forward_orientation else: z = self.rarm_endpoint_height - self.rarm_startpoint self.box_height_rarm = np.linalg.norm(z) z = z/self.box_height_rarm y = self.rarm_endpoint_width - self.rarm_startpoint self.box_width_rarm = np.linalg.norm(y) if y[1] < 0: y = - y/self.box_width_rarm else: y = y/self.box_width_rarm x = np.cross(y, z) x = x/np.linalg.norm(x) # Correct z: z = np.cross(x, y) ori = np.append(np.append([y],[z],axis = 0), [x], axis = 0).T assert gen.equal(np.linalg.det(ori), 1.0) n = ori[:,2] self.rarm.set_target(pos-self.depth*n, ori) self.larm_target(wait = False) self.rarm_target(wait = False) pint.wait_until_finished(target_list = ['rarm', 'larm']) self.sync_object() '''
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 intersection(self, line): M = np.array([[self.a, self.b],[line.a, line.b]]) c = np.array([- self.c, - line.c]) if gen.equal(np.linalg.det(M), 0.0): return None else: return Point_2D(np.dot(np.linalg.inv(M), c))
def ts_project(self, js_traj, phi_start = 0.0, phi_end = None): ''' projects the given jointspace trajectory into the taskspace The phase starts from phi_start and added by delta_phi in each step. if at any time the joint values are not feasible, the process is stopped. ''' if phi_end == None: phi_end = js_traj.phi_end pt = trajlib.Trajectory_Polynomial(dimension = 3*len(self.task_point) + 9*len(self.task_frame)) if phi_end > js_traj.phi_end: phi_end = js_traj.phi_end phi = phi_start stay = True while stay: if (phi > phi_end) or genmath.equal(phi, phi_end, epsilon = 0.1*self.ik_settings.time_step): phi = phi_end stay = False js_traj.set_phi(phi) if self.set_config(js_traj.current_position): pt.add_point(phi - phi_start, self.pose()) phi = phi + self.ik_settings.time_step return pt
def intersection(self, line): M = np.array([[self.a, self.b], [line.a, line.b]]) c = np.array([-self.c, -line.c]) if gen.equal(np.linalg.det(M), 0.0): return None else: return Point_2D(np.dot(np.linalg.inv(M), c))
def ts_project(self, js_traj, phi_start = 0.0, phi_end = None): ''' projects the given jointspace trajectory into the taskspace The phase starts from phi_start and added by delta_phi in each step. if at any time the joint values are not feasible, the process is stopped. ''' if phi_end == None: phi_end = js_traj.phi_end pt = trajlib.Trajectory_Polynomial(dimension = 3*len(self.task_point) + 9*len(self.task_frame)) if phi_end > js_traj.phi_end: phi_end = js_traj.phi_end phi = phi_start stay = True while stay: if (phi > phi_end) or genmath.equal(phi, phi_end, epsilon = 0.1*self.ik_settings.time_step): phi = phi_end stay = False js_traj.set_phi(phi) if self.set_config(js_traj.current_position): pt.add_point(phi - phi_start, self.pose()) phi = phi + self.ik_settings.time_step return pt
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 arc(traj=None, center_dw=0.1, center_dh=0.0, direction=None, angle=360.0, d_theta=10.0, clockwise=True, static_ends=True, in_degrees=True): ''' adds a set of points to the given trajectory establishing an arc in a plane parallel to the yz plane starting from initial_pos, The points follow an arc with given angle around the center input trj must be a Trajectory_Polynomial if trj is not given, a new trajectory will be created ''' if in_degrees: angle = angle * math.pi / 180.0 d_theta = d_theta * math.pi / 180.0 (trj, w, h, n, v) = decode_arguments(traj, direction, static_ends) if clockwise: sgn = 1 else: sgn = -1 center = center_dw * w + center_dh * h nseg = len(trj.segment) npnt = len(trj.segment[nseg - 1].point) initial_pos = np.copy(trj.segment[nseg - 1].point[npnt - 1].pos) initial_phi = trj.phi_end r = math.sqrt(center_dw**2 + center_dh**2) t0 = math.acos(-center_dw / r) t = 0.0 stay = True pos = np.zeros(3) while stay: t = t + d_theta if t > angle: t = angle pos = initial_pos + center + r * math.cos( sgn * t + t0) * w + r * math.sin(sgn * t + t0) * h vel = -r * sgn * math.sin(sgn * t + t0) * w + r * sgn * math.cos( sgn * t + t0) * h # trj.new_segment() trj.add_point(phi=initial_phi + t, pos=np.copy(pos), vel=vel) if gen.equal(t, angle): stay = False lsi = len(trj.segment) - 1 lpi = len(trj.segment[lsi].point) - 1 trj.segment[lsi].point[lpi].vel = v return trj
def positive(v, non_zero = False): permit = True n = len(v) i = 0 while permit and (i < n): permit = permit and (v[i] >= 0) and (not (non_zero and gen.equal(v[i], 0.0))) i += 1 return permit
def normalize(v): ''' Returns the unit vector parallel to the given vector. ''' l = np.linalg.norm(v) if gen.equal(l, 0.0): return(v) else: return(v/l)
def symbolic_term(a, x, plus_sign = True): if plus_sign and (a > 0): ps = '+ ' else: ps = '' if x == '': star = '' else: star = '*' if gen.equal(a , 0.0): s = '' elif gen.equal(a , 1.0) and (star == '*'): s = ps + x + ' ' elif gen.equal(a , - 1.0) and (star == '*'): s = '- ' + x + ' ' else: s = ps + str(a) + star + x return s
def symbolic_term(a, x, plus_sign=True): if plus_sign and (a > 0): ps = '+ ' else: ps = '' if x == '': star = '' else: star = '*' if gen.equal(a, 0.0): s = '' elif gen.equal(a, 1.0) and (star == '*'): s = ps + x + ' ' elif gen.equal(a, -1.0) and (star == '*'): s = '- ' + x + ' ' else: s = ps + str(a) + star + x return s
def zeta(self, phi = None): if phi == None: phi = self.angle() if gen.equal(phi, 0.0) and self.parametrization == 'identity': return self.m p = self.gen_fun(phi) return 2*math.tan(phi/2)*gen.inv(p)
def normalize(v): ''' Returns the unit vector parallel to the given vector. ''' l = np.linalg.norm(v) if gen.equal(l, 0.0): return (v) else: return (v / l)
def positive(v, non_zero=False): permit = True n = len(v) i = 0 while permit and (i < n): permit = permit and (v[i] >= 0) and (not (non_zero and gen.equal(v[i], 0.0))) i += 1 return permit
def zeta(self, phi=None): if phi == None: phi = self.angle() if gen.equal(phi, 0.0) and self.parametrization == 'identity': return self.m p = self.gen_fun(phi) return 2 * math.tan(phi / 2) * gen.inv(p)
def __init__(self, a=Point_2D(np.zeros(2)), b=Point_2D([1.0, 1.0]), c=None, representation='points'): ''' Equation of the line in 2D space: a*x + b*y + c = 0 ''' if representation == 'points': [x1, y1] = a.cartesian() [x2, y2] = b.cartesian() assert (not gen.equal(x1, x2)) or (not gen.equal( y1, y2)), "The two given points are identical" self.a = y2 - y1 self.b = x1 - x2 self.c = -x2 * self.a - y2 * self.b elif representation == 'point-angle': [x0, y0] = a.cartesian() if gen.equal(math.cos(b), 0.0): self.a = 1.0 self.b = 0.0 self.c = -x0 else: self.a = math.tan(b) self.b = -1.0 self.c = y0 - self.a * x0 elif representation == 'equation': self.a = a self.b = b self.c = c else: assert False, "Unknown representation" self.theta = math.atan2(-self.a, self.b) if gen.equal(self.b, 0.0): # Equation: x = h self.m = np.inf self.h = -self.c / self.a else: # Equation: y = m*x + h self.m = -self.a / self.b self.h = -self.c / self.b
def arcsincos(s, c): ''' return the angle that its sine is "s" and its cosine is "c" ''' assert gen.equal(s**2 + c**2, 1.0) if s < 0: theta = -arccos(c) else: theta = arccos(c) return theta
def arcsincos(s, c): ''' return the angle that its sine is "s" and its cosine is "c" ''' assert gen.equal(s**2 + c**2, 1.0) if s < 0: theta = - arccos(c) else: theta = arccos(c) return theta
def js_create(self, phi_end = None, traj_capacity = 2, traj_type = 'regular'): ''' ''' self.damping_factor = self.ik_settings.initial_damping_factor if self.ik_settings.generate_log: self.run_log.clear() keep_q = np.copy(self.q) H = self.transfer_matrices() if phi_end == None: phi_end = self.ik_settings.number_of_steps*self.ik_settings.time_step if traj_type == 'regular': jt = trajlib.Trajectory(dimension = self.config_settings.DOF, capacity = traj_capacity) elif traj_type == 'polynomial': jt = trajlib.Trajectory_Polynomial(dimension = self.config_settings.DOF, capacity = traj_capacity) else: assert False, "\n Unknown Trajectory Type \n" jt.plot_settings.axis_label = self.config_settings.joint_label if self.ik_settings.respect_limits_in_trajectories: jt.vel_max = self.ik_settings.max_js jt.acc_max = self.ik_settings.max_ja jt.pos_max = self.config_settings.qh jt.pos_min = self.config_settings.ql phi = 0.0 jt.add_position(0.0, pos = np.copy(self.q)) stay = not self.in_target() while stay: stay = not self.in_target() phi = phi + self.ik_settings.time_step if (phi > phi_end) or genmath.equal(phi, phi_end, epsilon = 0.1*self.ik_settings.time_step): phi = phi_end stay = False err_reduced = self.moveto_target() if err_reduced or (not self.ik_settings.respect_error_reduction): jt.add_position(phi = phi, pos = np.copy(self.q)) if traj_type == 'polynomial': jt.interpolate() self.set_config(keep_q) if self.ik_settings.generate_log: self.run_log.time_elapsed = time.time() - self.run_log.time_start return jt
def solve_equation(code, parameters): ''' Solves the trigonometric equation for t denoted by: code = 1: parameters should be: [p1, p2, p3] p1*cos(t) + p2*sin(t) + p3 = 0 code = 2: p1*cos^2(t) + p2*sin^2(t) + p3*sin(t)*cos(t) + p4*sin(t) + p5*cos(t) + p6 = 0 It returns the solution set as an array If no solution exists, it returns an empty array ''' solution_set = [] if code == 1: R = math.sqrt(parameters[0]**2 + parameters[1]**2) assert (not gen.equal(R, 0)) s = -parameters[2] / R if in_domain_sin(s): sai0 = math.atan2(parameters[0], parameters[1]) if gen.equal(math.sin(sai0), parameters[0] / R): sai = sai0 elif equal(math.sin(sai0), -parameters[0] / R): sai = sai0 + math.pi else: # If you came here, something is wrong ! assert (False) t1 = arcsin(s) - sai t2 = math.pi - t1 - 2 * sai if gen.equal( parameters[0] * math.cos(t1) + parameters[1] * math.sin(t1) + parameters[2], 0): solution_set.append(t1) if gen.equal( parameters[0] * math.cos(t2) + parameters[1] * math.sin(t2) + parameters[2], 0): solution_set.append(t2) return solution_set
def __init__(self, a = Point_2D(np.zeros(2)), b = Point_2D([1.0, 1.0]), c = None, representation = 'points'): ''' Equation of the line in 2D space: a*x + b*y + c = 0 ''' if representation == 'points': [x1, y1] = a.cartesian() [x2, y2] = b.cartesian() assert (not gen.equal(x1, x2)) or (not gen.equal(y1, y2)), "The two given points are identical" self.a = y2 - y1 self.b = x1 - x2 self.c = - x2*self.a - y2*self.b elif representation == 'point-angle': [x0, y0] = a.cartesian() if gen.equal(math.cos(b), 0.0): self.a = 1.0 self.b = 0.0 self.c = - x0 else: self.a = math.tan(b) self.b = - 1.0 self.c = y0 - self.a*x0 elif representation == 'equation': self.a = a self.b = b self.c = c else: assert False, "Unknown representation" self.theta = math.atan2(- self.a, self.b) if gen.equal(self.b, 0.0): # Equation: x = h self.m = np.inf self.h = - self.c/ self.a else: # Equation: y = m*x + h self.m = - self.a / self.b self.h = - self.c / self.b
def feasible_stepsize(direction, x, x_min, x_max): ''' when the correction of vector x is restricted by limits If you want to change vector x in a desired direction, and there are lower and upper bounds for x, how much are you allowed to change? This function returns a feasible stepsize for the given direction. The direction of change must be multiplied by a scalar value lower or equal to this feasible stepsize so that the applied changes are feasible. ''' valtyp = [np.ndarray, list, tuple] direction = genpy.check_type(direction, valtyp, __name__, function_name=sys._getframe().f_code.co_name, var_name='direction', shape_length=1) n = len(direction) [x, x_min, x_max] = genpy.check_types(variables=[x, x_min, x_max], type_lists=[valtyp, valtyp, valtyp], file_path=__name__, function_name=sys._getframe().f_code.co_name, var_names=['x', 'x_min', 'x_max'], shape_lengths=[1, 1, 1], array_lengths=[n, n, n]) etta = [] for i in range(n): if not gen.equal(direction[i], 0.0, epsilon=0.000000001): if x_min[i] == None: x_min[i] = -np.inf if x_max[i] == None: x_max[i] = np.inf a = (x_min[i] - x[i]) / direction[i] b = (x_max[i] - x[i]) / direction[i] etta.append(gen.sign_choice(b, a, direction[i])) else: etta.append(np.inf) if etta[len(etta) - 1] < 0: print 'x_min: ', x_min print 'x_max: ', x_max print 'x: ', x print 'dir: ', direction if etta == []: return 0.0 else: return min(etta)
def intersection(self, line): ''' a*x+b*y+c=0 y = m*x + h (x - x0)^2 + (m*x + h - y0)^2 = R^2 (1+m^2)*x^2 + 2*[m*(h-y0) - x0]*x + (h - y0)^2 - R^2 = 0 ''' [x0, y0] = self.center.cartesian() sol = [] h_y0 = line.h - y0 alpha = 1.0 + line.m*line.m if line.m == np.inf: delta = self.R*self.R - (x0 - line.h)**2 if gen.equal(delta , 0.0): sol.append(Point_2D(np.array([xp,y0]))) elif delta > 0: sd = math.sqrt(delta) sol.append(Point_2D(np.array([line.h, y0 - sd]))) sol.append(Point_2D(np.array([line.h, y0 + sd]))) else: betta = line.m*h_y0 - x0 gamma = x0*x0 + h_y0*h_y0 - self.R*self.R delta = betta*betta - alpha*gamma if gen.equal(delta , 0.0): x = - betta / alpha y = line.m*x + line.h sol.append(Point_2D(np.array([x,y]))) elif delta > 0: sd = math.sqrt(delta) x1 = (- betta - sd)/alpha y1 = line.m*x1 + line.h sol.append(Point_2D(np.array([x1,y1]))) x2 = (- betta + sd)/alpha y2 = line.m*x2 + line.h sol.append(Point_2D(np.array([x2,y2]))) return sol
def solve_equation(code, parameters): ''' Solves the trigonometric equation for t denoted by: code = 1: parameters should be: [p1, p2, p3] p1*cos(t) + p2*sin(t) + p3 = 0 code = 2: p1*cos^2(t) + p2*sin^2(t) + p3*sin(t)*cos(t) + p4*sin(t) + p5*cos(t) + p6 = 0 It returns the solution set as an array If no solution exists, it returns an empty array ''' solution_set = [] if code == 1: R = math.sqrt(parameters[0]**2 + parameters[1]**2) assert (not gen.equal(R, 0)) s = - parameters[2] / R if in_domain_sin(s): sai0 = math.atan2(parameters[0], parameters[1]) if gen.equal(math.sin(sai0), parameters[0]/R): sai = sai0 elif equal(math.sin(sai0), - parameters[0]/R): sai = sai0 + math.pi else: # If you came here, something is wrong ! assert(False) t1 = arcsin(s) - sai t2 = math.pi - t1 - 2*sai if gen.equal(parameters[0]*math.cos(t1) + parameters[1]*math.sin(t1) + parameters[2] , 0): solution_set.append(t1) if gen.equal(parameters[0]*math.cos(t2) + parameters[1]*math.sin(t2) + parameters[2] , 0): solution_set.append(t2) return solution_set
def intersection(self, line): ''' a*x+b*y+c=0 y = m*x + h (x - x0)^2 + (m*x + h - y0)^2 = R^2 (1+m^2)*x^2 + 2*[m*(h-y0) - x0]*x + (h - y0)^2 - R^2 = 0 ''' [x0, y0] = self.center.cartesian() sol = [] h_y0 = line.h - y0 alpha = 1.0 + line.m * line.m if line.m == np.inf: delta = self.R * self.R - (x0 - line.h)**2 if gen.equal(delta, 0.0): sol.append(Point_2D(np.array([xp, y0]))) elif delta > 0: sd = math.sqrt(delta) sol.append(Point_2D(np.array([line.h, y0 - sd]))) sol.append(Point_2D(np.array([line.h, y0 + sd]))) else: betta = line.m * h_y0 - x0 gamma = x0 * x0 + h_y0 * h_y0 - self.R * self.R delta = betta * betta - alpha * gamma if gen.equal(delta, 0.0): x = -betta / alpha y = line.m * x + line.h sol.append(Point_2D(np.array([x, y]))) elif delta > 0: sd = math.sqrt(delta) x1 = (-betta - sd) / alpha y1 = line.m * x1 + line.h sol.append(Point_2D(np.array([x1, y1]))) x2 = (-betta + sd) / alpha y2 = line.m * x2 + line.h sol.append(Point_2D(np.array([x2, y2]))) return sol
def tensor(self): if self.H == None: if gen.equal(self.angle(), 0.0) : return np.eye(3) p = self.vector() px = rot.skew(p) px2 = np.dot(px, px) p_nrm = np.linalg.norm(p) p2 = p_nrm*p_nrm p2_inv = gen.inv(p2) v = self.noo() z = self.zeta() m = self.moo() v2 = v*v v2_z = v2*gen.inv(z) self.H = m*np.eye(3) + 0.5*v2*px + p2_inv*(m - v2_z)*px2 return self.H
def tensor(self): if self.H == None: if gen.equal(self.angle(), 0.0): return np.eye(3) p = self.vector() px = rot.skew(p) px2 = np.dot(px, px) p_nrm = np.linalg.norm(p) p2 = p_nrm * p_nrm p2_inv = gen.inv(p2) v = self.noo() z = self.zeta() m = self.moo() v2 = v * v v2_z = v2 * gen.inv(z) self.H = m * np.eye(3) + 0.5 * v2 * px + p2_inv * (m - v2_z) * px2 return self.H
def draw_shape(self, shape_trajectory):# add wait argument later arm = self.reference_arm() keep_dt = arm.dt arm.dt = shape_trajectory.phi_end/100 jt = arm.js_project(shape_trajectory, relative = False, traj_type = 'polynomial') arm.dt = keep_dt # jt.fix_points() jt.consistent_velocities() # jt.plot() if not gen.equal(jt.phi_end, shape_trajectory.phi_end): print "Warning from PyRide_PR2.draw_shape(): All the shape is not in the workspace. Part of it will be drawn !" L = sum(shape_trajectory.points_dist()) ''' shape_trajectory.plot3d() for j in range(7): jt.plot(axis = j, show_points = True) ''' pint.run_config_trajectory(jt, duration = L/self.arm_speed, is_left_arm = self.larm_reference)
def joint_stepsize_interval(self, direction, max_speed = gen.infinity, delta_t = 0.001): etta_l = [] etta_h = [] # for ii in range(self.config_settings.njoint): for i in range(self.config_settings.DOF): if not gen.equal(direction[i], 0.0): if self.config_settings.limited[i] and self.config_settings.joint_handling[i] == 'NM': a = (self.ql[i] - self.q[i])/direction[i] b = (self.qh[i] - self.q[i])/direction[i] etta_l.append(gen.sign_choice(a, b, direction[i])) etta_h.append(gen.sign_choice(b, a, direction[i])) a = delta_t*max_speed/direction[i] etta_l.append(gen.sign_choice(-a, a, direction[i])) etta_h.append(gen.sign_choice( a,-a, direction[i])) if (etta_l == []) or (etta_h == []): return (0.0, 0.0) else: return (max(etta_l), min(etta_h))
def feasible_stepsize(direction, x, x_min, x_max): ''' when the correction of vector x is restricted by limits If you want to change vector x in a desired direction, and there are lower and upper bounds for x, how much are you allowed to change? This function returns a feasible stepsize for the given direction. The direction of change must be multiplied by a scalar value lower or equal to this feasible stepsize so that the applied changes are feasible. ''' valtyp = [np.ndarray, list, tuple] direction = genpy.check_type(direction, valtyp, __name__, function_name = sys._getframe().f_code.co_name, var_name = 'direction', shape_length = 1) n = len(direction) [x, x_min, x_max] = genpy.check_types(variables = [x,x_min,x_max], type_lists = [valtyp,valtyp,valtyp], file_path = __name__, function_name = sys._getframe().f_code.co_name, var_names = ['x', 'x_min', 'x_max'], shape_lengths = [1,1,1], array_lengths = [n,n,n]) etta = [] for i in range(n): if not gen.equal(direction[i], 0.0, epsilon = 0.000000001): if x_min[i] == None: x_min[i] = - np.inf if x_max[i] == None: x_max[i] = np.inf a = (x_min[i] - x[i])/direction[i] b = (x_max[i] - x[i])/direction[i] etta.append(gen.sign_choice(b, a, direction[i])) else: etta.append(np.inf) if etta[len(etta)-1] < 0: print 'x_min: ', x_min print 'x_max: ', x_max print 'x: ', x print 'dir: ', direction if etta == []: return 0.0 else: return min(etta)
def draw_shape(self, shape_trajectory): # add wait argument later arm = self.reference_arm() keep_dt = arm.dt arm.dt = shape_trajectory.phi_end / 100 jt = arm.js_project(shape_trajectory, relative=False, traj_type='polynomial') arm.dt = keep_dt # jt.fix_points() jt.consistent_velocities() # jt.plot() if not gen.equal(jt.phi_end, shape_trajectory.phi_end): print "Warning from PyRide_PR2.draw_shape(): All the shape is not in the workspace. Part of it will be drawn !" L = sum(shape_trajectory.points_dist()) ''' shape_trajectory.plot3d() for j in range(7): jt.plot(axis = j, show_points = True) ''' pint.run_config_trajectory(jt, duration=L / self.arm_speed, is_left_arm=self.larm_reference)
def run_config_trajectory(j_traj, duration = 10.0, dt = None, phi_dot = None, is_left_arm = False): global rarm_reached, rarm_failed global larm_reached, larm_failed if is_left_arm: larm_failed = False larm_reached = False else: rarm_failed = False rarm_reached = False phi = 0.0 dic_list = [] t = 0.0 if phi_dot == None: phi_dot = j_traj.phi_end/duration if dt == None: dt = 0.1*duration stay = True while stay: if (t > duration) or gen.equal(t, duration, epsilon = 0.1*dt): t = duration stay = False j_traj.set_phi(t*phi_dot) if is_left_arm: g = gen_larm_joint_posvel_dict(j_traj.current_position, j_traj.current_velocity*phi_dot, dt) else: g = gen_rarm_joint_posvel_dict(j_traj.current_position, j_traj.current_velocity*phi_dot, dt) dic_list.append(g) t = t + dt PyPR2.moveArmWithJointTrajectoryAndSpeed(dic_list)
def distance_from_ellipsoid(a, b, c, x0, y0, z0, t0 = None, silent = True, maximum = False): global max_iter ''' Find theta and phi that minimize function: f(theta, phi) = ( a*cos(theta)*cos(phi) - x0 )^2 + ( b*sin(theta)*cos(phi) - y0 )^2 + ( c*sin(phi) - z0 )^2 The method is numerical using the Hessian matrix. The iterations start from theta_0 and phi_0 ''' a2 = a*a b2 = b*b c2 = c*c if t0 == None: if gen.equal(x0, 0.0) and gen.equal(y0, 0.0) and gen.equal(z0, 0.0): if maximum: if max(a,b,c) == a: # To maximize the function, min must be replaced by max t0 = np.zeros(2) elif max(a,b,c) == b: t0 = np.array([math.pi/2, 0.0]) else: t0 = np.array([0.0, math.pi/2]) else: if min(a,b,c) == a: # To maximize the function, min must be replaced by max t0 = np.zeros(2) elif min(a,b,c) == b: t0 = np.array([math.pi/2, 0.0]) else: t0 = np.array([0.0, math.pi/2]) else: if maximum: ssign = - 1.0 else: ssign = 1.0 s = ssign/math.sqrt(x0*x0/a2 + y0*y0/b2 + z0*z0/c2) # To find the max distance s must be negative s = -1.0/math.sqrt(...) the = math.atan2(a*y0, b*x0) phi = math.atan2(z0*s/c, s*math.sqrt(x0*x0/a2 + y0*y0/b2)) assert gen.equal(a*math.cos(the)*math.cos(phi), x0*s) assert gen.equal(b*math.sin(the)*math.cos(phi), y0*s) assert gen.equal(c*math.sin(phi), z0*s) t0 = np.array([the,phi]) H = np.zeros((2,2)) df = np.zeros(2) t = t0 stay = True cnt = 0 while stay and (cnt < max_iter): ct = math.cos(t[0]) cf = math.cos(t[1]) st = math.sin(t[0]) sf = math.sin(t[1]) ct2 = ct*ct st2 = st*st cf2 = cf*cf sf2 = sf*sf c2t = ct2 - st2 c2f = cf2 - sf2 s2t = 2*st*ct s2f = 2*sf*cf df[0] = 0.5*(b2 - a2)*s2t*cf2 + cf*(a*x0*st - b*y0*ct) df[1] = - 0.5*s2f*(a2*ct2 + b2*st2 - c2) + sf*(a*x0*ct + b*y0*st) - c*z0*cf stay = not gen.equal(np.linalg.norm(df), 0.0) if stay: H[0,0] = (b2 - a2)*c2t*cf2 + cf*(a*x0*ct + b*y0*st) H[0,1] = 0.5*(a2 - b2)*s2t*s2f - sf*(a*x0*st - b*y0*ct) H[1,0] = H[0,1] H[1,1] = - c2f*(a2*ct2 + b2*st2 - c2) + cf*(a*x0*ct + b*y0*st) + c*z0*sf t = t - np.dot(np.linalg.inv(H), df) cnt += 1 if not silent: print print 'iteration:', cnt, ' cost function value:', (a*ct*cf - x0)**2 + (b*st*cf - y0)**2 + (c*sf - z0)**2, ' norm(div_f):', np.linalg.norm(df) return (t[0], t[1])
def possess(self, point): x = point.cartesian() return gen.equal(self.a * x[0] + self.b * x[1] + self.c, 0.0)
def js_project(self, pos_traj, ori_traj = None, phi_start = 0.0, phi_end = None, relative = True, traj_capacity = 2, traj_type = 'regular'): ''' projects the given taskspace pose trajectory into the jointspace using the numeric inverse kinematics. The phase starts from phi_start and added by self.ik_settings.time_step in each step. at any time, if a solution is not found, the target is ignored and no configuration is added to the trajectory ''' self.damping_factor = self.ik_settings.initial_damping_factor keep_q = np.copy(self.q) H = self.transfer_matrices() if ori_traj == None: ori_traj = trajlib.Orientation_Path() ori_traj.add_point(0.0, self.task_frame[0].orientation(H)) ori_traj.add_point(pos_traj.phi_end, self.task_frame[0].orientation(H)) if phi_end == None: phi_end = min(pos_traj.phi_end, ori_traj.phi_end) if (phi_end > pos_traj.phi_end) or (phi_end > ori_traj.phi_end): phi_end = min(pos_traj.phi_end, ori_traj.phi_end) if traj_type == 'regular': jt = trajlib.Trajectory(dimension = self.config_settings.DOF, capacity = traj_capacity) elif traj_type == 'polynomial': jt = trajlib.Trajectory_Polynomial(dimension = self.config_settings.DOF, capacity = traj_capacity) else: assert False, "\n Unknown Trajectory Type \n" if self.ik_settings.respect_limits_in_trajectories: jt.vel_max = self.ik_settings.max_js jt.acc_max = self.ik_settings.max_ja jt.pos_max = self.config_settings.qh jt.pos_min = self.config_settings.ql phi = phi_start pos_traj.set_phi(phi) ori_traj.set_phi(phi) if relative: p0 = self.task_point[0].position(H) - pos_traj.current_position self.set_target([self.task_point[0].position(H)], [self.task_frame[0].orientation(H)]) else: p0 = np.zeros(3) self.set_target([pos_traj.current_position], [ori_traj.current_orientation]) self.goto_target() jt.add_position(0.0, pos = np.copy(self.q)) if self.ik_settings.generate_log: self.run_log.clear() self.run_log.step.append(self.report_status()) stay = True while stay: phi = phi + self.ik_settings.time_step if (phi > phi_end) or genmath.equal(phi, phi_end, epsilon = 0.1*self.ik_settings.time_step): phi = phi_end stay = False pos_traj.set_phi(phi) ori_traj.set_phi(phi) p = p0 + pos_traj.current_position self.set_target([p], [ori_traj.current_orientation]) err_reduced = self.moveto_target() if err_reduced or (not self.ik_settings.respect_error_reduction): jt.add_position(phi = phi - phi_start, pos = np.copy(self.q)) if traj_type == 'polynomial': jt.interpolate() self.set_config(keep_q) if self.ik_settings.generate_log: self.run_log.time_elapsed = time.time() - self.run_log.time_start return jt
def possess(self, point): x = point.cartesian() - self.center.cartesian() r2 = sum(x * x) return gen.equal(r2, self.R * self.R)
def possess(self, point): x = point.cartesian() return gen.equal(self.a*x[0] + self.b*x[1] + self.c, 0.0)
def possess(self, point): x = point.cartesian() - self.center.cartesian() r2 = sum(x*x) return gen.equal(r2, self.R*self.R)
def js_project(self, pos_traj, ori_traj = None, phi_start = 0.0, phi_end = None, relative = True, traj_capacity = 2, traj_type = 'regular'): ''' projects the given taskspace pose trajectory into the jointspace using the numeric inverse kinematics. The phase starts from phi_start and added by self.ik_settings.time_step in each step. at any time, if a solution is not found, the target is ignored and no configuration is added to the trajectory ''' self.damping_factor = self.ik_settings.initial_damping_factor keep_q = np.copy(self.q) H = self.transfer_matrices() if ori_traj == None: ori_traj = trajlib.Orientation_Path() ori_traj.add_point(0.0, self.task_frame[0].orientation(H)) ori_traj.add_point(pos_traj.phi_end, self.task_frame[0].orientation(H)) if phi_end == None: phi_end = min(pos_traj.phi_end, ori_traj.phi_end) if (phi_end > pos_traj.phi_end) or (phi_end > ori_traj.phi_end): phi_end = min(pos_traj.phi_end, ori_traj.phi_end) if traj_type == 'regular': jt = trajlib.Trajectory(dimension = self.config_settings.DOF, capacity = traj_capacity) elif traj_type == 'polynomial': jt = trajlib.Trajectory_Polynomial(dimension = self.config_settings.DOF, capacity = traj_capacity) else: assert False, "\n Unknown Trajectory Type \n" if self.ik_settings.respect_limits_in_trajectories: jt.vel_max = self.ik_settings.max_js jt.acc_max = self.ik_settings.max_ja jt.pos_max = self.config_settings.qh jt.pos_min = self.config_settings.ql phi = phi_start pos_traj.set_phi(phi) ori_traj.set_phi(phi) if relative: p0 = self.task_point[0].position(H) - pos_traj.current_position self.set_target([self.task_point[0].position(H)], [self.task_frame[0].orientation(H)]) else: p0 = np.zeros(3) self.set_target([pos_traj.current_position], [ori_traj.current_orientation]) self.goto_target() jt.add_position(0.0, pos = np.copy(self.q)) if self.ik_settings.generate_log: self.run_log.clear() self.run_log.step.append(self.report_status()) stay = True while stay: phi = phi + self.ik_settings.time_step if (phi > phi_end) or genmath.equal(phi, phi_end, epsilon = 0.1*self.ik_settings.time_step): phi = phi_end stay = False pos_traj.set_phi(phi) ori_traj.set_phi(phi) p = p0 + pos_traj.current_position self.set_target([p], [ori_traj.current_orientation]) err_reduced = self.moveto_target() if err_reduced or (not self.ik_settings.respect_error_reduction): jt.add_position(phi = phi - phi_start, pos = np.copy(self.q)) if traj_type == 'polynomial': jt.interpolate() self.set_config(keep_q) if self.ik_settings.generate_log: self.run_log.time_elapsed = time.time() - self.run_log.time_start return jt
def possess(self, point): x_c = point.cartesian() - self.center.cartesian() d = np.dot(np.dot((x_c).T, self.M), x_c) return gen.equal(d, 1.0)
def rotation_matrix(rv, parametrization = 'unit_quaternion', symbolic = False, derivative = False, C = None, S = None, U = None, W = None): ''' This function returns the rotation matrix corresponding to rotation vector as p(phi)*U where: phi is the rotation angle and U is the unit vector directing towards the axis of rotation If type is "Linear Parametrization", then: p(phi) = sin(phi) if "parametrization = 'unit_quaternion', a unit quaternion must be given. The first element rv[0] is the scalar part and the next three elements (rv[1], rv[2], rv[3]) represent the vectorial part if argument derivative is True the derivative of the rotation matrix wrt angle phi (rv[0]) is return. Arguments "symbolic" and "derivative" are now supported only for angle-axis parametrization. C = cos(phi) , S = sin(phi) when phi is the rotation angle and [Ux, Uy, Uz] is the axis ''' if symbolic: from sympy import Symbol C = gen.replace_if_none(C, Symbol('c')) S = gen.replace_if_none(S, Symbol('s')) U = gen.replace_if_none(U, [Symbol('ux'), Symbol('uy'), Symbol('uz')]) W = gen.replace_if_none(W, Symbol('w')) u = numpy.zeros((3)) if parametrization == 'angular_spherical': sin_phi = math.sin(rv[0]) cos_phi = math.cos(rv[0]) u[0] = math.sin(rv[1])*math.cos(rv[2]) u[1] = math.sin(rv[1])*math.sin(rv[2]) u[2] = math.cos(rv[1]) elif parametrization == 'angle_axis': if symbolic: sin_phi = S cos_phi = C u[0] = U[0] u[1] = U[1] u[2] = U[2] else: sin_phi = math.sin(rv[0]) cos_phi = math.cos(rv[0]) u[0] = rv[1] u[1] = rv[2] u[2] = rv[3] elif parametrization == 'unit_quaternion': assert len(rv) == 4 assert gen.equal(numpy.linalg.norm(rv), 1.0) RM = numpy.eye(3) a2 = rv[0]**2 b2 = rv[1]**2 c2 = rv[2]**2 d2 = rv[3]**2 ab = 2*rv[0]*rv[1] ac = 2*rv[0]*rv[2] ad = 2*rv[0]*rv[3] bc = 2*rv[1]*rv[2] bd = 2*rv[1]*rv[3] cd = 2*rv[2]*rv[3] RM[0,0] = a2 + b2 - c2 - d2 RM[0,1] = bc - ad RM[0,2] = bd + ac RM[1,0] = bc + ad RM[1,1] = a2 - b2 + c2 - d2 RM[1,2] = cd - ab RM[2,0] = bd - ac RM[2,1] = cd + ab RM[2,2] = a2 - b2 - c2 + d2 return(RM) else: p = numpy.linalg.norm(rv) u = rv/p if parametrization == 'vectorial_identity': #p = math.pi + angle_in_range(p) sin_phi = math.sin(p) cos_phi = math.cos(p) elif parametrization == 'vectorial_linear': assert (p <= 1) sin_phi = p cos_phi = math.sqrt(1 - p*p) elif parametrization == 'vectorial_reduced_Euler_Rodrigues': assert (p <= 2) sin_phi_2 = p/2 cos_phi_2 = math.sqrt(1 - p*p/4) sin_phi = 2*sin_phi_2*cos_phi_2 cos_phi = 2*cos_phi_2*cos_phi_2 - 1 elif parametrization == 'vectorial_Cayley_Gibbs_Rodrigues': t2 = p*p/4 sin_phi = p/(1 + t2) cos_phi = (1 - t2)/(1 + t2) elif parametrization == 'vectorial_Wiener_Milenkovic': phi_4 = math.atan(p/4) sin_phi = math.sin(4*phi_4) cos_phi = math.cos(4*phi_4) u = vectors_and_matrices.normalize(u) skew_u = vectors_and_matrices.skew(u) if derivative: RM = cos_phi*skew_u + sin_phi*numpy.dot(skew_u,skew_u) else: RM = numpy.eye(3) + sin_phi*skew_u + (1 - cos_phi)*numpy.dot(skew_u,skew_u) return(RM)
def possess(self, point): x_c = point.cartesian() - self.center.cartesian() d = np.dot(np.dot((x_c).T, self.M), x_c) return gen.equal(d, 1.0)