def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' a = joint_angle x = self.links[joint_name][0] y = self.links[joint_name][1] z = self.links[joint_name][2] c = cos(a) s = sin(a) matrix_x = matrix([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [x, y, z, 1]]) matrix_y = matrix([[0, 1, 0, 0], [0, c, -s, 0], [-s, 0, c, 0], [x, y, z, 1]]) matrix_z = matrix([[c, s, 0, 0], [-s, c, 0, 0], [0, 0, 1, 0], [x, y, z, 1]]) if 'Roll' in joint_name: #x return matrix_x elif 'Pitch' in joint_name: #y return matrix_y else: return matrix_z
def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' T = identity(4) # YOUR CODE HERE s = math.sin(joint_angle) c = math.cos(joint_angle) r_x = matrix([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [0, 0, 0, 1]]) r_z = matrix([[c, s, 0, 0], [-s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) r_y = matrix([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0], [0, 0, 0, 1]]) if "Yaw" in joint_name: T *= r_z if "Pitch" in joint_name: T *= r_y if "Roll" in joint_name: T *= r_x T[0, 3] = self.jointLength[joint_name][0] T[1, 3] = self.jointLength[joint_name][1] T[2, 3] = self.jointLength[joint_name][2] return T
def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' T = identity(4) # YOUR CODE HERE R_x = identity(4) R_y = identity(4) R_z = identity(4) sin_angle = sin(joint_angle) cos_angle = cos(joint_angle) x = self.link_offsets[joint_name][0] y = self.link_offsets[joint_name][1] z = self.link_offsets[joint_name][2] if 'Roll' in joint_name: R_x = matrix([[1, 0, 0, 0], [0, cos_angle, -sin_angle, 0], [0, sin_angle, cos_angle, 0], [x, y, z, 1]]) elif 'Pitch' in joint_name: R_y = matrix([[cos_angle, 0, sin_angle, 0], [0, 1, 0, 0], [-sin_angle, 0, cos_angle, 0], [x, y, z, 1]]) elif 'Yaw' in joint_name: R_z = matrix([[cos_angle, sin_angle, 0, 0], [-sin_angle, cos_angle, 0, 0], [0, 0, 1, 0], [x, y, z, 1]]) T = R_x * R_y * R_z return T
def test_basic(self): # test basic construction T = Transform() assert_allclose(T, np.matrix(np.eye(4))) # test initializing with rotation matrix T = Transform(Rpitch(pi / 4)) N = np.eye(4) N[:3, :3] = Rpitch(pi / 4) assert_allclose(T, N) # test initializing with position vector pos = np.matrix([5, 2, 4]).T T = Transform(p=pos) N = np.eye(4) N[:3, 3] = pos assert_allclose(T, N) # test initializing with both rotation and position vector R = Ryaw(5) p = np.matrix('3;2;4') T = Transform(R, p) N = np.eye(4) N[:3, :3] = R N[:3, 3] = p assert_allclose(T, N)
def update_pos(self, pose, absolute=False): """Convenience function to do a position update.""" # assuming that the ekf is never lost by more than pi (except on startup), # if we have absolute (i.e. QR-code) data, and it's far from our current # estimate, then assume we're lost and reset the EKF. if absolute: # we can intelligently mod the incoming pose angle to be close # to the current estimate - note that the ordered if statements # mean that if we are super-lost, we will randomly add some pi # but not infinitely loop. # this if statement says that if we're lost in theta, don't worry about it. if self.covariance[2,2] < 10: while pose.theta - self.state[2,0] > pi: pose.theta -= tau while self.state[2,0] - pose.theta > pi: pose.theta += tau dx = self.state[0,0]-pose.x dy = self.state[1,0]-pose.y dt = fmod(self.state[2,0]-pose.theta,2*pi) if dx*dx + dy*dy > 0.1 or abs(dt) > 0.3: self.state = column_vector(pose.x, pose.y, pose.theta) self.covariance = matrix(pose.cov).reshape(3, 3) return else: pose.x += self.state[0,0] pose.y += self.state[1,0] while pose.theta > pi: pose.theta -= tau while pose.theta < -pi: pose.theta += tau pose.theta += self.state[2,0] y = column_vector(pose.x, pose.y, pose.theta) rospy.loginfo("Updating with position %s",y) W = matrix(pose.cov).reshape(3, 3) self.update(y, W)
def LagkJointMomentsFromMRAP(H, K=0, L=1): """ Returns the lag-L joint moments of a marked rational arrival process. Parameters ---------- H : list/cell of matrices of shape(M,M), length(N) The H0...HN matrices of the MRAP to check K : int, optional The dimension of the matrix of joint moments to compute. If K=0, the MxM joint moments will be computed. The default value is 0 L : int, optional The lag at which the joint moments are computed. The default value is 1 prec : double, optional Numerical precision to check if the input is valid. The default value is 1e-14 Returns ------- Nm : list/cell of matrices of shape(K+1,K+1), length(N) The matrices containing the lag-L joint moments, starting from moment 0. """ if butools.checkInput and not CheckMRAPRepresentation(H): raise Exception( "LagkJointMomentsFromMRAP: Input is not a valid MRAP representation!" ) if K == 0: K = H[0].shape[0] - 1 M = len(H) - 1 H0 = H[0] sumH = ml.zeros(H[0].shape) for i in range(M): sumH += H[i + 1] H0i = la.inv(-H0) P = H0i * sumH pi = DRPSolve(P) Pw = ml.eye(H0.shape[0]) H0p = [ml.matrix(Pw)] for i in range(1, K + 1): Pw *= i * H0i H0p.append(ml.matrix(Pw)) Pl = la.matrix_power(P, L - 1) Nm = [] for m in range(M): Nmm = ml.zeros((K + 1, K + 1)) for i in range(K + 1): for j in range(K + 1): Nmm[i, j] = np.sum(pi * H0p[i] * H0i * H[m + 1] * Pl * H0p[j]) Nm.append(ml.matrix(Nmm)) return Nm
def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' T = identity(4) # YOUR CODE HERE c = cos(joint_angle) s = sin(joint_angle) if (joint_name == 'LHipYawPitch' or joint_name == 'LHipYawPitch'): T1 = matrix([[c, s, 0, 0], [-s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) T2 = matrix([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0], [0, 0, 0, 1]]) T = dot(T2, T1) #print("1") elif ('Roll' in joint_name): T = matrix([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [0, 0, 0, 1]]) #print("2") elif ('Pitch' in joint_name): T = matrix([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0], [0, 0, 0, 1]]) #print("3") elif ('Yaw' in joint_name): T = matrix([[c, s, 0, 0], [-s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) #print("4") #print(T) return T
def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' T = matrix((4, 4)) # YOUR CODE HERE if joint_name.endswith("Pitch"): #X T = matrix([[1, 0, 0, 0], [0, math.cos(joint_angle), -math.sin(joint_angle), 0], [0, math.sin(joint_angle), math.cos(joint_angle), 0], [0, 0, 0, 1]]) elif joint_name.endswith("Yaw"): #Y T = matrix([[math.cos(joint_angle), 0, math.sin(joint_angle), 0], [0, 1, 0, 0], [-math.sin(joint_angle), 0, math.cos(joint_angle), 0], [0, 0, 0, 1]]) elif joint_name.endswith("Roll"): #Z T = matrix([[math.cos(joint_angle), -math.sin(joint_angle), 0, 0], [math.sin(joint_angle), math.cos(joint_angle), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) else: print "Error!!" print joint_name return T
def update_pos(self, pose, absolute=False): """Convenience function to do a position update.""" # assuming that the ekf is never lost by more than pi (except on startup), # if we have absolute (i.e. QR-code) data, and it's far from our current # estimate, then assume we're lost and reset the EKF. if absolute: # we can intelligently mod the incoming pose angle to be close # to the current estimate - note that the ordered if statements # mean that if we are super-lost, we will randomly add some pi # but not infinitely loop. # this if statement says that if we're lost in theta, don't worry about it. if self.covariance[2, 2] < 10: while pose.theta - self.state[2, 0] > pi: pose.theta -= tau while self.state[2, 0] - pose.theta > pi: pose.theta += tau dx = self.state[0, 0] - pose.x dy = self.state[1, 0] - pose.y dt = fmod(self.state[2, 0] - pose.theta, 2 * pi) if dx * dx + dy * dy > 0.1 or abs(dt) > 0.3: self.state = column_vector(pose.x, pose.y, pose.theta) self.covariance = matrix(pose.cov).reshape(3, 3) return else: pose.x += self.state[0, 0] pose.y += self.state[1, 0] while pose.theta > pi: pose.theta -= tau while pose.theta < -pi: pose.theta += tau pose.theta += self.state[2, 0] y = column_vector(pose.x, pose.y, pose.theta) rospy.loginfo("Updating with position %s", y) W = matrix(pose.cov).reshape(3, 3) self.update(y, W)
def local_trans(self, joint_name, joint_angle, translation): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' T = identity(4) # YOUR CODE HERE s = sin(joint_angle) c = cos(joint_angle) x, y, z = translation if 'Yaw' in joint_name: T = matrix([[c, -s, 0.0, x], [s, c, 0.0, y], [0.0, 0.0, 1.0, z], [0.0, 0.0, 0.0, 1.0]]) if 'Roll' in joint_name: T = matrix([[1.0, 0.0, 0.0, x], [0.0, c, -s, y], [0.0, s, c, z], [0.0, 0.0, 0.0, 1.0]]) if 'Pitch' in joint_name: T = matrix([[c, 0.0, s, x], [0.0, 1.0, 0.0, y], [-s, 0.0, c, z], [0.0, 0.0, 0.0, 1.0]]) return T
def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' T = np.zeros([4, 4]) sin_vec = sin(joint_angle) cos_vec = cos(joint_angle) # Differ between the three possible movements angles (Roll, Pitch and Yaw) if 'Roll' in joint_name: T = matrix([[1, 0, 0, 0], [0, cos_vec, -sin_vec, 0], [0, sin_vec, cos_vec, 0], [0, 0, 0, 1]]) if 'Pitch' in joint_name: T = matrix([[cos_vec, 0, sin_vec, 0], [0, 1, 0, 0], [-sin_vec, 0, cos_vec, 0], [0, 0, 0, 1]]) if 'Yaw' in joint_name: T = matrix([[cos_vec, -sin_vec, 0, 0], [sin_vec, cos_vec, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) T[3, 0:3] = self.len_joint[joint_name] return T
def test_mult_simple(self): tempa = [[1,0,0],[0,1,0],[0,0,1]] self.a = matlib.matrix(tempa) self.b = matlib.matrix(tempa) self.c = matlib.matrix(tempa) self.d = multiplierMatrice(self.a, self.b) self.assertTrue(numpy.array_equal(self.c, self.d))
def loglikelihood(self, obs): # compute the log likelihood given a sequence of obs t_mat = npmat.matrix(self.t_mat) # use alpha alpha = npmat.matrix((self.pi_vec * self.z_mat[obs[0], :])[:,np.newaxis]) # print('self.z_mat[obs[0], :]: '+ str(self.z_mat[obs[0], :])) # print('self.pi_vec: '+ str(self.pi_vec)) # print('self.pi_vec * self.z_mat[obs[0], :]: '+ str(self.pi_vec * self.z_mat[obs[0], :])) # print('alpha: '+ str(alpha)) log_coef = 0 for i in range(1,len(obs)): alpha = npmat.matrix(((t_mat * alpha).getA()[:,0] * self.z_mat[obs[i], :])[:,np.newaxis]) asum = alpha.sum() alpha /= asum log_coef += np.log(asum) alpha_ll = log_coef + np.log(alpha.sum()) # use beta if False: beta = npmat.ones((t_mat.shape[0], 1)) log_coef = 0 for i in range(len(obs)-2,-1,-1): # print('self.z_mat[obs[i+1], :]: ' + str(self.z_mat[obs[i+1], :])) # print('beta.getA()[0]: ' + str(beta.getA()[:,0])) # print('self.z_mat[obs[i+1], :] * beta.getA()[:,0]: ' + str(self.z_mat[obs[i+1], :] * beta.getA()[:,0])) # print('npmat.matrix(result): ' + str(npmat.matrix((self.z_mat[obs[i+1], :] * beta.getA()[:,0])[:,np.newaxis]))) # print('t_mat.getT(): ' + str(t_mat.getT())) beta = t_mat.getT() * npmat.matrix((self.z_mat[obs[i+1], :] * beta.getA()[:,0])[:,np.newaxis]) bsum = beta.sum() beta /= bsum log_coef += np.log(bsum) beta_ll = log_coef + np.log(np.sum(beta.getA()[0] * self.pi_vec * self.z_mat[obs[0],:])) # print('alpha_ll: ' + str(alpha_ll)) # print('beta_ll: ' + str(beta_ll)) return alpha_ll
def test_euler(self): """Test axis-angle and euler representation conversions.""" q1 = Quaternion([0, 0, 0, 0], np.float64) #q1.from_euler(0, 0, 0) #print(q1) #self.assertEqual(q1, Quaternion([1,0,0,0])) for i in range(100): (phi, theta, psi) = self.rand_euler() q1.from_euler(phi, theta, psi) q_phi = np.arctan2(q1[0] * q1[2] + q1[1] * q1[3], -(q1[1] * q1[2] - q1[0] * q1[3])) q_theta = np.arccos(-q1[0]**2 - q1[1]**2 + q1[2]**2 + q1[3]**2) q_psi = np.arctan2(q1[0] * q1[2] - q1[1] * q1[3], q1[1] * q1[2] + q1[0] * q1[3]) # Convert between different conventions if q_phi < 0: q_phi += 2 * np.pi #if q_theta < 0: #q_theta += 2 * np.pi if q_psi < 0: q_psi += 2 * np.pi r1 = m.matrix([phi, theta, psi]) r1q = m.matrix([q_phi, q_theta, q_psi]) #print(r1- r1q) self.assertTrue(np.linalg.norm(r1 - r1q) < 1e-10)
def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' T = identity(4) # YOUR CODE HERE offsets = { # x,y,z 'HeadYaw': [0, 0, 0], 'HeadPitch': [0, 0, 126.5], 'LShoulderPitch': [0, 0, 0], 'LShoulderRoll': [105, 15, 0], 'LElbowYaw': [0, 0, 0], 'LElbowRoll': [55.95, 0, 0], 'ShoulderOffset': [0, 98, 100], 'LHipYawPitch': [0, 0, 0], 'LHipRoll': [0, 0, 0], 'LHipPitch': [0, 0, -100], 'LKneePitch': [0, 0, -102.9], 'LAnklePitch': [0, 0, 0], 'LAnkleRoll': [0, 0, 0], 'HipOffset': [0, 0, -85] } # 0 = x, 1 = y, 2 = z if joint_name[0] == "R": coords = offsets["L"+(joint_name[1:])] coords[1] = -1 * coords[1] # invert y for right leg else: coords = offsets[joint_name] # movements: roll - x, pitch - y, yaw - z s = numpy.sin(joint_angle) c = numpy.cos(joint_angle) Rx = matrix([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [coords[0], coords[1], coords[2], 1]]) Ry = matrix([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0], [coords[0], coords[1], coords[2], 1]]) Rz = matrix([[c, s, 0, 0], [-s, c, 0, 0], [0, 0, 1, 0], [coords[0], coords[1], coords[2], 1]]) if joint_name.endswith("YawPitch"): T = Ry / 2 + Rz / 2 elif joint_name.endswith("Roll"): T = Rx elif joint_name.endswith("Pitch"): T = Ry elif joint_name.endswith("Yaw"): T = Rz return T
def fetch_pids_ttol(pnts: MatrixVector, psys: PanelSystem, ztol: float=0.01, ttol: float=0.1): shp = pnts.shape pnts = pnts.reshape((-1, 1)) numpnt = pnts.shape[0] numpnl = len(psys.pnls) pidm = zeros((1, numpnl), dtype=int) wintm = zeros((numpnt, numpnl), dtype=bool) abszm = zeros((numpnt, numpnl), dtype=float) for pnl in psys.pnls.values(): pidm[0, pnl.ind] = pnl.pid wintm[:, pnl.ind], abszm[:, pnl.ind] = pnl.within_and_absz_ttol(pnts[:, 0], ttol=ttol) abszm[wintm is False] = float('inf') minm = argmin(abszm, axis=1) minm = array(minm).flatten() pidm = array(pidm).flatten() pids = pidm[minm] pids = matrix([pids], dtype=int).transpose() indp = arange(numpnt) minz = array(abszm[indp, minm]).flatten() minz = matrix([minz], dtype=float).transpose() chkz = minz < ztol pids[logical_not(chkz)] = 0 pids = pids.reshape(shp) chkz = chkz.reshape(shp) return pids, chkz
def testzyz_fk_ik(self): R = np.matrix([[1., -0., 0.], [0., 1., 0.], [-0., 0., 1.]]) self.assertTrue(np.allclose(zyz_euler_angle(0, 0, 0), R)) self.assertTrue( np.allclose(zyz_euler_angle(*inverse_zyz_euler_angle(R)), R)) R = np.matrix([[0.70710678, -0.70710678, 0.], [0.70710678, 0.70710678, 0.], [-0., 0., 1.]]) self.assertTrue(np.allclose(zyz_euler_angle(math.pi / 4, 0, 0), R)) self.assertTrue( np.allclose(zyz_euler_angle(*inverse_zyz_euler_angle(R)), R)) R = np.matrix([[0.5, -0., 0.8660254], [0., 1., 0.], [-0.8660254, 0., 0.5]]) self.assertTrue(np.allclose(zyz_euler_angle(0, math.pi / 3, 0), R)) self.assertTrue( np.allclose(zyz_euler_angle(*inverse_zyz_euler_angle(R)), R)) R = np.matrix([[0.5, -0.5, 0.70710678], [0.70710678, 0.70710678, 0.], [-0.5, 0.5, 0.70710678]]) self.assertTrue( np.allclose(zyz_euler_angle(0, math.pi / 4, math.pi / 4), R)) self.assertTrue( np.allclose(zyz_euler_angle(*inverse_zyz_euler_angle(R)), R)) R = np.matrix([[-0.55034481, -0.70029646, 0.45464871], [0.70029646, -0.09064712, 0.70807342], [-0.45464871, 0.70807342, 0.54030231]]) self.assertTrue(np.allclose(zyz_euler_angle(1, 1, 1), R)) self.assertTrue( np.allclose(zyz_euler_angle(*inverse_zyz_euler_angle(R)), R))
def SamplesFromDPH(a, A, k): """ Generates random samples from a discrete phase-type distribution. Parameters ---------- alpha : matrix, shape (1,M) The initial probability vector of the discrete phase- type distribution. A : matrix, shape (M,M) The transition probability matrix of the discrete phase- type distribution. K : integer The number of samples to generate. prec : double, optional Numerical precision to check if the input phase-type distribution is valid. The default value is 1e-14. Returns ------- x : vector, length(K) The vector of random samples """ if butools.checkInput and not CheckDPHRepresentation(a, A): raise Exception( "SamplesFromDPH: input is not a valid DPH representation!") # auxilary variables a = a.A.flatten() N = len(a) cummInitial = np.cumsum(a) logp = np.log(np.diag(A)) sojourn = 1.0 / (1.0 - np.diag(A)) nextpr = ml.matrix(np.diag(sojourn)) * A nextpr = nextpr - ml.matrix(np.diag(np.diag(nextpr))) nextpr = np.hstack((nextpr, 1.0 - np.sum(nextpr, 1))) nextpr = np.cumsum(nextpr, 1) x = np.zeros(k) for n in range(k): time = 0 # draw initial distribution r = rand() state = 0 while cummInitial[state] <= r: state += 1 # play state transitions while state < N: time += 1 + int(np.log(rand()) / logp[state]) r = rand() nstate = 0 while nextpr[state, nstate] <= r: nstate += 1 state = nstate x[n] = time return x
def forward_kinematics(self, joints): '''forward kinematics :param joints: {joint_name: joint_angle} ''' for chain_joints in self.chains.values(): T = identity(4) for joint in chain_joints: T1 = identity(4) if joint == "HeadYaw": T1 = matrix([[1, 0.0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.1265], [0, 0, 0, 1]]) elif joint == "RShoulderPitch": T1 = matrix([[1, 0, 0, 0], [0, 1, 0, -0.098], [0, 0, 1, 0.100], [0, 0, 0, 1]]) elif joint == "LShoulderPitch": T1 = numpy.matrix([[1, 0, 0, 0], [0, 1, 0, 0.098], [0, 0, 1, 0.100], [0, 0, 0, 1]]) elif joint == "LHipYawPitch": T1 = matrix([[1, 0, 0, 0], [0, 1, 0, 0.050], [0, 0, 1, -0.085], [0, 0, 0, 1]]) elif joint == "RHipYawPitch": T1 = matrix([[1, 0, 0, 0], [0, 1, 0, -0.050], [0, 0, 1, -0.085], [0, 0, 0, 1]]) angle = joints[joint] Tl = self.local_trans(joint, angle) T = T1 * T * Tl self.transforms[joint] = T """
def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' T = identity(4) # YOUR CODE HERE s = math.sin(joint_angle) c = math.cos(joint_angle) xo, yo, zo = self.jointoffs[joint_name] #x if (joint_name.find('Roll') > 0): T = matrix([[1, 0, 0, xo], [0, c, -s, yo], [0, s, c, zo], [0, 0, 0, 1]]) #y elif (joint_name.find('Pitch') > 0): T = matrix([[c, 0, s, xo], [0, 1, 0, yo], [-s, 0, c, zo], [0, 0, 0, 1]]) #z elif (joint_name.find('Yaw') > 0): T = matrix([[c, -s, 0, xo], [s, c, 0, yo], [0, 0, 1, zo], [0, 0, 0, 1]]) else: print("Invalid joint_name") return T
def optimize(bd_con, init_val=None): # bd_con: boundary conditions - (k0, x1, y1, theta1, k1) # init_val: (p1, p2, sg) if init_val is None or init_val[2] < 0: init_val = ((2 * bd_con[0] + bd_con[4]) / 3, (bd_con[0] + 2 * bd_con[4]) / 3, np.sqrt(bd_con[1]**2 + bd_con[2]**2) + 5 * min(np.abs(bd_con[3]), np.abs(2 * np.pi - bd_con[3]))) # revise the parameters init_val = list(init_val) init_val[0] = max(min(init_val[0], 0.2), -0.2) init_val[1] = max(min(init_val[1], 0.2), -0.2) init_val[2] = max(min(init_val[2], 1000.), 1.) # print('IterTimes: {0},P = {1}'.format(0, (init_val[0], init_val[1], init_val[2]))) q_g = matlib.matrix([[bd_con[1]], [bd_con[2]], [bd_con[3]]]) p = (bd_con[0], init_val[0], init_val[1], bd_con[4], init_val[2]) r = (__a(p), __b(p), __c(p), __d(p)) pp = matlib.matrix([[init_val[0]], [init_val[1]], [init_val[2]]]) dq = matlib.matrix([[1.], [1.], [1.]]) eps1, eps2 = 1.e-4, 1.e-6 times = 0 while (np.abs(dq[0, 0]) > eps1 or np.abs(dq[1, 0]) > eps1 or np.abs(dq[2, 0]) > eps2) and times <= 100: times += 1 J = __Jacobian(p, r) # print('J={0}'.format(J)) theta_p = __theta(p[4], r) x_p, y_p = __xy_calc(p[4], r) # print('q = {0}'.format((x_p,y_p,theta_p))) dq = q_g - matlib.matrix([[x_p], [y_p], [theta_p]]) pp += J**-1 * dq # revise the parameters pp[0, 0] = max(min(pp[0, 0], 0.2), -0.2) pp[1, 0] = max(min(pp[1, 0], 0.2), -0.2) pp[2, 0] = max(min(pp[2, 0], 1000.), 1.) # # if pp[0,0] > 0.2: # pp[0,0] = 0.2 # elif pp[0,0] < -0.2: # pp[0,0] = -0.2 # if pp[1,0] > 0.2: # pp[1,0] = 0.2 # elif pp[1,0] < -0.2: # pp[1,0] = -0.2 # if pp[2,0] < 1.: # pp[2,0] = 1. # elif pp[2,0] > 1000.: # pp[2,0] = 1000. p = (bd_con[0], pp[0, 0], pp[1, 0], bd_con[4], pp[2, 0]) # print('p={0}'.format(p)) r = (__a(p), __b(p), __c(p), __d(p)) # print('r={0}'.format(r)) # print('IterTimes: {0},P = {1}'.format(times, (p[1],p[2],p[4]))) # print('IterTimes: {0}'.format(times)) if times > 100: # pp = matlib.matrix([[-1.],[-1.],[-1.]]) return None else: return pp[0, 0], pp[1, 0], pp[2, 0]
def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' T = identity(4) # YOUR CODE HERE #get sin and cos of joint angle sin = math.sin(joint_angle) cos = math.cos(joint_angle) #get link values for joint joint_links = (0.0, 0.0, 0.0) if joint_name in self.links: joint_links = self.links[joint_name] #Check if X,Y or Z and set T according to 3D forward kinematics if (joint_name.endswith("Roll")): T = matrix([[1, 0, 0, 0], [0, cos, -sin, 0], [0, sin, cos, 0], [joint_links[0], joint_links[1], joint_links[2], 1]]) if (joint_name.endswith("Pitch")): T = matrix([[cos, 0, sin, 0], [0, 1, 0, 0], [-sin, 0, cos, 0], [joint_links[0], joint_links[1], joint_links[2], 1]]) if (joint_name.endswith("Yaw")): T = matrix([[cos, sin, 0, 0], [-sin, cos, 0, 0], [0, 0, 1, 0], [joint_links[0], joint_links[1], joint_links[2], 1]]) return T
def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' T = identity(4) # YOUR CODE HERE s = math.sin(joint_angle) c = math.cos(joint_angle) if joint_name.endswith("Roll"): #X T = matrix([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [0, 0, 0, 1]]) elif joint_name.endswith("Pitch"): #Y T = matrix([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0], [0, 0, 0, 1]]) elif joint_name.endswith("Yaw"): #Z T = matrix([[c, s, 0, 0], [-s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) else: print "Error!!" print joint_name T[0, 3] = self.jointLengths[joint_name][0] T[1, 3] = self.jointLengths[joint_name][1] T[2, 3] = self.jointLengths[joint_name][2] return T
def test_euler(self): """Test axis-angle and euler representation conversions.""" q1 = Quaternion([0,0,0,0], np.float64) #q1.from_euler(0, 0, 0) #print(q1) #self.assertEqual(q1, Quaternion([1,0,0,0])) for i in range(100): (phi, theta, psi) = self.rand_euler() q1.from_euler(phi, theta, psi) q_phi = np.arctan2(q1[0]*q1[2] + q1[1]*q1[3],-(q1[1]*q1[2]-q1[0]*q1[3])) q_theta = np.arccos(-q1[0]**2 -q1[1]**2 + q1[2]**2 + q1[3]**2) q_psi = np.arctan2(q1[0]*q1[2] - q1[1]*q1[3], q1[1]*q1[2] + q1[0]*q1[3]) # Convert between different conventions if q_phi < 0: q_phi += 2 * np.pi #if q_theta < 0: #q_theta += 2 * np.pi if q_psi < 0: q_psi += 2 * np.pi r1 = m.matrix([phi, theta, psi]) r1q = m.matrix([q_phi, q_theta, q_psi]) #print(r1- r1q) self.assertTrue(np.linalg.norm(r1 - r1q) < 1e-10)
def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' T = identity(4) # YOUR CODE HERE a = math.sin(joint_angle) b = math.cos(joint_angle) if joint_name.endswith("Roll"): T = np.dot( T, matrix([[1, 0, 0, 0], [0, b, -a, 0], [0, a, b, 0], [0, 0, 0, 1]])) elif joint_name.endswith("Pitch"): T = np.dot( T, matrix([[b, 0, a, 0], [0, 1, 0, 0], [-a, 0, b, 0], [0, 0, 0, 1]])) elif joint_name.endswith("Yaw"): T = np.dot( T, matrix([[b, a, 0, 0], [-a, b, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])) T[0, 3] = self.jointLength[joint_name][0] T[1, 3] = self.jointLength[joint_name][1] T[2, 3] = self.jointLength[joint_name][2] return T
def SamplesFromDPH (a,A,k): """ Generates random samples from a discrete phase-type distribution. Parameters ---------- alpha : matrix, shape (1,M) The initial probability vector of the discrete phase- type distribution. A : matrix, shape (M,M) The transition probability matrix of the discrete phase- type distribution. K : integer The number of samples to generate. prec : double, optional Numerical precision to check if the input phase-type distribution is valid. The default value is 1e-14. Returns ------- x : vector, length(K) The vector of random samples """ if butools.checkInput and not CheckDPHRepresentation(a,A): raise Exception("SamplesFromDPH: input is not a valid DPH representation!") # auxilary variables a = a.A.flatten() N = len(a) cummInitial = np.cumsum(a) logp = np.log(np.diag(A)); sojourn = 1.0/(1.0-np.diag(A)) nextpr = ml.matrix(np.diag(sojourn))*A nextpr = nextpr - ml.matrix(np.diag(np.diag(nextpr))) nextpr = np.hstack((nextpr, 1.0-np.sum(nextpr,1))) nextpr = np.cumsum(nextpr,1) x = np.zeros(k) for n in range(k): time = 0 # draw initial distribution r = rand() state = 0 while cummInitial[state]<=r: state+=1 # play state transitions while state<N: time += 1 + int(np.log(rand()) / logp[state]) r = rand() nstate = 0 while nextpr[state,nstate]<=r: nstate += 1 state = nstate x[n] = time return x
def em_step(num_states, num_obs, pi_vec, z_mat, t_mat, obs_all): # given initial parameters, run em from the obs # obs are 2d with each row being a trajectory # returns estimated observation and transition matrices num_steps = obs_all.shape[1] t_mat = npmat.matrix(t_mat) gamma_sum = np.zeros((num_states,)) xi_sum = np.zeros((num_states, num_states)) obs_sum = np.zeros((num_obs, num_states)) for t in range(obs_all.shape[0]): obs = obs_all[t, :] # compute alpha alpha = npmat.zeros((num_states, num_steps)) alpha[:, 0] = npmat.matrix((pi_vec * z_mat[obs[0], :])[:, np.newaxis]) for i in range(1, len(obs)): alpha[:, i] = npmat.matrix(((t_mat * alpha[:, i - 1]).getA()[:, 0] * z_mat[obs[i], :])[:, np.newaxis]) asum = alpha[:, i].sum() alpha[:, i] /= asum # print('alpha\n' + str(alpha)) # compute beta beta = npmat.zeros((num_states, num_steps)) beta[:, num_steps - 1] = npmat.ones((num_states, 1)) for i in range(len(obs) - 2, -1, -1): beta[:, i] = t_mat.getT() * npmat.matrix( (z_mat[obs[i + 1], :] * beta[:, i + 1].getA()[:, 0])[:, np.newaxis]) bsum = beta[:, i].sum() beta[:, i] /= bsum # print(beta) # compute gamma gamma = alpha.getA() * beta.getA() gamma = gamma / (gamma.sum(axis=0)[np.newaxis, :]) # print(gamma) # compute xi (transposed from the paper) xi = np.zeros((num_states, num_states, num_steps - 1)) for i in range(num_steps - 1): xi[:, :, i] = np.transpose(alpha[:, i].getA()) * t_mat.getA() * (z_mat[obs[i + 1], :] * beta[:, i + 1].getA()[0])[:, np.newaxis] xsum = xi[:, :, i].sum() xi[:, :, i] /= xsum # print('xi[:,:,i]' + str(xi[:,:,i])) # print('np.transpose(alpha[:,i].getA()): ' + str(np.transpose(alpha[:,i].getA()))) # print('t_mat: ' + str(t_mat)) # print('result of *: ' + str(np.transpose(alpha[:,i].getA()) * t_mat.getA())) # add to the sums gamma_sum += gamma.sum(axis=1) xi_sum += xi.sum(axis=2) for z in range(num_obs): obs_sum[z, :] += gamma[:, obs == z].sum(axis=1) # print('gamma_sum\n' + str(gamma_sum)) # print('xi_sum\n' + str(xi_sum)) # print('obs_sum\n' + str(obs_sum)) # finally compute estimates est_z_mat = obs_sum / obs_sum.sum(axis=0)[np.newaxis, :] est_t_mat = xi_sum / xi_sum.sum(axis=0)[np.newaxis, :] return est_z_mat, est_t_mat;
def test_state_values(): # StateValues tests sv = StateValues(X) assert (sv[nm.matrix('1 2 1; 0 0 0; 0 0 0')] == 0.5) assert (sv[nm.matrix('1 2 1; 0 0 0; 0 0 0')] == 0.5) assert (sv[nm.matrix('1 1 1; 0 0 0; 0 0 0')] == 1) assert (sv[nm.matrix('0 1 0; 0 1 0; 0 1 0')] == 1) assert (sv[nm.matrix('0 0 2; 0 2 0; 2 0 0')] == 0)
def test_mult_dimensions(self): tempa = [[1,1],[2,2],[3,3]] tempb = [[3,3,3],[2,2,2]] tempc = [[5,5,5],[10,10,10],[15, 15, 15]] self.a = matlib.matrix(tempa) self.b = matlib.matrix(tempb) self.c = matlib.matrix(tempc) self.assertTrue(numpy.array_equal(multiplierMatrice(self.a, self.b), self.c))
def test_state_values(): # StateValues tests sv = StateValues(X) assert(sv[nm.matrix('1 2 1; 0 0 0; 0 0 0')] == 0.5) assert(sv[nm.matrix('1 2 1; 0 0 0; 0 0 0')] == 0.5) assert(sv[nm.matrix('1 1 1; 0 0 0; 0 0 0')] == 1) assert(sv[nm.matrix('0 1 0; 0 1 0; 0 1 0')] == 1) assert(sv[nm.matrix('0 0 2; 0 2 0; 2 0 0')] == 0)
def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' # YOUR CODE HERE s = sin(joint_angle) c = cos(joint_angle) #set rotation #Z-axis if (joint_name[-3:] == 'Yaw'): T = matrix([[c, -s, 0, 0], [s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) #X-axis elif (joint_name[-4:] == 'Roll'): T = matrix([[1, 0 , 0, 0], [0, c, -s, 0], [0, s, c, 0], [0, 0, 0, 1]]) #Y-axis elif (joint_name[-5:] == 'Pitch'): T = matrix([[c, 0 , -s, 0], [0, 1, 0, 0], [s, 0, c, 0], [0, 0, 0, 1]]) #set X,Y,Z for joints that have a different coordinate difference to the joint before than 0,0,0 if (joint_name[0] == 'L' or joint_name[0] == 'R'): if (joint_name == 'LElbowYaw' or joint_name == 'RElbowYaw'): T[:3, -1] = self.lengths['ArmShoulderRollElbowYaw'].T elif (joint_name == 'LWristYaw' or joint_name == 'RWristYaw'): T[:3, -1] = self.lengths['ArmElbowRollWristYaw'].T elif (joint_name == 'LKneePitch' or joint_name == 'RKneePitch'): T[:3, -1] = self.lengths['LegHipPitchKneePitch'].T elif (joint_name == 'LAnklePitch' or joint_name == 'RAnklePitch'): T[:3, -1] = self.lengths['LegKneePitchAnklePitch'].T #for transformation from torso to hip (left leg) elif (joint_name == 'LRoll'): T[:3, -1] = self.lengths['LLegTorsoHip'].T #for transformation from torso to hip (right leg) elif (joint_name == 'RRoll'): T[:3, -1] = self.lengths['RLegTorsoHip'].T #for transformation from torso to shoulder (left arm) elif (joint_name == 'LYaw'): T[:3, -1] = self.lengths['LArmTorsoShoulder'].T #for transformation from torso to shoulder (right arm) elif (joint_name == 'RYaw'): T[:3, -1] = self.lengths['RArmTorsoShoulder'].T return T
def LagkJointMomentsFromMRAP (H, K=0, L=1): """ Returns the lag-L joint moments of a marked rational arrival process. Parameters ---------- H : list/cell of matrices of shape(M,M), length(N) The H0...HN matrices of the MRAP to check K : int, optional The dimension of the matrix of joint moments to compute. If K=0, the MxM joint moments will be computed. The default value is 0 L : int, optional The lag at which the joint moments are computed. The default value is 1 prec : double, optional Numerical precision to check if the input is valid. The default value is 1e-14 Returns ------- Nm : list/cell of matrices of shape(K+1,K+1), length(N) The matrices containing the lag-L joint moments, starting from moment 0. """ if butools.checkInput and not CheckMRAPRepresentation (H): raise Exception("LagkJointMomentsFromMRAP: Input is not a valid MRAP representation!") if K==0: K = H[0].shape[0]-1 M = len(H)-1 H0 = H[0] sumH = ml.zeros(H[0].shape) for i in range(M): sumH += H[i+1] H0i = la.inv(-H0) P = H0i*sumH pi = DRPSolve(P) Pw = ml.eye(H0.shape[0]) H0p = [ml.matrix(Pw)] for i in range(1,K+1): Pw *= i*H0i H0p.append(ml.matrix(Pw)) Pl = la.matrix_power (P, L-1) Nm = [] for m in range(M): Nmm = ml.zeros ((K+1,K+1)) for i in range(K+1): for j in range(K+1): Nmm[i,j] = np.sum (pi * H0p[i] * H0i * H[m+1] * Pl * H0p[j]) Nm.append(ml.matrix(Nmm)) return Nm
def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' T = identity(4) if joint_name == 'LHipYawPitch': Rl = matrix( [[1, 0, 0], [0, math.cos(math.pi / 4), -1 * math.sin(math.pi / 4)], [0, math.sin(math.pi / 4), math.cos(math.pi / 4)]]) T[0:3, 0:3] = Rl elif joint_angle == 'RHipYawPitch': Rr = matrix( [[1, 0, 0], [ 0, math.cos(-1 * math.pi / 4), -1 * math.sin(-1 * math.pi / 4) ], [0, math.sin(-1 * math.pi / 4), math.cos(-1 * math.pi / 4)]]) T[0:3, 0:3] = Rr else: if joint_name.endswith("Roll"): Rx = matrix( [[1, 0, 0], [0, math.cos(joint_angle), -1 * math.sin(joint_angle)], [0, math.sin(joint_angle), math.cos(joint_angle)]]) T[0:3, 0:3] = Rx elif joint_name.endswith("Pitch"): Ry = matrix( [[math.cos(joint_angle), 0, math.sin(joint_angle)], [0, 1, 0], [-1 * math.sin(joint_angle), 0, math.cos(joint_angle)]]) T[0:3, 0:3] = Ry elif joint_name.endswith("Pitch"): Rz = matrix( [[math.cos(joint_angle), -1 * math.sin(joint_angle), 0], [math.sin(joint_angle), math.cos(joint_angle), 0], [0, 0, 1]]) T[0:3, 0:3] = Rz for key in self.chains.keys(): if joint_name in self.chains[key]: T[0, 3] = self.l[key][self.chains[key].index(joint_name)][0] T[1, 3] = self.l[key][self.chains[key].index(joint_name)][1] T[2, 3] = self.l[key][self.chains[key].index(joint_name)][2] return T
def plot_belief_trajectory_cpp(B, U, model, start, goal, T): bDim = model.bDim uDim = model.uDim B = np.matrix(B) B = np.matrix(B.reshape(bDim, T)) model.T = T model.setStartState(ml.matrix(start).T) model.setGoalState(ml.matrix(goal).T) plot_belief_trajectory(B, U, model)
def local_trans(self, joint_name, joint_angle): '''calculate local transformation of one joint :param str joint_name: the name of joint :param float joint_angle: the angle of joint in radians :return: transformation :rtype: 4x4 matrix ''' T = identity(4) # YOUR CODE HERE s = math.sin(joint_angle) c = math.sin(joint_angle) # Roll = X Pitch = Y Yaw = Z if joint_name.endswith("Roll"): T = matrix([ [1, 0, 0, self.links[joint_name][0]], [0, c, -s, self.links[joint_name][1]], [0, s, c, self.links[joint_name][2]], [0, 0, 0, 1], ]) if joint_name.endswith("Pitch"): T = matrix([ [c, 0, s, self.links[joint_name][0]], [0, 1, 0, self.links[joint_name][1]], [-s, 0, c, self.links[joint_name][2]], [0, 0, 0, 1], ]) if joint_name.endswith("Yaw"): T = matrix([ [c, s, 0, self.links[joint_name][0]], [-s, c, 0, self.links[joint_name][1]], [0, 0, 1, self.links[joint_name][2]], [0, 0, 0, 1], ]) if joint_name.endswith("YawPitch"): Y = matrix([ [c, s, 0, self.links[joint_name][0]], [-s, c, 0, self.links[joint_name][1]], [0, 0, 1, self.links[joint_name][2]], [0, 0, 0, 1], ]) P = matrix([ [c, 0, s, self.links[joint_name][0]], [0, 1, 0, self.links[joint_name][1]], [-s, 0, c, self.links[joint_name][2]], [0, 0, 0, 1], ]) T = np.dot(Y, P) return T
def QBDSolve (B, L, F, L0, prec=1e-14): """ Returns the parameters of the matrix-geometrically distributed stationary distribution of a QBD. Using vector pi0 and matrix R provided by this function the stationary solution can be obtained by .. math:: \pi_k=\pi_0 R^k. Parameters ---------- B : matrix, shape (N,N) The matrix corresponding to backward transitions L : matrix, shape (N,N) The matrix corresponding to local transitions F : matrix, shape (N,N) The matrix corresponding to forward transitions L0 : matrix, shape (N,N) The matrix corresponding to local transitions at level zero precision : double, optional The fundamental matrix R is computed up to this precision. The default value is 1e-14 Returns ------- pi0 : matrix, shape (1,N) The stationary probability vector of level zero R : matrix, shape (N,N) The matrix parameter of the matrix geometrical distribution of the QBD """ m = L0.shape[0] I = ml.eye(m) R = QBDFundamentalMatrices (B, L, F, "R", prec) # Convert to discrete time problem, if needed if np.sum(np.diag(L0) < 0): # continues time lamb = float(np.max(-np.diag(L0))) B = ml.matrix(B) / lamb L0 = ml.matrix(L0) / lamb + I pi0 = DTMCSolve(L0+R*B) nr = np.sum(pi0*la.inv(I-R)) pi0 /= nr return pi0, R
def test_board_states(): # board state tests b = Board() pX = Player(X,b) pO = Player(O,b) b.state = nm.matrix("1 0 0; 1 1 1; 1 0 0") assert(are_winning_rows(b.state, X) and are_winning_cols(b.state, X)) assert(is_winner(b.state, pX.side)) b.state = nm.matrix("2 2 2; 0 0 2; 1 0 2") assert(are_winning_rows(b.state, O) and are_winning_cols(b.state, O)) assert(is_winner(b.state, pO.side)) b.state = nm.matrix("1 0 0; 0 1 0; 0 0 1") assert(are_winning_diags(b.state, X)) assert(is_winner(b.state, pX.side)) b.state = nm.matrix("0 0 1; 0 1 0; 1 0 0") assert(are_winning_diags(b.state, X)) assert(is_winner(b.state, pX.side)) b.state = nm.matrix("0 0 1; 0 1 0; 1 0 0") assert(are_winning_diags(b.state, X)) assert(is_winner(b.state, pX.side)) b.state = nm.matrix("1 1 2; 2 2 1; 1 1 2") assert(not are_winning_diags(b.state, X) and not are_winning_rows(b.state, O) and not are_winning_cols(b.state, O)) assert(not is_winner(b.state, pX.side)) assert(not is_winner(b.state, pO.side)) assert(is_draw(b.state)) b.state = nm.matrix("1 1 1; 2 2 1; 2 2 1") assert(is_winner(b.state, pX.side)) assert(not is_winner(b.state, pO.side)) b.state = nm.matrix("1 2 1; 2 2 1; 2 2 1") assert(is_winner(b.state, pX.side)) assert(is_winner(b.state, pO.side)) b.state = nm.matrix("1 1 2; 2 2 1; 1 2 1") assert(not is_winner(b.state, pX.side)) assert(not is_winner(b.state, pO.side)) assert(is_draw(b.state)) b.state = nm.matrix("2 1 1; 1 2 2; 1 2 1") assert(not is_winner(b.state, pX.side)) assert(not is_winner(b.state, pO.side)) assert(is_draw(b.state))
def test_board_states(): # board state tests b = Board() pX = Player(X, b) pO = Player(O, b) b.state = nm.matrix("1 0 0; 1 1 1; 1 0 0") assert (are_winning_rows(b.state, X) and are_winning_cols(b.state, X)) assert (is_winner(b.state, pX.side)) b.state = nm.matrix("2 2 2; 0 0 2; 1 0 2") assert (are_winning_rows(b.state, O) and are_winning_cols(b.state, O)) assert (is_winner(b.state, pO.side)) b.state = nm.matrix("1 0 0; 0 1 0; 0 0 1") assert (are_winning_diags(b.state, X)) assert (is_winner(b.state, pX.side)) b.state = nm.matrix("0 0 1; 0 1 0; 1 0 0") assert (are_winning_diags(b.state, X)) assert (is_winner(b.state, pX.side)) b.state = nm.matrix("0 0 1; 0 1 0; 1 0 0") assert (are_winning_diags(b.state, X)) assert (is_winner(b.state, pX.side)) b.state = nm.matrix("1 1 2; 2 2 1; 1 1 2") assert (not are_winning_diags(b.state, X) and not are_winning_rows(b.state, O) and not are_winning_cols(b.state, O)) assert (not is_winner(b.state, pX.side)) assert (not is_winner(b.state, pO.side)) assert (is_draw(b.state)) b.state = nm.matrix("1 1 1; 2 2 1; 2 2 1") assert (is_winner(b.state, pX.side)) assert (not is_winner(b.state, pO.side)) b.state = nm.matrix("1 2 1; 2 2 1; 2 2 1") assert (is_winner(b.state, pX.side)) assert (is_winner(b.state, pO.side)) b.state = nm.matrix("1 1 2; 2 2 1; 1 2 1") assert (not is_winner(b.state, pX.side)) assert (not is_winner(b.state, pO.side)) assert (is_draw(b.state)) b.state = nm.matrix("2 1 1; 1 2 2; 1 2 1") assert (not is_winner(b.state, pX.side)) assert (not is_winner(b.state, pO.side)) assert (is_draw(b.state))
def __init__(self, simspark_ip='localhost', simspark_port=3100, teamname='DAInamite', player_id=0, sync_mode=True): super(ForwardKinematicsAgent, self).__init__(simspark_ip, simspark_port, teamname, player_id, sync_mode) self.transforms = {n: identity(4) for n in self.joint_names} # chains defines the name of chain and joints of the chain self.chains = {'Head': ['HeadYaw', 'HeadPitch'], 'LArm': ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll'], 'LLeg': ['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll'], 'RLeg': ['RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'], 'RArm': ['RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll'] } #lengths bodypart from.. to.. [X, Y, Z] self.lengths = {'ArmShoulderRollElbowYaw': matrix([105.0, 15.0, 0.0]), 'ArmElbowRollWristYaw': matrix([55.95, 0.0, 0.0]), 'LegHipPitchKneePitch': matrix([0.0, 0.0, -100.0]), 'LegKneePitchAnklePitch': matrix([0.0, 0.0, -102.9]), 'LLegTorsoHip': matrix([0.0, 50.0, -85.0]), 'RLegTorsoHip': matrix([0.0, -50.0, -85.0]), 'LArmTorsoShoulder': matrix([0.0, 98.0, 100.0]), 'RArmTorsoShoulder': matrix([0.0, -98.0, 100.0]) } self.start_time =self.perception.time
def rotation_matrix(self, axis, angle): matrix_dict = { 'x': matrix([[1, 0, 0, 0], [0, np.cos(angle), -np.sin(angle), 0], [0, np.sin(angle), np.cos(angle), 0], [0, 0, 0, 1]]), 'y': matrix([[np.cos(angle), 0, np.sin(angle), 0], [0, 1, 0, 0], [-np.sin(angle), 0, np.cos(angle), 0], [0, 0, 0, 1]]), 'z': matrix([[np.cos(angle), -np.sin(angle), 0, 0], [np.sin(angle), np.cos(angle), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) } return matrix_dict.get(axis)
def shortest(B) : (j,k) = B.shape if j==1 : return B b = LLL(B).getBasis() # representasi basis dalam array 2d while True : # aproximasi reduced basis b_ = orthoproject(b[0], b[1:]) # rekursif b__ = shortest(b_) # lifting dg transformasi linear, catatan 13/12 T = solve_linear( b_.T,b__ ) # b_ perlu ditransform, liat spek # transpose T karena msh kolom v = matrix( T.T )*matrix( b[1:] ) # ubah ke matrix dulu b[1:] = np.asarray(v) # override array lagi b0n = norm(b[0]) b1n = norm(b[1]) if b1n**2 >= (3.0/4)*b0n**2 : break # swap t = np.copy(b[0]) b[0] = b[1] b[1] = t # temukan nilai batas j0 g = Gram(b) # cari vektor yg norm-nya lebih besar dari b0 i = 1 while i < j : v = g.getBstar(i) if norm(v) >= b0n : break i += 1 # enumerate v1 = enumerate(b[:i], g) # select-basis B = select_basis( np.append([v1], b, axis=0) ) # ulangi lagi step spt yg didalam loop B_ = orthoproject( B[0], B[1:] ) B__ = shortest(B_) # rekursif T = solve_linear( B_.T, B__ ) # tangani kondisi ketika pengenolan # apakah dalam enumerate alpha diubah2 secara share V = matrix( T.T )*matrix( B[1:] ) # perlu bentuk matrix B[1:] = np.asarray(V) return B
def SimilarityMatrix (A1, A2): """ Returns the matrix that transforms A1 to A2. Parameters ---------- A1 : matrix, shape (N,N) The smaller matrix A2 : matrix, shape (M,M) The larger matrix (M>=N) Returns ------- B : matrix, shape (N,M) The matrix satisfying `A_1\,B = B\,A_2` Notes ----- For the existence of a (unique) solution the larger matrix has to inherit the eigenvalues of the smaller one. """ if A1.shape[0]!=A1.shape[1] or A2.shape[0]!=A2.shape[1]: raise Exception("SimilarityMatrix: The input matrices must be square!") N1 = A1.shape[0] N2 = A2.shape[1] if N1>N2: raise Exception("SimilarityMatrix: The first input matrix must be smaller than the second one!") [R1,Q1]=la.schur(A1,'complex') [R2,Q2]=la.schur(A2,'complex') Q1 = ml.matrix(Q1) Q2 = ml.matrix(Q2) c1 = ml.matrix(np.sum(Q2.H,1)) c2 = np.sum(Q1.H,1) I = ml.eye(N2) X = ml.zeros((N1,N2), dtype=complex) for k in range(N1-1,-1,-1): M = R1[k,k]*I-R2 if k==N1: m = ml.zeros((1,N2)) else: m = -R1[k,k+1:]*X[k+1:,:] X[k,:] = Linsolve(np.hstack((M,c1)),np.hstack((m,c2[k]))) return (Q1*X*ml.matrix(Q2).H ).real
def predict(self, odom_pose): delta = self.get_odom_delta(odom_pose) if delta is None: # Can't do a delta update the first time. return dx, dy, dt = self.state_tuple(delta) # Give bonus change in angel since odom cannot # read that properly if abs(dt) > 0: if rospy.gettime() - self.lastdt > 0.25: bonus = copysign(ACCEL_ANGEL_BONUS, dt) delta[2] += bonus self.lastdt = rospy.get_time # PREDICT NEW STATE (X, Y, THETA) self.state = self.state + delta dist = sqrt(dx*dx + dy*dy) fwderr = 0.05 * dist sideerr = 0.1 * dist yawerr = 0.2 * dist cth = cos(odom_pose.theta) sth = sin(odom_pose.theta) V = matrix([ [cth*cth*fwderr + sth*sth*sideerr, sth*cth*(sideerr - fwderr) , 0.0 ], [sth*cth*(sideerr - fwderr) , sth*sth*fwderr + cth*cth*sideerr, 0.0 ], [0.0 , 0.0 , yawerr]]) # PREDICT COVARIANCE self.covariance = self.covariance + V
def Translation(x, y, z): Tl = identity(4) Tl = matrix([[1,0,0,x], [0,1,0,y], [0,0,1,z], [0,0,0,1]]) return Tl
def lireFichierMatrice(fichier): with open(fichier, "r") as fichier: noMatrice = int(fichier.readline()) dimensions = map(int, filter(lambda x: x != "", map(lambda x: x.strip(), fichier.readline().strip().split(" ")))) assert (len(dimensions) == noMatrice+1),"Erreur de dimensions dans le fichier" matrices = [None] for i in range(0, len(dimensions)-1): matrice = [] # On remplit la matrice for j in range(0, dimensions[i]): temp = map(int, filter(lambda x: x != "", map(lambda x: x.strip(), fichier.readline().strip().split(" ") ) ) ) assert(len(temp) == dimensions[i+1]),"Erreur dans le fichier source" matrice.append(temp) matrices.append(matlib.matrix(matrice)) return noMatrice, dimensions, matrices
def CRPSolve (Q): """ Computes the stationary solution of a continuous time rational process (CRP). Parameters ---------- Q : matrix, shape (M,M) The generator matrix of the rational process Returns ------- pi : row vector, shape (1,M) The vector that satisfies `\pi\, Q = 0, \sum_i \pi_i=1` Notes ----- Continuous time rational processes are like continuous time Markov chains, but the generator does not have to pass the :func:`CheckGenerator` test (but the rowsums still have to be zeros). """ if butools.checkInput and np.any(np.sum(Q,1)>butools.checkPrecision): raise Exception("CRPSolve: The matrix has a rowsum which isn't zero!") M = np.array(Q) M[:,0] = np.ones(M.shape[0]) m = np.zeros(M.shape[0]) m[0] = 1.0 return ml.matrix(la.solve (M.T, m))
def Linsolve(A,b): """ Solves the linear system A*x=b (if b is a column vector), or x*A=b (if b is a row vector). Matrix "A" does not need to be square, this function uses rank-revealing QR decomposition to solve the system. Parameters ---------- A : matrix, shape (M,N) The coefficient matrix of the linear system. b : matrix, shape (M,1) or (1,N) The right hand side of the linear system Returns ------- x : matrix, shape (M,1) or (1,N) If b is a column vector, then x is the solution of A*x=b. If b is a row vector, it returns the solution of x*A=b. """ if b.shape[0]==1: x = Linsolve(np.conj(A.T), np.conj(b.T)) return np.conj(x.T) elif b.shape[1]==1: Q,R = la.qr(A) N = A.shape[1] return ml.matrix(la.solve(R[0:N,0:N], np.array(np.conj(Q.T)*b).flatten()[0:N])).T
def potential_fields_test(): T_f = 150 T_s = .01 t = np.arange(0, T_f, T_s) c = controller_1.Controller(0, T_s) c.q = np.matrix('150; 0') p = plant.Plant(0, npm.matrix([[0, 60, 0, 1, 0, 0]]).T, np.matrix([[0, 0]]).T, T_s, ) x = np.zeros((6, t.size)) x_ref = np.zeros((2, t.size)) for i, time in enumerate(t): x[:, i] = np.reshape(p.x.T, 6) x_ref[:, i] = np.reshape(c.ref.T, 2) p.iterate_state() c.y = p.y c.iterate_state() p.u = c.u x[:, -1] = np.reshape(p.x.T, 6) plt.plot(x[0, :], x[1, :], label='boat') plt.plot(x_ref[0, :], x_ref[1, :], label='reference') plt.legend() plt.axis('equal') plt.grid() plt.show()
def MarginalDistributionFromMRAP (H): """ Returns the phase type distributed marginal distribution of a marked rational arrival process. Parameters ---------- H : list/cell of matrices of shape(M,M), length(N) The H0...HN matrices of the MRAP precision : double, optional Numerical precision for checking if the input is valid. The default value is 1e-14 Returns ------- alpha : matrix, shape (1,M) The initial vector of the matrix exponentially distributed marginal A : matrix, shape (M,M) The matrix parameter of the matrix exponentially distributed marginal """ if butools.checkInput and not CheckMRAPRepresentation (H): raise Exception("MarginalDistributionFromMRAP: Input is not a valid MRAP representation!") Hk = ml.matrix(H[1]) for i in range (2, len(H)): Hk += H[i] return (DRPSolve(la.inv(-H[0])*Hk), H[0])
def Scale2dMat(sx, sy): """ Return an affine translation matrix that scales the points toward/away from the origin by a factor of sx on x-axis and sy on the y-axis. """ return matrix([[sx, 0, 0], [0, sy, 0], [0, 0, 1]])
def __init__(self, **kw): super(AffineTransform,self).__init__(**kw) # This buffer prevents having to allocate memory for each point. self._op_buf = matrix([[0.0], [0.0], [1.0]])
def Rotate2dMat(t): """ Return an affine transformation matrix that rotates the points around the origin by t radians. """ return matrix([[cos(t), -sin(t), 0], [sin(t), cos(t), 0], [ 0 , 0 , 1]])
def Translate2dMat(xoff,yoff): """ Return an affine transformation matrix that translates points by the offset (xoff,yoff). """ return matrix([[1, 0, xoff], [0, 1, yoff], [0, 0, 1 ]])
def RotationZ(joint_angle): Tz = identity(4) c = math.cos(joint_angle) s = math.sin(joint_angle) Tz = matrix([[c,-s,0,0], [s,c,0,0], [0,0,1,0], [0,0,0,1]]) return Tz
def RotationY(joint_angle): Ty = identity(4) c = math.cos(joint_angle) s = math.sin(joint_angle) Ty = matrix([[c,0,s,0], [0,1,0,0], [-s,0,c,0], [0,0,0,1]]) return Ty
def RotationX(joint_angle): Tx = identity(4) c = math.cos(joint_angle) s = math.sin(joint_angle) Tx = matrix([[1,0,0,0], [0,c,-s,0], [0,s,c,0], [0,0,0,1]]) return Tx
def __new__(cls, m, n): """ """ if not all(( isinstance(m, int), isinstance(n, int), )): raise ValueError data = matlib.hstack([ matlib.vstack(( matlib.hstack((matlib.identity(n, dtype=int), matlib.matrix(p, dtype=int).T)), matlib.hstack((matlib.matrix(p, dtype=int), matlib.ones(1))), )) for p in product(range(m), repeat=n) ]) return super().__new__(cls, data, dtype=int).view(cls)
def GM1StationaryDistr (B, R, K): """ Returns the stationary distribution of the G/M/1 type Markov chain up to a given level K. Parameters ---------- A : length(M) list of matrices of shape (N,N) Matrix blocks of the G/M/1 type generator in the regular part, from 0 to M-1. B : length(M) list of matrices of shape (N,N) Matrix blocks of the G/M/1 type generator at the R : matrix, shape (N,N) Matrix R of the G/M/1 type Markov chain K : integer The stationary distribution is returned up to this level. Returns ------- pi : array, shape (1,(K+1)*N) The stationary probability vector up to level K """ if not isinstance(B,np.ndarray): B = np.vstack(B) m = R.shape[0] I = ml.eye(m) temp = (I-R).I if np.max(temp<-100*butools.checkPrecision): raise Exception("The spectral radius of R is not below 1: GM1 is not pos. recurrent") maxb = B.shape[0]//m BR = B[(maxb-1)*m:,:] for i in range(maxb-1,0,-1): BR = R * BR + B[(i-1)*m:i*m,:] pix = DTMCSolve(BR) pix = pix / np.sum(pix*temp) pi = [pix] sumpi = np.sum(pix) numit = 1 while sumpi < 1.0-1e-10 and numit < 1+K: pix = pix*R; # compute pi_(numit+1) numit += 1 sumpi += np.sum(pix); pi.append(ml.matrix(pix)) if butools.verbose: print("Accumulated mass after ", numit, " iterations: ", sumpi) if butools.verbose and numit == K+1: print("Maximum Number of Components ", numit-1, " reached") return np.hstack(pi)