def harris(img, parch_size, kappa): sobel_para = np.asarray([-1, 0, 1]) sobel_orth = np.asarray([1, 2, 1]) Ix = sp.signal.convolve2(np.transpose(sobel_orth), sobel_para, img, 'valid') Iy = sp.signal.convolve2(np.transpose(sobel_para), sobel_orth, img, 'valid') Ixx = np.square(Ix) Iyy = np.square(Iy) Ixy = np.multiply(Ix, Iy) patch = np.ones(patch_size, patch_size) pr = np.floor(patch_size / 2) # patch radius sIxx = sp.signal.convolve2(Ixx, patch, 'valid') sIyy = sp.signal.convolve2(Iyy, patch, 'valid') sIxy = sp.signal.convolve2(Ixy, patch, 'valid') scores = (np.mult(sIxx, sIyy) - np.square(sIxy)) - np.mult( kappa, np.square(sIxx + sIyy)) scores[scores < 0] = 0 scores = np.pad(scores, np.array((1 + pr, 1 + pr)))
def plot_phi2d(point, ax, withAlpha=False, step=0.05): xval = yval = np.arange(0, 1 + step, step) zval = [ x * y for x in point.getPhi(1, xval) for y in point.getPhi(2, yval) ] if withAlpha: np.mult(point.alpha, zval) xmesh, ymesh = np.meshgrid(xval, yval) zmesh = np.reshape(zval, xmesh.shape) ax.plot_surface(xmesh, ymesh, zmesh, cstride=1, rstride=1)
def plot_phi2d(point, ax, withAlpha = False, step = 0.05): xval = yval = np.arange(0, 1 + step, step) zval = [x * y for x in point.getPhi(1, xval) for y in point.getPhi(2, yval)] if withAlpha: np.mult(point.alpha, zval) xmesh, ymesh = np.meshgrid(xval, yval) zmesh = np.reshape(zval, xmesh.shape) ax.plot_surface(xmesh, ymesh, zmesh, cstride=1, rstride=1)
def coeffs(u1): """ Calculate coefficients of basis polynomials and weights """ wL = solve(ML, u1[:N+1]) wR = solve(MR, u1[N:]) oL = weights(wL, λs) oR = weights(wR, λs) if N==1: return (mult(wL,oL) + mult(wR,oR)) / (oL + oR) wCL = solve(MCL, u1[fhN:fhN2]) oCL = weights(wCL, λc) if nStencils==3: return (mult(wL,oL) + mult(wCL,oCL) + mult(wR,oR)) / (oL + oCL + oR) oCR = weights(wCR, λc) wCR = solve(MCR, u1[chN:chN2]) return (mult(wL,oL) + mult(wCL,oCL) + mult(wCR,oCR) + mult(wR,oR)) / (oL + oCL + oCR + oR)
def setup(self, rand_state, prev_layer): input_shape = prev_layer.output.shape self.batch_size = input_shape[0] input_size = np.mult(input_shape[1:]) self.init_weights(rand_state, input_size) # input is a view to output of previous layer self.input_ = prev_layer.output.reshape((self.batch_size, input_size)) self.input_grad = prev_layer.output_grad.reshape( (self.batch_size, input_size)) # allocate output self.output = np.empty((self.batch_size, self.output_size)) self.output_grad = np.empty(self.output.shape) # allocate weights self.weights = np.empty((input_size, self.output_size)) self.biases = np.empty(self.output_size) self.weights_grad = np.zeros(self.weights.shape) self.biases_grad = np.zeros(self.biases.shape) self.init_weights(rand_state)
def callback(self, odom, traj, accel): X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) V = np.dot(R, _V) acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z - 9.8]]) #acc = np.dot(R, _acc) #print acc # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) """np.putmask(Omega, abs(Omega)<1e-9,0);""" Omega_hat = ar([[0, -Omega[2][0], Omega[1][0]], [Omega[2][0], 0, -Omega[0][0]], [-Omega[1][0], Omega[0][0], 0]]) # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction #Xd = ar([[traj.desired_position.x], [traj.desired_position.y], [traj.desired_position.z]]) Vd = ar([[traj.desired_velocity.x], [traj.desired_velocity.y], [traj.desired_velocity.z]]) ad = ar([[traj.desired_acceleration.x], [traj.desired_acceleration.y], [traj.desired_acceleration.z]]) b1d = ar([[traj.desired_direction.x], [traj.desired_direction.y], [traj.desired_direction.z]]) b1d_dot = ar([[traj.desired_direction_dot.x], [traj.desired_direction_dot.y], [traj.desired_direction_dot.z]]) b1d_ddot = ar([[traj.desired_direction_ddot.x], [traj.desired_direction_ddot.y], [traj.desired_direction_ddot.z]]) #correction = np.array([[0],[0],[0.1]]) ex = ar([[0], [0], [0]]) #X-Xd#-correction ev = V - Vd _b3c = -(mult(self.kx, ex) + mult(self.kv, ev)) / self.m + self.g * tp( self.e[:, 2][np.newaxis]) + ad # desired direction b3c = ar(_b3c / np.linalg.norm(_b3c)) # get a normalized vector #b1c = -tp(np.cross(tp(b3c), np.cross(tp(b3c),tp(b1d))))/np.linalg.norm(np.cross(tp(b3c), tp(b1d))) #b2c = tp(np.cross(tp(b3c), tp(b1c))) b2c = tp( np.cross(tp(b3c), tp(b1d)) / np.linalg.norm(np.cross(tp(b3c), tp(b1d)))) # vector b2d b1c = tp(np.cross(tp(b2c), tp(b3c))) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix f0 = open('rotation.txt', 'a') f0.write("x:%s,%s\n" % (R, Rc)) """ if self.counter != 0: delt = time.time()-self.previous_time Rc_dot = (Rc-self.Rc_)/delt Rc_ddot = (Rc_dot-self.Rc_dot_)/delt self.Rc_ = Rc; self.Rc_dot_ = Rc_dot self.previous_time = time.time() else: Rc_dot = np.column_stack((tp(np.zeros((1,3))), tp(np.zeros((1,3))), tp(np.zeros((1,3))))); self.Rc_ = Rc Rc_ddot = np.column_stack((tp(np.zeros((1,3))), tp(np.zeros((1,3))), tp(np.zeros((1,3))))); self.Rc_dot_ = Rc_dot self.previous_time = time.time() """ if self.counter != 0: delt = time.time() - self.previous_time ad_dot = (ad - self._ad) / delt ad_ddot = (ad_dot - self._ad_dot) / delt V_ddot = (acc - self.acc_) / delt self._ad = ad self.acc_ = acc self._ad_dot = ad_dot self.previous_time = time.time() else: ad_dot = tp(np.zeros((1, 3))) self._ad = ad V_ddot = tp(np.zeros((1, 3))) self.acc_ = acc ad_ddot = tp(np.zeros((1, 3))) self._ad_dot = ad_dot self.previous_time = time.time() """ if self.counter == 0: ad_dot = tp(np.zeros((1,3))); ad_ddot = tp(np.zeros((1,3))); V_ddot = tp(np.zeros((1,3))) self.ad_series.append(ad); self.ad_dot_series.append(ad_dot) self.acc_series.append(acc) ; self.timeseries.append(time.time()) elif self.counter == 1: delt = time.time()-self.timeseries[-1] ad_dot = (ad-self.ad_series[-1])/delt; ad_ddot = (ad_dot-self.ad_dot_series[-1])/delt V_ddot = (acc-self.acc_series[-1])/delt self.ad_series.append(ad); self.ad_dot_series.append(ad_dot) self.acc_series.append(acc) ; self.timeseries.append(time.time()) else: delt = (time.time()-self.timeseries[-2])/2 ad_dot = (3*ad-4*self.ad_series[-1]+self.ad_series[-2])/(2*delt) ad_ddot = (3*ad_dot-4*self.ad_dot_series[-1]+self.ad_dot_series[-2])/(2*delt) V_ddot = (3*acc-4*self.acc_series[-1]+self.acc_series[-2])/(2*delt) self.ad_series.append(ad); self.ad_dot_series.append(ad_dot) self.acc_series.append(acc) ; self.timeseries.append(time.time()) self.ad_series.pop(0); self.ad_dot_series.pop(0); self.acc_series.pop(0); self.timeseries.pop(0) """ A = (mult(self.kx, ex) + mult(self.kv, ev) / self.m - self.g * tp(self.e[:, 2][np.newaxis]) - ad) ex_dot = ev ev_dot = acc - ad A_dot = (mult(self.kx, ex_dot) + mult(self.kv, ev_dot) / self.m - ad_dot) # calculate desired direction b3c_dot = -A_dot / np.linalg.norm(A) + (np.dot(tp(A), A_dot) / np.linalg.norm(A)**3) * A C = tp(np.cross(tp(b3c), tp(b1d))) C_dot = tp( np.cross(tp(b3c_dot), tp(b1d)) + np.cross(tp(b3c), tp(b1d_dot))) b2c_dot = C_dot / np.linalg.norm(C) - (np.dot(tp(C), C_dot) / np.linalg.norm(C)**3) * C b1c_dot = tp( np.cross(tp(b2c_dot), tp(b3c)) + np.cross(tp(b2c), tp(b3c_dot))) Rc_dot = np.column_stack( (b1c_dot, b2c_dot, b3c_dot)) # desired rotation matrix derivative #Omega_c_hat = np.dot(tp(Rc),Rc_dot) # skew-symmetric desired angular velocity Omega_c_hat = np.dot(Rc.transpose(), Rc_dot) Omega_c = ar([[-Omega_c_hat[1][2]], [Omega_c_hat[0][2]], [-Omega_c_hat[0][1]]]) ex_ddot = ev_dot ev_ddot = V_ddot - ad_dot A_ddot = (mult(self.kx, ex_ddot) + mult(self.kv, ev_ddot)) / self.m - ad_ddot b3c_ddot = -A_ddot/np.linalg.norm(A) + 2*(np.dot(tp(A),A_dot)/np.linalg.norm(A)**3)*A_dot +\ (np.linalg.norm(A_dot)**2+np.dot(tp(A),A_ddot))*A/np.linalg.norm(A)**3 - 3*(np.dot(tp(A),A_dot)**2/np.linalg.norm(A)**5)*A C_ddot = tp( np.cross(tp(b3c_ddot), tp(b1d)) + 2 * np.cross(tp(b3c_dot), tp(b1d_dot)) + np.cross(tp(b3c), tp(b1d_ddot))) b2c_ddot = C_ddot/np.linalg.norm(C) - 2*(np.dot(tp(C),C_dot)/np.linalg.norm(C)**3)*C_dot -\ (np.linalg.norm(C_dot)**2+np.dot(tp(C),C_ddot))*C/np.linalg.norm(C)**3 + 3*(np.dot(tp(C),C_dot)**2/np.linalg.norm(C)**5)*C b1c_ddot = tp( np.cross(tp(b2c_ddot), tp(b3c)) + 2 * np.cross(tp(b2c_dot), tp(b3c_dot)) + np.cross(tp(b2c), tp(b3c_ddot))) Rc_ddot = np.column_stack( (b1c_ddot, b2c_ddot, b3c_ddot)) # desired rotation matrix derivative Omega_c_dot_hat = np.dot(tp(Rc), Rc_ddot) + np.dot( Omega_c_hat.transpose(), Omega_c_hat) Omega_c_dot = ar([[-Omega_c_dot_hat[1][2]], [Omega_c_dot_hat[0][2]], [-Omega_c_dot_hat[0][1]]]) eR_hat = 0.5 * (np.dot(tp(Rc), R) - np.dot(tp(R), Rc) ) # eR skew symmetric matrix eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation eOmega = Omega - np.dot(np.dot( tp(R), Rc), Omega_c) # vector that gives error in angular velocity f1 = open('omega.txt', 'a') f1.write("x:%s,%s,%s,xd:%s,%s,%s\n" % (Omega[0][0], Omega[1][0], Omega[2][0], Omega_c[0][0], Omega_c[1][0], Omega_c[2][0])) f2 = open('rotation_error.txt', 'a') f2.write("x:%s,%s,%s,xd:%s,%s,%s\n" % (eR[0][0], eR[1][0], eR[2][0], eOmega[0][0], eOmega[1][0], eOmega[2][0])) f3 = open('position.txt', 'a') f3.write("%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s\n" % (X[0][0],X[1][0],X[2][0], V[0][0],V[1][0],V[2][0], \ acc[0][0],acc[1][0],acc[2][0], V_ddot[0][0],V_ddot[1][0],V_ddot[2][0])) Z = np.dot(np.dot(Omega_hat.dot(tp(R)), Rc), Omega_c) - np.dot( np.dot(tp(R), Rc), Omega_c_dot) self.f = self.m * np.dot(tp(_b3c), tp(R[:, 2][np.newaxis])) self.M1 = -1.0*(mult(self.kR_normalized, eR) + mult(self.kOmega_normalized, eOmega)) + \ tp(np.cross(tp(Omega), tp(Omega))) - Z self.M = np.dot(self.J, self.M1) T = tp(ar([[self.M[0][0], self.M[1][0], self.M[2][0], self.f[0][0]]])) c1 = tp( ar([[0, -self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t]])) c2 = tp( ar([[self.d * self.tau_t, 0, -self.tau_t * self.tau_m, self.tau_t]])) c3 = tp( ar([[0, self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t]])) c4 = tp( ar([[ -self.d * self.tau_t, 0, -self.tau_t * self.tau_m, self.tau_t ]])) C = np.column_stack( (c1, c2, c3, c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(np.linalg.inv(C), T) w = np.sqrt(np.abs(w_square)) Msg = Actuators() Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] self.pub.publish(Msg) self.counter += 1
def send_commands(self, odom, traj): """send command to px4""" X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) # suppressed for now as Patrick's code doesnt give angular velocities, not needed also with Pixhawk and approximate # controller # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? #Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) #Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction Xd = ar([[traj.pdes.x], [traj.pdes.y], [traj.pdes.z]]) Vd = ar([[traj.vdes.x], [traj.vdes.y], [traj.vdes.z]]) ad = ar([[traj.ades.x], [traj.ades.y], [traj.ades.z]]) b1d = ar([[traj.ddes.x], [traj.ddes.y], [traj.ddes.z]]) ex = ar([[0],[0],[0]]) if traj.controller == 1 else X-Xd ev = V-Vd #if self.counter==0: # ei = 0; self.ei = ei #else: # ei = (ex+ev)*self.dt; ei = self.ei+ei; self.ei = ei #_b3c = -(mult(self.kx, ex) + mult(self.kv, ev)+ mult(self.ki, ei))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction _b3c = -(mult(self.kx, ex) + mult(self.kv, ev))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction b3c = ar(_b3c/np.linalg.norm(_b3c)) # get a normalized vector b2c = tp(np.cross(b3c.T,b1d.T)/np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix #qc = self.rotmat2quat(Rc) #qc = rowan.to_matrix(Rc) qc = self.rotmat2quat(Rc) #print 'qc', qc omega_des = q.inverse*qc phi = np.arctan2(b2c[1][0], b2c[0][0]) if phi < 0: phi = phi + 2 * np.pi #error = 0.5 * np.trace(self.e-np.dot(Rc.T, R)) if self.counter != 0: phi_dot = (phi-self.phi_)/self.dt; self.phi_ = phi else: phi_dot = 0; self.phi_ = phi Omega_c = ar([[0], [0], [phi_dot]]) #eR_hat = 0.5*(np.dot(Rc.T, R) - np.dot(R.T, Rc)) # eR skew symmetric matrix # #eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation #eOmega = Omega - np.dot(np.dot(R.T, Rc), Omega_c) # vector that gives error in angular velocity self.f = self.m*np.dot(_b3c.T, tp(R[:,2][np.newaxis]))/self.norm_thrust_constant # normalized thrust #self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp(np.cross(Omega.T, tp(np.dot(self.J,Omega)))) msg = AttitudeTarget() msg.header.stamp = odom.header.stamp msg.type_mask = 128 msg.body_rate.x = self.factor*np.sign(omega_des[0])*omega_des[1] msg.body_rate.y = self.factor*np.sign(omega_des[0])*omega_des[2] msg.body_rate.z = self.factor*np.sign(omega_des[0])*omega_des[3] msg.thrust = min(1.0, self.f[0][0]) print 'thrust:', self.f*self.norm_thrust_constant, 'thrust_factor:', min(1.0, self.f[0][0]) print 'omega_des',msg.body_rate.x, msg.body_rate.y, msg.body_rate.z print 'zheight', traj.pdes.z f0 = open(self.path + 'commands.txt', 'a') f0.write("%s, %s, %s, %s\n" % (msg.thrust, msg.body_rate.x, msg.body_rate.y, msg.body_rate.z)) self.counter = self.counter+1 self.pub.publish(msg)
def callback(self, odom2, odom, accel, traj): X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) # next two matrices are position and orientation offset of the vi sensor imu with the center of gravity #vi_pos_off_hat = ar([[0.0, 0.03, 0.0], [-0.03, 0.0, -0.1], [0.0, 0.1, 0.0]]) #vi_rot_off = ar([[0.9950042, 0.0, 0.0998334], [0.0, 1.0, 0.0], [-0.0998334, 0.0, 0.9950042]]) #intermediate = np.dot(-vi_pos_off_hat, vi_rot_off.T) #V11 = _V + np.dot(vi_pos_off_hat, Omega) #Omega = np.dot(vi_rot_off.T, Omega) Omega_hat = ar([[0, -Omega[2][0], Omega[1][0]], [Omega[2][0], 0, -Omega[0][0]], [-Omega[1][0], Omega[0][0], 0]]) #VV = ar([[odom2.twist.twist.linear.x], [odom2.twist.twist.linear.y], [odom2.twist.twist.linear.z]]) _acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z - 9.8]]) #print '1', R1 #R = np.dot(vi_rot_off.T, R1) #print '2', R #ff = open('rotation.txt', 'a') #ff.write("%s, %s\n" %(R1, R)) V1 = np.dot(R, _V) acc = np.dot(R, _acc) #f6 = open('velocity_imu_cog.txt', 'a') #f6.write("%s, %s, %s, %s, %s, %s\n" % (V11[0][0], V11[1][0], V11[2][0], V1[0][0], V1[1][0], V1[2][0])) # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction Xd = ar([[traj.desired_position.x], [traj.desired_position.y], [traj.desired_position.z]]) Vd = ar([[traj.desired_velocity.x], [traj.desired_velocity.y], [traj.desired_velocity.z]]) ad = ar([[traj.desired_acceleration.x], [traj.desired_acceleration.y], [traj.desired_acceleration.z]]) ad_dot = ar([[0], [0], [0]]) #ad_dot = ar([[traj.desired_jerk.x], [traj.desired_jerk.y], [traj.desired_jerk.z]]) b1d = ar([[traj.desired_direction.x], [traj.desired_direction.y], [traj.desired_direction.z]]) #b1d = ar([[1], [1], [0]]) b1d_dot = ar([[0], [0], [0]]) b1d_ddot = ar([[0], [0], [0]]) #current_time = time.time() #t = current_time - self.time #self.time_points.append(t) """ X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]); q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) V = np.dot(R, _V) acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z]]) #acc = np.dot(R, _acc) #print acc # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) #Omega = np.dot(R, _Omega) Omega_hat = ar([[0,-Omega[2][0], Omega[1][0]], [Omega[2][0],0,-Omega[0][0]], [-Omega[1][0], Omega[0][0], 0]]) # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction Xd = ar([[traj.desired_position.x], [traj.desired_position.y], [traj.desired_position.z]]) Vd = ar([[traj.desired_velocity.x], [traj.desired_velocity.y], [traj.desired_velocity.z]]) ad = ar([[traj.desired_acceleration.x], [traj.desired_acceleration.y], [traj.desired_acceleration.z]]) b1d = ar([[traj.desired_direction.x], [traj.desired_direction.y], [traj.desired_direction.z]]) b1d_dot = ar([[traj.desired_direction_dot.x], [traj.desired_direction_dot.y], [traj.desired_direction_dot.z]]) b1d_ddot = ar([[traj.desired_direction_ddot.x], [traj.desired_direction_ddot.y], [traj.desired_direction_ddot.z]]) f0= open('trajectory.txt', 'a') f0.write("%s, %s, %s, %s, %s, %s\n" % (X[0][0], X[1][0], X[2][0], Xd[0][0], Xd[1][0], Xd[2][0])) """ #correction = np.array([[0],[0],[0.1]]) ex = X - Xd #-correction ev = V1 - Vd _b3c = -(mult(self.kx, ex) + mult(self.kv, ev)) / self.m + self.g * self.e[:, 2][ np.newaxis].T + ad # desired direction b3c = ar(_b3c / np.linalg.norm(_b3c)) # get a normalized vector b2c = tp( np.cross(b3c.T, b1d.T) / np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix """ if self.counter != 0: delt = time.time()-self.previous_time Rc_dot = (Rc-self.Rc_)/delt Rc_ddot = (Rc_dot-self.Rc_dot_)/delt self.Rc_ = Rc; self.Rc_dot_ = Rc_dot self.previous_time = time.time() else: Rc_dot = np.column_stack((tp(np.zeros((1,3))), tp(np.zeros((1,3))), tp(np.zeros((1,3))))); self.Rc_ = Rc Rc_ddot = np.column_stack((tp(np.zeros((1,3))), tp(np.zeros((1,3))), tp(np.zeros((1,3))))); self.Rc_dot_ = Rc_dot self.previous_time = time.time() """ if self.counter != 0: self.current_time = time.time() delt = self.current_time - self.previous_time ad_dot = (ad - self._ad) / delt ad_ddot = (ad_dot - self._ad_dot) / delt V_ddot = (acc - self.acc_) / delt self._ad = ad self.acc_ = acc self._ad_dot = ad_dot self.previous_time = time.time() else: ad_dot = tp(np.zeros((1, 3))) self._ad = ad V_ddot = tp(np.zeros((1, 3))) self.acc_ = acc ad_ddot = tp(np.zeros((1, 3))) self._ad_dot = ad_dot self.previous_time = time.time() f0 = open('velocity.txt', 'a') f0.write("%s, %s, %s, %s, %s, %s\n" % (V1[0][0], V1[1][0], V1[2][0], Vd[0][0], Vd[1][0], Vd[2][0])) f1 = open('accel.txt', 'a') f1.write( "%s, %s, %s, %s, %s, %s\n" % (acc[0][0], acc[1][0], acc[2][0], ad[0][0], ad[1][0], ad[2][0])) f2 = open('position.txt', 'a') f2.write("%s, %s, %s, %s, %s, %s\n" % (X[0][0], X[1][0], X[2][0], Xd[0][0], Xd[1][0], Xd[2][0])) """ if self.counter == 0: ad_dot = tp(np.zeros((1,3))); ad_ddot = tp(np.zeros((1,3))); V_ddot = tp(np.zeros((1,3))) self.ad_series.append(ad); self.ad_dot_series.append(ad_dot) self.acc_series.append(acc) ; self.timeseries.append(time.time()) elif self.counter == 1: delt = time.time()-self.timeseries[-1] ad_dot = (ad-self.ad_series[-1])/delt; ad_ddot = (ad_dot-self.ad_dot_series[-1])/delt V_ddot = (acc-self.acc_series[-1])/delt self.ad_series.append(ad); self.ad_dot_series.append(ad_dot) self.acc_series.append(acc) ; self.timeseries.append(time.time()) else: delt = (time.time()-self.timeseries[-2])/2 ad_dot = (3*ad-4*self.ad_series[-1]+self.ad_series[-2])/(2*delt) ad_ddot = (3*ad_dot-4*self.ad_dot_series[-1]+self.ad_dot_series[-2])/(2*delt) V_ddot = (3*acc-4*self.acc_series[-1]+self.acc_series[-2])/(2*delt) self.ad_series.append(ad); self.ad_dot_series.append(ad_dot) self.acc_series.append(acc) ; self.timeseries.append(time.time()) self.ad_series.pop(0); self.ad_dot_series.pop(0); self.acc_series.pop(0); self.timeseries.pop(0) """ A = (mult(self.kx, ex) + mult( self.kv, ev)) / self.m - self.g * self.e[:, 2][np.newaxis].T - ad ex_dot = ev ev_dot = acc - ad A_dot = (mult(self.kx, ex_dot) + mult( self.kv, ev_dot)) / self.m - ad_dot # calculate desired direction b3c_dot = -A_dot / np.linalg.norm(A) + (np.dot(A.T, A_dot) / np.linalg.norm(A)**3) * A C = tp(np.cross(b3c.T, b1d.T)) C_dot = tp(np.cross(b3c_dot.T, b1d.T) + np.cross(b3c.T, b1d_dot.T)) b2c_dot = C_dot / np.linalg.norm(C) - (np.dot(C.T, C_dot) / np.linalg.norm(C)**3) * C b1c_dot = tp(np.cross(b2c_dot.T, b3c.T) + np.cross(b2c.T, b3c_dot.T)) Rc_dot = np.column_stack( (b1c_dot, b2c_dot, b3c_dot)) # desired rotation matrix derivative #Omega_c_hat = np.dot(tp(Rc),Rc_dot) # skew-symmetric desired angular velocity Omega_c_hat = np.dot(Rc.T, Rc_dot) #Omega_c_hat = ar([[0,0,0], [0,0,0], [0, 0, 0]]) Omega_c = ar([[-Omega_c_hat[1][2]], [Omega_c_hat[0][2]], [-Omega_c_hat[0][1]]]) ex_ddot = ev_dot ev_ddot = V_ddot - ad_dot A_ddot = (mult(self.kx, ex_ddot) + mult(self.kv, ev_ddot)) / self.m - ad_ddot b3c_ddot = -A_ddot/np.linalg.norm(A) + 2*(np.dot(A.T,A_dot)/np.linalg.norm(A)**3)*A_dot +\ (np.linalg.norm(A_dot)**2+np.dot(A.T,A_ddot))*A/np.linalg.norm(A)**3 - 3*(np.dot(A.T,A_dot)**2/np.linalg.norm(A)**5)*A C_ddot = tp( np.cross(b3c_ddot.T, b1d.T) + 2 * np.cross(b3c_dot.T, b1d_dot.T) + np.cross(b3c.T, b1d_ddot.T)) b2c_ddot = C_ddot/np.linalg.norm(C) - 2*(np.dot(C.T,C_dot)/np.linalg.norm(C)**3)*C_dot -\ (np.linalg.norm(C_dot)**2+np.dot(C.T,C_ddot))*C/np.linalg.norm(C)**3 + 3*(np.dot(C.T,C_dot)**2/np.linalg.norm(C)**5)*C b1c_ddot = tp( np.cross(b2c_ddot.T, b3c.T) + 2 * np.cross(b2c_dot.T, b3c_dot.T) + np.cross(b2c.T, b3c_ddot.T)) Rc_ddot = np.column_stack( (b1c_ddot, b2c_ddot, b3c_ddot)) # desired rotation matrix derivative #Omega_c_dot_hat = np.dot(Rc.T,Rc_ddot) + np.dot(Omega_c_hat.T,Omega_c_hat) Omega_c_dot_hat = np.dot(Rc.T, Rc_ddot) - np.linalg.matrix_power( Omega_c_hat, 2) #Omega_c_dot_hat = ar([[0,0,0], [0,0,0], [0, 0, 0]]) Omega_c_dot = ar([[-Omega_c_dot_hat[1][2]], [Omega_c_dot_hat[0][2]], [-Omega_c_dot_hat[0][1]]]) eR_hat = 0.5 * (np.dot(Rc.T, R) - np.dot(R.T, Rc) ) # eR skew symmetric matrix eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation eOmega = Omega - np.dot(np.dot( R.T, Rc), Omega_c) # vector that gives error in angular velocity #Z = np.dot(np.dot(Omega_hat.dot(R.T),Rc),Omega_c) - np.dot(np.dot(R.T,Rc), Omega_c_dot) Z = np.dot(np.dot(Omega_hat, np.dot(R.T, Rc)), Omega_c) - np.dot( np.dot(R.T, Rc), Omega_c_dot) self.f = self.m * np.dot(_b3c.T, tp(R[:, 2][np.newaxis])) self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp( np.cross(Omega.T, tp(np.dot(self.J, Omega)))) #- np.dot(self.J,Z) #print self.M Msg = control_inputs() Msg.header.stamp = rospy.Time.now() Msg.Total_thrust = self.f[0][0] Msg.Moment_x = self.M[0][0] Msg.Moment_y = self.M[1][0] Msg.Moment_z = self.M[2][0] """ T = tp(ar([[self.M[0][0],self.M[1][0],self.M[2][0],self.f[0][0]]])) c1 = tp(ar([[0, -self.d*self.tau_t, self.tau_t*self.tau_m, self.tau_t]])) c2 = tp(ar([[self.d*self.tau_t, 0, -self.tau_t*self.tau_m, self.tau_t]])) c3 = tp(ar([[0, self.d*self.tau_t, self.tau_t*self.tau_m, self.tau_t]])) c4 = tp(ar([[-self.d*self.tau_t, 0, -self.tau_t*self.tau_m, self.tau_t]])) C = np.column_stack((c1,c2,c3,c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(np.linalg.inv(C),T) w = np.sqrt(np.abs(w_square)) Msg = Actuators() Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] """ self.pub.publish(Msg) self.counter += 1
def callback(self, odom, accel, traj): """ X = ar([[odom.pose.pose.position.x-0.1], [odom.pose.pose.position.y], [odom.pose.pose.position.z+0.03]]); q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) _acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z-9.8]]) V11 = np.dot(R, _V); acc = np.dot(R, _acc) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) Omega_hat = ar([[0,-Omega[2][0], Omega[1][0]], [Omega[2][0],0,-Omega[0][0]], [-Omega[1][0], Omega[0][0], 0]]) #imu_off = np.array([[0],[0],[0.07999]]) # offset of imu #imu_off_hat = ar([[0,-imu_off[2][0], imu_off[1][0]], [imu_off[2][0],0,-imu_off[0][0]], [-imu_off[1][0], imu_off[0][0], 0]]) #V1 = V1 + np.dot(imu_off_hat, Omega) # next two matrices are position and orientation offset of the vi sensor imu with the center of gravity vi_pos_off_hat = ar([[0.0, 0.03, 0.0], [-0.03, 0.0, -0.1], [0.0, 0.1, 0.0]]) vi_rot_off =ar([[0.9950042, 0.0, 0.0998334], [0.0, 1.0, 0.0], [-0.0998334, 0.0, 0.9950042]]) intermediate = np.dot(vi_pos_off_hat, vi_rot_off) V1 = np.dot(vi_rot_off, V11) + np.dot(intermediate, Omega) # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction Xd = ar([[traj.desired_position.x], [traj.desired_position.y], [traj.desired_position.z]]) Vd = ar([[traj.desired_velocity.x], [traj.desired_velocity.y], [traj.desired_velocity.z]]) ad = ar([[traj.desired_acceleration.x], [traj.desired_acceleration.y], [traj.desired_acceleration.z]]) ad_dot = ar([[traj.desired_jerk.x], [traj.desired_jerk.y], [traj.desired_jerk.z]]) b1d = ar([[traj.desired_direction.x], [traj.desired_direction.y], [traj.desired_direction.z]]) b1d_dot = ar([[0], [0], [0]]) b1d_ddot = ar([[0], [0], [0]]) X = ar([[odom.pose.pose.position.x-0.1], [odom.pose.pose.position.y], [odom.pose.pose.position.z+0.03]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) # next two matrices are position and orientation offset of the vi sensor imu with the center of gravity vi_pos_off_hat = ar([[0.0, -0.03, 0.0], [0.03, 0.0, 0.1], [0.0, -0.1, 0.0]]) vi_rot_off = ar([[0.9950042, 0.0, -0.0998334], [0.0, 1.0, 0.0], [0.0998334, 0.0, 0.9950042]]) intermediate = np.dot(vi_pos_off_hat, vi_rot_off) V11 = np.dot(vi_rot_off, _V) + np.dot(intermediate, Omega) V1 = np.dot(R, V11) Omega = np.dot(vi_rot_off, Omega) Omega_hat = ar([[0,-Omega[2][0], Omega[1][0]], [Omega[2][0],0,-Omega[0][0]], [-Omega[1][0], Omega[0][0], 0]]) #VV = ar([[odom2.twist.twist.linear.x], [odom2.twist.twist.linear.y], [odom2.twist.twist.linear.z]]) _acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z - 9.8]]) acc = np.dot(R, _acc) R = np.dot(vi_rot_off, R) """ X = ar([[odom.pose.pose.position.x - 0.1], [odom.pose.pose.position.y], [odom.pose.pose.position.z + 0.03]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R1 = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega1 = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) # next two matrices are position and orientation offset of the vi sensor imu with the center of gravity vi_pos_off_hat = ar([[0.0, 0.03, 0.0], [-0.03, 0.0, -0.1], [0.0, 0.1, 0.0]]) vi_rot_off = ar([[0.9950042, 0.0, 0.0998334], [0.0, 1.0, 0.0], [-0.0998334, 0.0, 0.9950042]]) intermediate = np.dot(vi_pos_off_hat, vi_rot_off) V1 = np.dot(vi_rot_off, _V) + np.dot(intermediate, Omega1) Omega = np.dot(vi_rot_off, Omega1) Omega_hat = ar([[0, -Omega[2][0], Omega[1][0]], [Omega[2][0], 0, -Omega[0][0]], [-Omega[1][0], Omega[0][0], 0]]) #VV = ar([[odom2.twist.twist.linear.x], [odom2.twist.twist.linear.y], [odom2.twist.twist.linear.z]]) acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z - 9.8]]) R = np.dot(vi_rot_off.T, R1) #f6 = open('velocity_imu_cog.txt', 'a') #f6.write("%s, %s, %s, %s, %s, %s\n" % (V11[0][0], V11[1][0], V11[2][0], V1[0][0], V1[1][0], V1[2][0])) # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction Xd = ar([[traj.desired_position.x], [traj.desired_position.y], [traj.desired_position.z]]) Vd = ar([[traj.desired_velocity.x], [traj.desired_velocity.y], [traj.desired_velocity.z]]) ad = ar([[traj.desired_acceleration.x], [traj.desired_acceleration.y], [traj.desired_acceleration.z]]) ad_dot = ar([[0], [0], [0]]) #ad_dot = ar([[traj.desired_jerk.x], [traj.desired_jerk.y], [traj.desired_jerk.z]]) b1d = ar([[traj.desired_direction.x], [traj.desired_direction.y], [traj.desired_direction.z]]) #b1d = ar([[1], [1], [0]]) b1d_dot = ar([[0], [0], [0]]) b1d_ddot = ar([[0], [0], [0]]) """ current_time = time.time() t = current_time - self.time self.time_points.append(t) self.smooth_vx.append(V1[0][0]); self.smooth_vy.append(V1[1][0]); self.smooth_vz.append(V1[2][0]) if self.counter < 5: V = V1; acc = tp(np.zeros((1,3))); V_ddot = tp(np.zeros((1,3))) else: t = list(np.linspace(min(self.time_points), max(self.time_points), self.counter+1)) if self.counter > 100: # number of points used in making spline t.pop(0); self.smooth_vx.pop(0); self.smooth_vy.pop(0); self.smooth_vz.pop(0) t = list(np.linspace(min(self.time_points), max(self.time_points), 101)) a = splrep(t, self.smooth_vx, k = 2, s = 2) b = splrep(t, self.smooth_vy, k = 2, s = 2) c = splrep(t, self.smooth_vz, k = 2, s = 2) _vx = splev(t, a); _vy = splev(t, b); _vz = splev(t, c) _ax = splev(t, a, der = 1); _ay = splev(t, b, der = 1); _az = splev(t, c, der = 1) _jx = splev(t, a, der = 2); _jy = splev(t, b, der = 2); _jz = splev(t, c, der = 2) V = ar([[_vx[-1]], [_vy[-1]], [_vz[-1]]]) acc = ar([[_ax[-1]], [_ay[-1]], [_az[-1]]]) V_ddot = ar([[_jx[-1]], [_jy[-1]], [_jz[-1]]]) if self.counter != 0: delt = (max(self.time_points)-min(self.time_points))/self.counter+1 ad_ddot = (ad_dot-self._ad_dot)/delt; self._ad_dot = ad_dot; self.previous_time = time.time() else: ad_ddot = tp(np.zeros((1,3))); self._ad_dot = ad_dot; self.previous_time = time.time() """ #correction = np.array([[0],[0],[0.1]]) if traj.controller == 1: ex = ar([[0], [0], [0]]) else: ex = X - Xd ev = V1 - Vd _b3c = -(mult(self.kx, ex) + mult(self.kv, ev)) / self.m + self.g * self.e[:, 2][ np.newaxis].T + ad # desired direction b3c = ar(_b3c / np.linalg.norm(_b3c)) # get a normalized vector b2c = tp( np.cross(b3c.T, b1d.T) / np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix current_time = time.time() t = current_time - self.time self.time_points.append(t) if self.counter != 0: delt = (max(self.time_points) - min(self.time_points)) / self.counter + 1 #self.current_time = time.time() #delt = self.current_time-self.previous_time #ad_dot = (ad-self._ad)/delt; ad_ddot = (ad_dot - self._ad_dot) / delt V_ddot = (acc - self.acc_) / delt #self._ad = ad; self.acc_ = acc self._ad_dot = ad_dot self.previous_time = time.time() else: #ad_dot = tp(np.zeros((1,3))); #self._ad = ad V_ddot = tp(np.zeros((1, 3))) self.acc_ = acc ad_ddot = tp(np.zeros((1, 3))) self._ad_dot = ad_dot self.previous_time = time.time() f = open('angular_velocity.txt', 'a') f.write("%s, %s, %s\n" % (Omega[0][0], Omega[1][0], Omega[2][0])) f0 = open('velocity.txt', 'a') f0.write("%s, %s, %s, %s, %s, %s\n" % (V1[0][0], V1[1][0], V1[2][0], Vd[0][0], Vd[1][0], Vd[2][0])) f1 = open('accel.txt', 'a') f1.write( "%s, %s, %s, %s, %s, %s\n" % (acc[0][0], acc[1][0], acc[2][0], ad[0][0], ad[1][0], ad[2][0])) f2 = open('position.txt', 'a') f2.write("%s, %s, %s, %s, %s, %s\n" % (X[0][0], X[1][0], X[2][0], Xd[0][0], Xd[1][0], Xd[2][0])) f3 = open('jerk.txt', 'a') f3.write("%s, %s, %s, %s, %s, %s\n" % (V_ddot[0][0], V_ddot[1][0], V_ddot[2][0], ad_dot[0][0], ad_dot[1][0], ad_dot[2][0])) #f4 = open('smoothing_compare.txt', 'a') #f4.write("%s, %s, %s, %s, %s, %s\n" % (V[0][0], V[1][0], V[2][0], V1[0][0], V1[1][0], V1[2][0])) A = (mult(self.kx, ex) + mult( self.kv, ev)) / self.m - self.g * self.e[:, 2][np.newaxis].T - ad #ex_dot = ar([[0],[0],[0]]) if traj.controller == 1: ex_dot = ar([[0], [0], [0]]) else: ex_dot = ev #ex_dot = ev ev_dot = acc - ad A_dot = (mult(self.kx, ex_dot) + mult( self.kv, ev_dot)) / self.m - ad_dot # calculate desired direction b3c_dot = -A_dot / np.linalg.norm(A) + (np.dot(A.T, A_dot) / np.linalg.norm(A)**3) * A C = tp(np.cross(b3c.T, b1d.T)) C_dot = tp(np.cross(b3c_dot.T, b1d.T) + np.cross(b3c.T, b1d_dot.T)) b2c_dot = C_dot / np.linalg.norm(C) - (np.dot(C.T, C_dot) / np.linalg.norm(C)**3) * C b1c_dot = tp(np.cross(b2c_dot.T, b3c.T) + np.cross(b2c.T, b3c_dot.T)) Rc_dot = np.column_stack( (b1c_dot, b2c_dot, b3c_dot)) # desired rotation matrix derivative #Omega_c_hat = np.dot(tp(Rc),Rc_dot) # skew-symmetric desired angular velocity Omega_c_hat = np.dot(Rc.T, Rc_dot) #Omega_c_hat = ar([[0,0,0], [0,0,0], [0, 0, 0]]) Omega_c = ar([[-Omega_c_hat[1][2]], [Omega_c_hat[0][2]], [-Omega_c_hat[0][1]]]) # ex_ddot = ar([[0],[0],[0]]) if traj.controller == 1: ex_ddot = ar([[0], [0], [0]]) else: ex_ddot = ev_dot ev_ddot = V_ddot - ad_dot A_ddot = (mult(self.kx, ex_ddot) + mult(self.kv, ev_ddot)) / self.m - ad_ddot b3c_ddot = -A_ddot/np.linalg.norm(A) + 2*(np.dot(A.T,A_dot)/np.linalg.norm(A)**3)*A_dot +\ (np.linalg.norm(A_dot)**2+np.dot(A.T,A_ddot))*A/np.linalg.norm(A)**3 - 3*(np.dot(A.T,A_dot)**2/np.linalg.norm(A)**5)*A C_ddot = tp( np.cross(b3c_ddot.T, b1d.T) + 2 * np.cross(b3c_dot.T, b1d_dot.T) + np.cross(b3c.T, b1d_ddot.T)) b2c_ddot = C_ddot/np.linalg.norm(C) - 2*(np.dot(C.T,C_dot)/np.linalg.norm(C)**3)*C_dot -\ (np.linalg.norm(C_dot)**2+np.dot(C.T,C_ddot))*C/np.linalg.norm(C)**3 + 3*(np.dot(C.T,C_dot)**2/np.linalg.norm(C)**5)*C b1c_ddot = tp( np.cross(b2c_ddot.T, b3c.T) + 2 * np.cross(b2c_dot.T, b3c_dot.T) + np.cross(b2c.T, b3c_ddot.T)) Rc_ddot = np.column_stack( (b1c_ddot, b2c_ddot, b3c_ddot)) # desired rotation matrix derivative #Omega_c_dot_hat = np.dot(Rc.T,Rc_ddot) + np.dot(Omega_c_hat.T,Omega_c_hat) Omega_c_dot_hat = np.dot(Rc.T, Rc_ddot) - np.linalg.matrix_power( Omega_c_hat, 2) #Omega_c_dot_hat = ar([[0,0,0], [0,0,0], [0, 0, 0]]) Omega_c_dot = ar([[-Omega_c_dot_hat[1][2]], [Omega_c_dot_hat[0][2]], [-Omega_c_dot_hat[0][1]]]) eR_hat = 0.5 * (np.dot(Rc.T, R) - np.dot(R.T, Rc) ) # eR skew symmetric matrix eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation eOmega = Omega - np.dot(np.dot( R.T, Rc), Omega_c) # vector that gives error in angular velocity #Z = np.dot(np.dot(Omega_hat.dot(R.T),Rc),Omega_c) - np.dot(np.dot(R.T,Rc), Omega_c_dot) Z = np.dot(np.dot(Omega_hat, np.dot(R.T, Rc)), Omega_c) - np.dot( np.dot(R.T, Rc), Omega_c_dot) self.f = self.m * np.dot(_b3c.T, tp(R[:, 2][np.newaxis])) self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp( np.cross(Omega.T, tp(np.dot(self.J, Omega)))) # - np.dot(self.J,Z) # correction factor to be applied to the moment vector to include the effect of the offset in imu mass theta = np.arctan2(b1d[1][0], b1d[0][0]) if theta < 0: theta = theta + 2 * np.pi Msg = control_inputs() Msg.header.stamp = rospy.Time.now() Msg.Total_thrust = self.f[0][0] Msg.Moment_x = self.M[0][0] - 0.14 * np.sin(theta) Msg.Moment_y = self.M[1][0] - 0.14 * np.cos( theta ) # this constant torque is added to offset the unbalanced weight of the firefly Msg.Moment_z = self.M[2][0] """ T = tp(ar([[self.M[0][0],self.M[1][0],self.M[2][0],self.f[0][0]]])) c1 = tp(ar([[0, -self.d*self.tau_t, self.tau_t*self.tau_m, self.tau_t]])) c2 = tp(ar([[self.d*self.tau_t, 0, -self.tau_t*self.tau_m, self.tau_t]])) c3 = tp(ar([[0, self.d*self.tau_t, self.tau_t*self.tau_m, self.tau_t]])) c4 = tp(ar([[-self.d*self.tau_t, 0, -self.tau_t*self.tau_m, self.tau_t]])) C = np.column_stack((c1,c2,c3,c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(np.linalg.inv(C),T) w = np.sqrt(np.abs(w_square)) Msg = Actuators() Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] """ self.pub.publish(Msg) self.counter += 1
def callback(self, odom, accel): a = time.time() X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) V = np.dot(R, _V) acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z - 9.8]]) #acc = np.dot(R, _acc) #print acc # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) #Omega = np.dot(R, _Omega) """np.putmask(Omega, abs(Omega)<1e-9,0);""" Omega_hat = ar([[0, -Omega[2][0], Omega[1][0]], [Omega[2][0], 0, -Omega[0][0]], [-Omega[1][0], Omega[0][0], 0]]) """ # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction Xd = ar([[traj.desired_position.x], [traj.desired_position.y], [traj.desired_position.z]]) Vd = ar([[traj.desired_velocity.x], [traj.desired_velocity.y], [traj.desired_velocity.z]]) ad = ar([[traj.desired_acceleration.x], [traj.desired_acceleration.y], [traj.desired_acceleration.z]]) b1d = ar([[traj.desired_direction.x], [traj.desired_direction.y], [traj.desired_direction.z]]) b1d_dot = ar([[traj.desired_direction_dot.x], [traj.desired_direction_dot.y], [traj.desired_direction_dot.z]]) b1d_ddot = ar([[traj.desired_direction_ddot.x], [traj.desired_direction_ddot.y], [traj.desired_direction_ddot.z]]) """ # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction Xd = ar([[0], [0], [1]]) Vd = ar([[0], [0], [0]]) ad = ar([[0], [0], [0]]) b1d = ar([[1], [0], [0]]) b1d_dot = ar([[0], [0], [0]]) b1d_ddot = ar([[0], [0], [0]]) correction = np.array([[0], [0], [0.1]]) ex = X - Xd - correction ev = V - Vd """ # new addition of initegral term if self.counter != 0: self.ct = time.time(); dt = self.ct-self.pt self.ei = self.ei + ex*dt; self.pt = time.time() else: self.pt = time.time() self.ei = tp(np.zeros((1,3))) if ex[0][0] <=0: self.a == -1 if ex[1][0] <=0: self.b == -1 if ex[2][0] <=0: self.c == -1 self.ki = np.array([[self.a*0.2], [self.b*0.2], [self.c*0.2]]) """ #_b3c = -(mult(self.kx, ex) + mult(self.kv, ev)+mult(self.ki,self.ei))/self.m + self.g*self.e[:,2][np.newaxis].T + ad _b3c = -(mult(self.kx, ex) + mult(self.kv, ev)) / self.m + self.g * self.e[:, 2][ np.newaxis].T + ad # desired direction #print np.linalg.norm(_b3c) b3c = ar(_b3c / np.linalg.norm(_b3c)) # get a normalized vector #b1c = -tp(np.cross(tp(b3c), np.cross(tp(b3c),tp(b1d))))/np.linalg.norm(np.cross(tp(b3c), tp(b1d))) #b2c = tp(np.cross(tp(b3c), tp(b1c))) b2c = tp( np.cross(b3c.T, b1d.T) / np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix f0 = open('rotation_desired.txt', 'a') f0.write("%s,%s,%s,%s,%s,%s,%s,%s,%s\n" % (b1c[0][0], b1c[1][0],b1c[2][0],b2c[0][0],b2c[1][0],b2c[2][0],b3c[0][0],\ b3c[1][0],b3c[2][0])) #ff0= open('rotation_current.txt', 'a') #ff0.write("%s,%s,%s,%s,%s,%s,%s,%s,%s\n" % (R[0][0],R[1][0],R[2][0],R[0][1],R[1][1],R[2][1],R[0][2],R[1][2],R[2][2])) """ if self.counter != 0: delt = time.time()-self.previous_time Rc_dot = (Rc-self.Rc_)/delt Rc_ddot = (Rc_dot-self.Rc_dot_)/delt self.Rc_ = Rc; self.Rc_dot_ = Rc_dot self.previous_time = time.time() else: Rc_dot = np.column_stack((tp(np.zeros((1,3))), tp(np.zeros((1,3))), tp(np.zeros((1,3))))); self.Rc_ = Rc Rc_ddot = np.column_stack((tp(np.zeros((1,3))), tp(np.zeros((1,3))), tp(np.zeros((1,3))))); self.Rc_dot_ = Rc_dot self.previous_time = time.time() """ if self.counter != 0: self.current_time = time.time() delt = self.current_time - self.previous_time #acc = (V-self._V)/delt ad_dot = (ad - self._ad) / delt ad_ddot = (ad_dot - self._ad_dot) / delt V_ddot = (acc - self.acc_) / delt self._V = V self._ad = ad self.acc_ = acc self._ad_dot = ad_dot self.previous_time = time.time() #f4= open('times.txt', 'a') #f4.write("%s,%s,%s\n" % (self.current_time, self.previous_time,delt)) else: self.current_time = time.time() #acc = tp(np.zeros((1,3))) ad_dot = tp(np.zeros((1, 3))) self._ad = ad self._V = V V_ddot = tp(np.zeros((1, 3))) self.acc_ = acc ad_ddot = tp(np.zeros((1, 3))) self._ad_dot = ad_dot self.previous_time = time.time() delt = self.current_time - self.previous_time """ f3 = open('position1.txt', 'a') f3.write("%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s\n" % (X[0][0],X[1][0],X[2][0], V[0][0],V[1][0],V[2][0], \ acc[0][0],acc[1][0],acc[2][0], V_ddot[0][0],V_ddot[1][0],V_ddot[2][0])) if self.counter == 0: ad_dot = tp(np.zeros((1,3))); ad_ddot = tp(np.zeros((1,3))); V_ddot = tp(np.zeros((1,3))) self.ad_series.append(ad); self.ad_dot_series.append(ad_dot) self.acc_series.append(acc) ; self.timeseries.append(time.time()) elif self.counter == 1: delt = time.time()-self.timeseries[-1] ad_dot = (ad-self.ad_series[-1])/delt; ad_ddot = (ad_dot-self.ad_dot_series[-1])/delt V_ddot = (acc-self.acc_series[-1])/delt self.ad_series.append(ad); self.ad_dot_series.append(ad_dot) self.acc_series.append(acc) ; self.timeseries.append(time.time()) else: delt = (time.time()-self.timeseries[-2])/2 ad_dot = (3*ad-4*self.ad_series[-1]+self.ad_series[-2])/(2*delt) ad_ddot = (3*ad_dot-4*self.ad_dot_series[-1]+self.ad_dot_series[-2])/(2*delt) V_ddot = (3*acc-4*self.acc_series[-1]+self.acc_series[-2])/(2*delt) self.ad_series.append(ad); self.ad_dot_series.append(ad_dot) self.acc_series.append(acc) ; self.timeseries.append(time.time()) self.ad_series.pop(0); self.ad_dot_series.pop(0); self.acc_series.pop(0); self.timeseries.pop(0) """ f3 = open('position2.txt', 'a') f3.write("%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s\n" % (X[0][0],X[1][0],X[2][0], V[0][0],V[1][0],V[2][0], \ acc[0][0],acc[1][0],acc[2][0], V_ddot[0][0],V_ddot[1][0],V_ddot[2][0])) #A = (mult(self.kx, ex) + mult(self.kv, ev)+mult(self.ki, ex))/self.m - self.g*self.e[:,2][np.newaxis].T - ad A = (mult(self.kx, ex) + mult( self.kv, ev)) / self.m - self.g * self.e[:, 2][np.newaxis].T - ad ex_dot = ev ev_dot = acc - ad A_dot = (mult(self.kx, ex_dot) + mult( self.kv, ev_dot)) / self.m - ad_dot # calculate desired direction b3c_dot = -A_dot / np.linalg.norm(A) + (np.dot(A.T, A_dot) / np.linalg.norm(A)**3) * A C = tp(np.cross(b3c.T, b1d.T)) C_dot = tp(np.cross(b3c_dot.T, b1d.T) + np.cross(b3c.T, b1d_dot.T)) b2c_dot = C_dot / np.linalg.norm(C) - (np.dot(C.T, C_dot) / np.linalg.norm(C)**3) * C b1c_dot = tp(np.cross(b2c_dot.T, b3c.T) + np.cross(b2c.T, b3c_dot.T)) Rc_dot = np.column_stack( (b1c_dot, b2c_dot, b3c_dot)) # desired rotation matrix derivative f5 = open('rotation_dot.txt', 'a') f5.write("%s,%s,%s,%s,%s,%s,%s,%s,%s\n" % (b1c_dot[0][0], b1c_dot[1][0],b1c_dot[2][0],b2c_dot[0][0],\ b2c_dot[1][0],b2c_dot[2][0],b3c_dot[0][0],b3c_dot[1][0],b3c_dot[2][0])) #Omega_c_hat = np.dot(tp(Rc),Rc_dot) # skew-symmetric desired angular velocity Omega_c_hat = np.dot(Rc.T, Rc_dot) #Omega_c_hat = ar([[0,0,0], [0,0,0], [0, 0, 0]]) Omega_c = ar([[-Omega_c_hat[1][2]], [Omega_c_hat[0][2]], [-Omega_c_hat[0][1]]]) ex_ddot = ev_dot ev_ddot = V_ddot - ad_dot #A_ddot = (mult(self.kx, ex_ddot) + mult(self.kv, ev_ddot)+mult(self.ki, ev))/self.m - ad_ddot A_ddot = (mult(self.kx, ex_ddot) + mult(self.kv, ev_ddot)) / self.m - ad_ddot b3c_ddot = -A_ddot/np.linalg.norm(A) + 2*(np.dot(A.T,A_dot)/np.linalg.norm(A)**3)*A_dot +\ (np.linalg.norm(A_dot)**2+np.dot(A.T,A_ddot))*A/np.linalg.norm(A)**3 - 3*(np.dot(A.T,A_dot)**2/np.linalg.norm(A)**5)*A C_ddot = tp( np.cross(b3c_ddot.T, b1d.T) + 2 * np.cross(b3c_dot.T, b1d_dot.T) + np.cross(b3c.T, b1d_ddot.T)) b2c_ddot = C_ddot/np.linalg.norm(C) - 2*(np.dot(C.T,C_dot)/np.linalg.norm(C)**3)*C_dot -\ (np.linalg.norm(C_dot)**2+np.dot(C.T,C_ddot))*C/np.linalg.norm(C)**3 + 3*(np.dot(C.T,C_dot)**2/np.linalg.norm(C)**5)*C b1c_ddot = tp( np.cross(b2c_ddot.T, b3c.T) + 2 * np.cross(b2c_dot.T, b3c_dot.T) + np.cross(b2c.T, b3c_ddot.T)) Rc_ddot = np.column_stack( (b1c_ddot, b2c_ddot, b3c_ddot)) # desired rotation matrix derivative #f7= open('rotation_ddot.txt', 'a') #f7.write("%s,%s,%s,%s,%s,%s,%s,%s,%s\n" % (b1c_ddot[0][0], b1c_ddot[1][0],b1c_ddot[2][0],b2c_ddot[0][0],\ #b2c_ddot[1][0],b2c_ddot[2][0],b3c_ddot[0][0], b3c_ddot[1][0],b3c_ddot[2][0])) Omega_c_dot_hat = np.dot(Rc.T, Rc_ddot) + np.dot( Omega_c_hat.T, Omega_c_hat) #Omega_c_dot_hat = np.dot(Rc.T,Rc_ddot) - np.linalg.matrix_power(Omega_c_hat,2) #Omega_c_dot_hat = ar([[0,0,0], [0,0,0], [0, 0, 0]]) Omega_c_dot = ar([[-Omega_c_dot_hat[1][2]], [Omega_c_dot_hat[0][2]], [-Omega_c_dot_hat[0][1]]]) eR_hat = 0.5 * (np.dot(Rc.T, R) - np.dot(R.T, Rc) ) # eR skew symmetric matrix eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation #Omega_c = np.array([[1],[0],[0]]) eOmega = Omega - np.dot(np.dot( R.T, Rc), Omega_c) # vector that gives error in angular velocity #eOmega = Omega - np.dot(np.dot(Rc.T, R), Omega_c) # vector that gives error in angular velocity # error function #phi = 0.5*np.trace(self.e-np.dot(Rc.T,R)) #ff2 = open('error_function.txt', 'a') #ff2.write("%s\n" % (phi)) f2 = open('rotation_error.txt', 'a') f2.write("%s,%s,%s,%s,%s,%s\n" % (eR[0][0], eR[1][0], eR[2][0], eOmega[0][0], eOmega[1][0], eOmega[2][0])) #f8 = open('desired_position.txt', 'a') #f8.write("%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s\n" % (Xd[0][0],Xd[1][0],Xd[2][0], Vd[0][0],Vd[1][0],Vd[2][0], \ #ad[0][0],ad[1][0],ad[2][0], ad_dot[0][0],ad_dot[1][0],ad_dot[2][0])) f55 = open('some_errors.txt', 'a') f55.write("%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s\n" % (ex[0][0], ex[1][0],ex[2][0],ev[0][0], ev[1][0],ev[2][0],\ ev_dot[0][0], ev_dot[1][0],ev_dot[2][0],ev_ddot[0][0], ev_ddot[1][0],ev_ddot[2][0])) #Z = np.dot(np.dot(Omega_hat.dot(R.T),Rc),Omega_c) - np.dot(np.dot(R.T,Rc), Omega_c_dot) Z = np.dot(np.dot(Omega_hat, np.dot(R.T, Rc)), Omega_c) - np.dot( np.dot(R.T, Rc), Omega_c_dot) self.f = self.m * np.dot(_b3c.T, tp(R[:, 2][np.newaxis])) #self.M1 = -(mult(self.kR_normalized, eR) + mult(self.kOmega_normalized, eOmega)) + tp(np.cross(Omega.T, Omega.T)) - Z #self.M = np.dot(self.J,self.M1) #self.M = tp(np.zeros((1,3))) self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp( np.cross(Omega.T, tp(np.dot(self.J, Omega)))) - np.dot(self.J, Z) #print self.M """ Msg = control_inputs() Msg.header.stamp = rospy.Time.now() Msg.Total_thrust = self.f[0][0] Msg.Moment_x = self.M[0][0] Msg.Moment_y = self.M[1][0] Msg.Moment_z = self.M[2][0] #f4 = open('forces.txt', 'a') #f4.write("%s,%s,%s,%s\n" % (self.f[0][0],self.M[0][0],self.M[1][0], self.M[2][0]) """ T = tp(ar([[self.M[0][0], self.M[1][0], self.M[2][0], self.f[0][0]]])) c1 = tp( ar([[0, -self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t]])) c2 = tp( ar([[self.d * self.tau_t, 0, -self.tau_t * self.tau_m, self.tau_t]])) c3 = tp( ar([[0, self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t]])) c4 = tp( ar([[ -self.d * self.tau_t, 0, -self.tau_t * self.tau_m, self.tau_t ]])) C = np.column_stack( (c1, c2, c3, c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(np.linalg.inv(C), T) w = np.sqrt(np.abs(w_square)) Msg = Actuators() Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] self.pub.publish(Msg) #self.pub.publish(Msg) #rospy.sleep(0.01) self.counter += 1 b = time.time() fa = open('time.txt', 'a') fa.write("%s,%s,%s,%s,%s,%s\n" % (a, b, b - a, self.current_time, self.previous_time, delt))
def odom_callback(self, odom): X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) #acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z]]) #V1 = _V#np.dot(R, _V); V1 = np.dot(R, _V); # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) #Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities """ X = ar([[odom.pose.position.x], [odom.pose.position.y], [odom.pose.position.z]]) q = Quaternion(odom.pose.orientation.w, odom.pose.orientation.x, odom.pose.orientation.y, odom.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.linear.x], [odom.twist.linear.y], [odom.twist.linear.z]]) #acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z]]) V1 = _V#np.dot(R, _V) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.angular.x], [odom.twist.angular.y], [odom.twist.angular.z]]) Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities """ """ # next two matrices are position and orientation offset of the vi sensor imu with the center of gravity vi_pos_off_hat = ar([[0.0, 0.03, 0.0], [-0.03, 0.0, -0.1], [0.0, 0.1, 0.0]]) vi_rot_off = ar([[0.9950042, 0.0, 0.0998334], [0.0, 1.0, 0.0], [-0.0998334, 0.0, 0.9950042]]) #vi_rot_off = ar([[0.7071, 0.0, 0.7071], [0.0, 1.0, 0.0], [-0.7071, 0.0, 0.7071]]) intermediate = np.dot(vi_pos_off_hat, vi_rot_off) V1 = np.dot(vi_rot_off,_V) + np.dot(intermediate, Omega) Omega = np.dot(vi_rot_off, Omega) R = np.dot(R, vi_rot_off.T) #Omega_hat = ar([[0,-Omega[2][0], Omega[1][0]], [Omega[2][0], 0, -Omega[0][0]], [-Omega[1][0], Omega[0][0], 0]]) V1 = np.dot(R, V1) #acc = np.dot(R, acc) """ # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction Xd = self.Xd Vd = self.Vd ad = self.ad ad_dot = self.ad_dot b1d = self.b1d b1d_dot = self.b1d_dot #b1d_dot = b1d_dot/np.linalg.norm(b1d_dot) if self.controller == 1: # velocity controller ex = ar([[0],[0],[0]]) else: ex = X-Xd ev = V1-Vd #_b3c = -(mult(self.kx, ex) + mult(self.kv, ev)+ mult(self.ki, ei))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction _b3c = -(mult(self.kx, ex) + mult(self.kv, ev))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction b3c = ar(_b3c/np.linalg.norm(_b3c)) # get a normalized vector b2c = tp(np.cross(b3c.T,b1d.T)/np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix """ phi = np.arctan2(b2c[1][0], b2c[0][0]) #phi = np.arctan2(b1d[1][0], b1d[0][0]) if phi < 0: phi = phi + 2 * np.pi error = 0.5 * np.trace(self.e-np.dot(Rc.T, R)) if self.counter != 0: phi_dot = (phi-self.phi_)/self.dt; self.phi_ = phi else: phi_dot = 0; self.phi_ = phi f0 = open('phi_phidot.txt', 'a') f0.write("%s, %s\n" % (phi, phi_dot)) Omega_c = ar([[0], [0], [phi_dot]]) """ if self.counter == 0: acc = tp(np.zeros((1,3))); self.previous_V = V1 else: acc = (V1-self.previous_V)/self.dt; self.previous_V = V1 # calculate derivative of rotation and find desired angular velocity A = (mult(self.kx, ex) + mult(self.kv, ev))/self.m - self.g*self.e[:,2][np.newaxis].T - ad ex_dot = ev; ev_dot = acc-ad A_dot = (mult(self.kx, ex_dot) + mult(self.kv, ev_dot))/self.m - ad_dot # calculate desired direction b3c_dot = -A_dot/np.linalg.norm(A) + (np.dot(A.T,A_dot)/np.linalg.norm(A)**3)*A C = tp(np.cross(b3c.T,b1d.T)) C_dot = tp(np.cross(b3c_dot.T, b1d.T) + np.cross(b3c.T, b1d_dot.T)) b2c_dot = C_dot/np.linalg.norm(C) - (np.dot(C.T,C_dot)/np.linalg.norm(C)**3)*C b1c_dot = tp(np.cross(b2c_dot.T, b3c.T) + np.cross(b2c.T, b3c_dot.T)) Rc_dot = np.column_stack((b1c_dot, b2c_dot, b3c_dot)) # desired rotation matrix derivative #Omega_c_hat = np.dot(tp(Rc),Rc_dot) # skew-symmetric desired angular velocity Omega_c_hat = np.dot(Rc.T, Rc_dot) #Omega_c_hat = ar([[0,0,0], [0,0,0], [0, 0, 0]]) Omega_c = ar([[-Omega_c_hat[1][2]], [Omega_c_hat[0][2]], [-Omega_c_hat[0][1]]]) f10= open('desired_ang_vel.txt', 'a') f10.write("%s, %s, %s\n" % ( Omega_c[0][0], Omega_c[1][0], Omega_c[2][0])) eR_hat = 0.5*(np.dot(Rc.T, R) - np.dot(R.T, Rc)) # eR skew symmetric matrix eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation eOmega = Omega - np.dot(np.dot(R.T, Rc), Omega_c) # vector that gives error in angular velocity #Z = np.dot(np.dot(Omega_hat.dot(R.T),Rc),Omega_c) - np.dot(np.dot(R.T,Rc), Omega_c_dot) #Z = np.dot(np.dot(Omega_hat,np.dot(R.T,Rc)),Omega_c) - np.dot(np.dot(R.T,Rc), Omega_c_dot) self.f = self.m*np.dot(_b3c.T, tp(R[:,2][np.newaxis])) #self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)+mult(self.ki_att, ei_att)) + tp(np.cross(Omega.T, tp(np.dot(self.J,Omega))))# - np.dot(self.J,Z) self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp(np.cross(Omega.T, tp(np.dot(self.J,Omega))))# - np.dot(self.J,Z) #f2= open('angular_velocity_comparison.txt', 'a') #f2.write("%s, %s, %s, %s, %s, %s\n" % (self.currentomega[0][0], self.omega[0][0], self.currentomega[1][0],\ #self.omega[1][0], self.currentomega[2][0], self.omega[2][0])) #a = tp(np.cross(self.omega.T, tp(np.dot(self.J,self.omega)))) #intermediate = moments-tp(np.cross(self.omega.T, tp(np.dot(self.J,self.omega)))) #self.omega = self.omega + self.dt* np.dot(np.linalg.inv(self.J), intermediate) T = tp(ar([[self.M[0][0],self.M[1][0],self.M[2][0],self.f[0][0]]])) msg2 = control_inputs() msg2.header.stamp = odom.header.stamp msg2.Total_thrust = T[3][0]; msg2.Moment_x = T[0][0]; msg2.Moment_y = T[1][0]; msg2.Moment_z = T[2][0] self.pub2.publish(msg2) f1= open('forces.txt', 'a') f1.write("%s, %s, %s, %s, %s\n" % (msg2.header.stamp, T[0][0], T[1][0], T[2][0], T[3][0])) #rospy.loginfo('moments and thrust is:%f, %f, %f, %f', T[0][0], T[1][0], T[2][0], T[3][0] ) Msg = Actuators() Msg.header.stamp = odom.header.stamp if self.uav == 'hummingbird': c1 = tp(ar([[0, 2/(self.d*self.tau_t), 0, -2/(self.d*self.tau_t)]])) c2 = tp(ar([[-2/(self.d*self.tau_t), 0, 2/(self.d*self.tau_t), 0]])) c3 = tp(ar([[1/(self.tau_t*self.m), -1/(self.tau_t*self.m), 1/(self.tau_t*self.m), -1/(self.tau_t*self.m)]])) c4 = tp(ar([[1/self.tau_t, 1/self.tau_t, 1/self.tau_t, 1/self.tau_t]])) C = 0.25*np.column_stack((c1,c2,c3,c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(C,T) #w_initial = np.array([[self.w0_h**2], [self.w0_h**2], [self.w0_h**2], [self.w0_h**2]]) w = np.sqrt(np.abs(w_square)) #w[w>800.0] = 800.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] elif self.uav== 'pelican': c1 = tp(ar([[0, 2/(self.d*self.tau_t), 0, -2/(self.d*self.tau_t)]])) c2 = tp(ar([[-2/(self.d*self.tau_t), 0, 2/(self.d*self.tau_t), 0]])) c3 = tp(ar([[1/(self.tau_t*self.m), -1/(self.tau_t*self.m), 1/(self.tau_t*self.m), -1/(self.tau_t*self.m)]])) c4 = tp(ar([[1/self.tau_t, 1/self.tau_t, 1/self.tau_t, 1/self.tau_t]])) C = 0.25*np.column_stack((c1,c2,c3,c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(C,T) #w_initial = np.array([[self.w0_p**2], [self.w0_p**2], [self.w0_p**2], [self.w0_p**2]]) #w = np.sqrt(np.abs(w_square+w_initial)) w = np.sqrt(np.abs(w_square)) #w[w>800.0] = 800.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] else: c1 = tp(ar([[sin(pi/6)*self.d*self.tau_t, -cos(pi/6)*self.d*self.tau_t, -self.tau_t*self.tau_m, self.tau_t]])) c2 = tp(ar([[sin(pi/2)*self.d*self.tau_t, -cos(pi/2)*self.d*self.tau_t, self.tau_t*self.tau_m, self.tau_t]])) c3 = tp(ar([[sin(5*pi/6)*self.d*self.tau_t, -cos(5*pi/6)*self.d*self.tau_t, -self.tau_t*self.tau_m, self.tau_t]])) c4 = tp(ar([[sin(7*pi/6)*self.d*self.tau_t, -cos(7*pi/6)*self.d*self.tau_t, self.tau_t*self.tau_m, self.tau_t]])) c5 = tp(ar([[sin(3*pi/2)*self.d*self.tau_t, -cos(3*pi/2)*self.d*self.tau_t, -self.tau_t*self.tau_m, self.tau_t]])) c6 = tp(ar([[sin(11*pi/6)*self.d*self.tau_t, -cos(11*pi/6)*self.d*self.tau_t, self.tau_t*self.tau_m, self.tau_t]])) C = np.column_stack((c1,c2,c3,c4,c5,c6)) # solving linear eq T = Cw^2 to get w^2 #inverted_matrix = np.dot(C.T, np.linalg.inv(np.dot(C, C.T))) #w_square = np.dot(inverted_matrix,T) w_square = np.dot(np.linalg.pinv(C),T) f2= open('ang_vel_sq.txt', 'a') f2.write("%s, %s, %s, %s, %s, %s, %s\n" % (Msg.header.stamp, w_square[0][0], w_square[1][0], w_square[2][0], w_square[3][0], w_square[4][0], w_square[5][0])) w = np.sqrt(np.abs(w_square)) #w[w>900.0] = 900.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0], w[4][0], w[5][0]] self.pub1.publish(Msg) self.counter+=1
def odom_callback(self, odom): """ control calculations""" #if self.counter == 0: # current_time = 0; self.start_time = time.time() #else: #current_time = time.time()-self.start_time current_time = odom.header.stamp.to_sec() self.ct = current_time if self.counter == 0: self.initial_odom_time = current_time X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) #acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z]]) #V1 = _V#np.dot(R, _V); V1 = np.dot(R, _V) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) #Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction try: index, value = min(enumerate(self.trajectory_time), key=lambda x: abs(x[1] - current_time)) Xd = self.despos[index] Vd = self.desvel[index] ad = self.desacc[index] b1d = self.desdirection[index] #print index, Xd, Vd, ad except: rospy.loginfo('Trajectory has not yet been received.') if self.controller == 1: # velocity controller ex = ar([[0], [0], [0]]) else: ex = X - Xd ev = V1 - Vd _b3c = -(mult(self.kx, ex) + mult(self.kv, ev)) / self.m + self.g * self.e[:, 2][ np.newaxis].T + ad # desired direction b3c = ar(_b3c / np.linalg.norm(_b3c)) # get a normalized vector b2c = tp( np.cross(b3c.T, b1d.T) / np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix ff = open(self.path + 'position_trajectory.txt', 'a') ff.write("%s, %s, %s, %s, %s, %s, %s\n" % (current_time, Xd[0][0], Xd[1][0], Xd[2][0], X[0][0], X[1][0], X[2][0])) ff = open(self.path + 'velocity_trajectory.txt', 'a') ff.write("%s, %s, %s, %s, %s, %s\n" % (Vd[0][0], Vd[1][0], Vd[2][0], V1[0][0], V1[1][0], V1[2][0])) phi = np.arctan2(b2c[1][0], b2c[0][0]) if phi < 0: phi = phi + 2 * np.pi error = 0.5 * np.trace(self.e - np.dot(Rc.T, R)) if self.counter != 0: phi_dot = (phi - self.phi_) / self.dt self.phi_ = phi else: phi_dot = 0 self.phi_ = phi Omega_c = ar([[0], [0], [phi_dot]]) eR_hat = 0.5 * (np.dot(Rc.T, R) - np.dot(R.T, Rc) ) # eR skew symmetric matrix eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation eOmega = Omega - np.dot(np.dot( R.T, Rc), Omega_c) # vector that gives error in angular velocity self.f = self.m * np.dot(_b3c.T, tp(R[:, 2][np.newaxis])) self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp( np.cross(Omega.T, tp(np.dot(self.J, Omega)))) # - np.dot(self.J,Z) T = tp( ar([[ self.M[0][0], self.M[1][0] - 0.27, self.M[2][0], self.f[0][0] ]])) # dummy torque to offset visensor moment Msg = Actuators() if self.uav == 'hummingbird': c1 = tp( ar([[ 0, 2 / (self.d * self.tau_t), 0, -2 / (self.d * self.tau_t) ]])) c2 = tp( ar([[ -2 / (self.d * self.tau_t), 0, 2 / (self.d * self.tau_t), 0 ]])) c3 = tp( ar([[ 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m), 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m) ]])) c4 = tp( ar([[ 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t ]])) C = 0.25 * np.column_stack( (c1, c2, c3, c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(C, T) #w_initial = np.array([[self.w0_h**2], [self.w0_h**2], [self.w0_h**2], [self.w0_h**2]]) w = np.sqrt(np.abs(w_square)) w[w > 800.0] = 800.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] elif self.uav == 'pelican': c1 = tp( ar([[ 0, 2 / (self.d * self.tau_t), 0, -2 / (self.d * self.tau_t) ]])) c2 = tp( ar([[ -2 / (self.d * self.tau_t), 0, 2 / (self.d * self.tau_t), 0 ]])) c3 = tp( ar([[ 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m), 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m) ]])) c4 = tp( ar([[ 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t ]])) C = 0.25 * np.column_stack( (c1, c2, c3, c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(C, T) w = np.sqrt(np.abs(w_square)) w[w > 800.0] = 800.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] else: c1 = tp( ar([[ sin(pi / 6) * self.d * self.tau_t, -cos(pi / 6) * self.d * self.tau_t, -self.tau_t * self.tau_m, self.tau_t ]])) c2 = tp( ar([[ sin(pi / 2) * self.d * self.tau_t, -cos(pi / 2) * self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t ]])) c3 = tp( ar([[ sin(5 * pi / 6) * self.d * self.tau_t, -cos(5 * pi / 6) * self.d * self.tau_t, -self.tau_t * self.tau_m, self.tau_t ]])) c4 = tp( ar([[ sin(7 * pi / 6) * self.d * self.tau_t, -cos(7 * pi / 6) * self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t ]])) c5 = tp( ar([[ sin(3 * pi / 2) * self.d * self.tau_t, -cos(3 * pi / 2) * self.d * self.tau_t, -self.tau_t * self.tau_m, self.tau_t ]])) c6 = tp( ar([[ sin(11 * pi / 6) * self.d * self.tau_t, -cos(11 * pi / 6) * self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t ]])) C = np.column_stack((c1, c2, c3, c4, c5, c6)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(np.linalg.pinv(C), T) w = np.sqrt(np.abs(w_square)) #w[w>900.0] = 900.0 Msg.angular_velocities = [ w[0][0], w[1][0], w[2][0], w[3][0], w[4][0], w[5][0] ] ff = open(self.path + 'motor_speeds.txt', 'a') ff.write("%s, %s, %s, %s, %s, %s\n" % (w[0][0], w[1][0], w[2][0], w[3][0], w[4][0], w[5][0])) self.pub.publish(Msg) self.counter += 1
def odom_callback(self, odom): """send commands to px4""" current_time = odom.header.stamp.to_sec() self.ct = current_time if self.counter == 0: self.initial_odom_time = current_time X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME. Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) #Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities try: index, value = min(enumerate(self.trajectory_time), key=lambda x: abs(x[1]-current_time)) Xd = self.despos[index]; Vd = self.desvel[index]; ad = self.desacc[index]; b1d = self.desdirection[index] #print index, Xd, Vd, ad except: rospy.loginfo('Trajectory has not yet been received.') if self.controller == 1: # velocity controller ex = ar([[0],[0],[0]]) else: ex = X-Xd ev = V-Vd _b3c = -(mult(self.kx, ex) + mult(self.kv, ev))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction b3c = ar(_b3c/np.linalg.norm(_b3c)) # get a normalized vector b2c = tp(np.cross(b3c.T,b1d.T)/np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix` qc = self.rotmat2quat(Rc) omega_des = q.inverse*qc thrust = self.m*np.dot(_b3c.T, tp(R[:,2][np.newaxis]))/self.norm_thrust_constant # normalized thrust msg = AttitudeTarget() msg.header.stamp = odom.header.stamp msg.type_mask = 128 msg.body_rate.x = self.factor_rp*np.sign(omega_des[0])*omega_des[1] msg.body_rate.y = self.factor_rp*np.sign(omega_des[0])*omega_des[2] msg.body_rate.z = self.factor_yaw*np.sign(omega_des[0])*omega_des[3] msg.thrust = min(1.0, thrust[0][0]) #print 'thrust:', thrust*self.norm_thrust_constant, 'thrust_factor:', min(1.0, thrust[0][0]) #print 'omega_des',msg.body_rate.x, msg.body_rate.y, msg.body_rate.z #print 'zheight', Xd[2][0] f1 = open(self.path+'position_trajectory_RN{}.txt'.format(self.RN), 'a') f1.write("%s, %s, %s, %s, %s, %s\n" % (Xd[0][0], Xd[1][0], Xd[2][0], X[0][0], X[1][0], X[2][0])) f2 = open(self.path+'velocity_trajectory_RN{}.txt'.format(self.RN), 'a') f2.write("%s, %s, %s, %s, %s, %s\n" % (Vd[0][0], Vd[1][0], Vd[2][0], V[0][0], V[1][0], V[2][0])) f3 = open(self.path + 'commands_generated_RN{}.txt'.format(self.RN), 'a') f3.write("%s, %s, %s, %s\n" % (msg.thrust, msg.body_rate.x, msg.body_rate.y, msg.body_rate.z)) self.counter = self.counter+1 self.pub.publish(msg)
def command_callback(self, odom): """send command to px4""" X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = self.angular_velocity #Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) #Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction Xd = self.pdes; Vd = self.vdes; ad = self.ades; b1d = self.ddes ex = ar([[0],[0],[0]]) if traj.controller == 1 else X-Xd ev = V-Vd _b3c = -(mult(self.kx, ex) + mult(self.kv, ev)) + self.g*self.e[:,2][np.newaxis].T + ad # desired direction b3c = ar(_b3c/np.linalg.norm(_b3c)) # get a normalized vector self.f = self.m* np.dot(_b3c.T, tp(R[:,2][np.newaxis]))/self.max_possible_thrust # normalized thrust yaw = np.arctan2(b2c[1][0], b2c[0][0]) yaw = yaw + 2 * np.pi if yaw < 0 else yaw # vary yaw between 0 and 360 degree # find rate of change of yaw and force vector if self.counter != 0: yaw_dot = (yaw-self.yaw_)/self.dt; self.yaw_ = yaw _b3c_dot = (_b3c-self._b3c_previous)/self.dt; self._b3c_previous = _b3c else: yaw_dot = 0; self.yaw_ = yaw _b3c_dot = ar([[0], [0], [0]]); self._b3c_previous = _b3c b1d = ar([[np.cos(yaw)], [np.sin(yaw)], [0]]) b1d_dot = ar([[-np.sin(yaw)*yaw_dot], [np.cos(yaw)*yaw_dot], [0]]) b2c = tp(np.cross(b3c.T,b1d.T)/np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix # take the derivatives middle1 = _b3c_dot/np.linalg.norm(_b3c) last_two = np.cross(middle1.T, b3c.T) b3c_dot = np.cross(b3c.T, last_two) middle2 = (np.cross(b3c_dot, b1d.T) + np.cross(b3c.T, b1d_dot.T))/np.linalg.norm(np.cross(b3c.T, b1d.T)) last_two = np.cross(middle2, b2c.T) b2c_dot = np.cross(b2c.T, last_two) b1c_dot = np.cross(b2c_dot, b3c.T) + np.cross(b2c.T, b3c_dot) Rc_dot = np.column_stack((b1c_dot.T, b2c_dot.T, b3c_dot.T)) # desired rotation matrix Omega_des = np.dot(Rc.T, Rc_dot) """ qc = self.rotmat2quat(Rc) omega_des = q.inverse*qc phi = np.arctan2(b2c[1][0], b2c[0][0]) if phi < 0: phi = phi + 2 * np.pi #error = 0.5 * np.trace(self.e-np.dot(Rc.T, R)) if self.counter != 0: phi_dot = (phi-self.phi_)/self.dt; self.phi_ = phi else: phi_dot = 0; self.phi_ = phi Omega_c = ar([[0], [0], [phi_dot]]) """ #eR_hat = 0.5*(np.dot(Rc.T, R) - np.dot(R.T, Rc)) # eR skew symmetric matrix # #eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation #eOmega = Omega - np.dot(np.dot(R.T, Rc), Omega_c) # vector that gives error in angular velocity #self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp(np.cross(Omega.T, tp(np.dot(self.J,Omega)))) msg = AttitudeTarget() msg.header.stamp = odom.header.stamp msg.type_mask = 128 msg.body_rate.x = Omega_des[1][2] msg.body_rate.y = -Omega_des[0][2] msg.body_rate.z = -Omega_des[0][1] #msg.body_rate.x = self.factor*np.sign(omega_des[0])*omega_des[1] #msg.body_rate.y = -self.factor*np.sign(omega_des[0])*omega_des[2] #msg.body_rate.z = -self.factor*np.sign(omega_des[0])*omega_des[3] msg.thrust = min(1.0, self.f[0][0]) print 'thrust', min(1.0, self.f[0][0]) print 'omega_des',msg.body_rate.x, msg.body_rate.y, msg.body_rate.z print 'zheight', traj.desired_position.z #f0 = open('commands.txt', 'a') #f0.write("%s, %s, %s, %s\n" % (msg.thrust, msg.body_rate.x, msg.body_rate.y, msg.body_rate.z)) self.counter = self.counter+1 self.pub.publish(msg)
def odom_callback(self, odom): """does the control calculations""" self.odom_time = odom.header.stamp.to_sec() X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) #acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z]]) #V1 = _V#np.dot(R, _V); V1 = np.dot(R, _V) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) #Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities t = float(self.odom_time - self.traj_start_time) if t <= self.traj_end_time[self.number_of_segments]: index = bisect.bisect(self.traj_end_time, t) - 1 else: index = bisect.bisect(self.traj_end_time, t) - 2 #fff = open('traj_time_in_odom.txt', 'a') #fff.write('%s, %s, %s, %s, %s\n' %(index, self.odom_time, self.traj_start_time, t, self.traj_end_time[self.number_of_segments])) if index == -1: pass else: xx = np.poly1d(self.p1[index]) vx = np.polyder(xx, 1) aax = np.polyder(xx, 2) jx = np.polyder(xx, 3) sx = np.polyder(xx, 4) yy = np.poly1d(self.p2[index]) vy = np.polyder(yy, 1) aay = np.polyder(yy, 2) jy = np.polyder(yy, 3) sy = np.polyder(yy, 4) zz = np.poly1d(self.p3[index]) vz = np.polyder(zz, 1) aaz = np.polyder(zz, 2) jz = np.polyder(zz, 3) sz = np.polyder(zz, 4) if t <= self.traj_end_time[self.number_of_segments]: if self.mode == 'hover' or self.mode == 'land': self.pdes = ar([[xx(t)], [yy(t)], [zz(t)]]) else: self.pdes = ar([[xx(t)], [yy(t)], [zz(t)]]) # needed to transform the trajectory back to cog #self.pdes = (np.dot(R.T, self.Rcg_vibase[0:3, 3:4]) + np.dot(self.Rcg_vibase[0:3, 0:3], self.pdes)) self.vdes = ar([[vx(t)], [vy(t)], [vz(t)]]) self.ades = ar([[aax(t)], [aay(t)], [aaz(t)]]) self.jdes = ar([[jx(t)], [jy(t)], [jz(t)]]) #d = self.ddes + t*(self.ddir)/self.traj_end_time[self.number_of_segments] self.ddes = self.ddir #self.vdes/np.linalg.norm(self.vdes)#self.ddir else: if self.mode == 'hover' or self.mode == 'land': self.pdes = ar([[xx(self.traj_end_time[-1])], [yy(self.traj_end_time[-1])], [zz(self.traj_end_time[-1])]]) else: self.pdes = ar([[xx(self.traj_end_time[-1])], [yy(self.traj_end_time[-1])], [zz(self.traj_end_time[-1])]]) # needed to transform the trajectory back to cog #self.pdes = (np.dot(R.T,self.Rcg_vibase[0:3, 3:4]) + np.dot(self.Rcg_vibase[0:3, 0:3], self.pdes)) self.vdes = ar([[vx(self.traj_end_time[-1])], [vy(self.traj_end_time[-1])], [vz(self.traj_end_time[-1])]]) self.ades = ar([[aax(self.traj_end_time[-1])], [aay(self.traj_end_time[-1])], [aaz(self.traj_end_time[-1])]]) self.jdes = ar([[jx(self.traj_end_time[-1])], [jy(self.traj_end_time[-1])], [jz(self.traj_end_time[-1])]]) self.ddes = self.ddir #self.vdes/np.linalg.norm(self.vdes)#self.ddir if self.mode == 'hover': self.ddes = self.ddir current_time = time.time() - self.start_time if self.counter == 0: self.initial_odom_time = current_time Xd = self.pdes Vd = self.vdes ad = self.ades b1d = self.ddes b1d_dot = ar([[0], [0], [0]]) ad_dot = self.jdes if self.controller == 1: # velocity controller ex = ar([[0], [0], [0]]) else: ex = X - Xd ev = V1 - Vd _b3c = -(mult(self.kx, ex) + mult(self.kv, ev)) / self.m + self.g * self.e[:, 2][ np.newaxis].T + ad # desired direction b3c = ar(_b3c / np.linalg.norm(_b3c)) # get a normalized vector b2c = tp( np.cross(b3c.T, b1d.T) / np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix ff = open('trajectory_subscribed.txt', 'a') ff.write( "%s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s\n" % (index, current_time, t, Xd[0][0], Xd[1][0], Xd[2][0], Vd[0][0], Vd[1][0], Vd[2][0], ad[0][0], ad[1][0], ad[2][0])) ff = open('trajectory_followed.txt', 'a') ff.write("%s, %s, %s, %s, %s, %s\n" % (X[0][0], X[1][0], X[2][0], V1[0][0], V1[1][0], V1[2][0])) phi = np.arctan2(b2c[1][0], b2c[0][0]) #if self.mode == 'hover': # phi = np.arctan2(b2c[1][0], b2c[0][0]) # self.previous_phi = phi #else: # phi = np.arctan2(b2c[1][0], b2c[0][0]) # phi = self.previous_phi + (phi-self.previous_phi)*(t/self.traj_end_time[self.number_of_segments]) if phi < 0: phi = phi + 2 * np.pi elif phi > 2 * np.pi: phi = phi - 2 * np.pi error = 0.5 * np.trace(self.e - np.dot(Rc.T, R)) if self.counter != 0: phi_dot = (phi - self.phi_) / self.dt self.phi_ = phi else: phi_dot = 0 self.phi_ = phi Omega_c = ar([[0], [0], [phi_dot]]) """ if self.counter == 0: acc = tp(np.zeros((1,3))); self.previous_V = V1 b1d_dot = tp(np.zeros((1,3))); self.previous_b1d = b1d else: acc = (V1-self.previous_V)/self.dt; self.previous_V = V1 b1d_dot = (b1d-self.previous_b1d)/self.dt; self.previous_b1d = b1d # calculate derivative of rotation and find desired angular velocity A = (mult(self.kx, ex) + mult(self.kv, ev))/self.m - self.g*self.e[:,2][np.newaxis].T - ad ex_dot = ev; ev_dot = acc-ad A_dot = (mult(self.kx, ex_dot) + mult(self.kv, ev_dot))/self.m - ad_dot # calculate desired direction b3c_dot = -A_dot/np.linalg.norm(A) + (np.dot(A.T,A_dot)/np.linalg.norm(A)**3)*A C = tp(np.cross(b3c.T,b1d.T)) C_dot = tp(np.cross(b3c_dot.T, b1d.T) + np.cross(b3c.T, b1d_dot.T)) b2c_dot = C_dot/np.linalg.norm(C) - (np.dot(C.T,C_dot)/np.linalg.norm(C)**3)*C b1c_dot = tp(np.cross(b2c_dot.T, b3c.T) + np.cross(b2c.T, b3c_dot.T)) Rc_dot = np.column_stack((b1c_dot, b2c_dot, b3c_dot)) # desired rotation matrix derivative #Omega_c_hat = np.dot(tp(Rc),Rc_dot) # skew-symmetric desired angular velocity Omega_c_hat = np.dot(Rc.T, Rc_dot) #Omega_c_hat = ar([[0,0,0], [0,0,0], [0, 0, 0]]) Omega_c = ar([[-Omega_c_hat[1][2]], [Omega_c_hat[0][2]], [-Omega_c_hat[0][1]]]) """ eR_hat = 0.5 * (np.dot(Rc.T, R) - np.dot(R.T, Rc) ) # eR skew symmetric matrix eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation eOmega = Omega - np.dot( np.dot(R.T, Rc), Omega_c) # vector that gives error in angular velocity self.f = self.m * np.dot(_b3c.T, tp(R[:, 2][np.newaxis])) self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp( np.cross(Omega.T, tp(np.dot(self.J, Omega)))) # - np.dot(self.J,Z) ff = open('rotation_error.txt', 'a') ff.write("%s, %s, %s, %s, %s, %s, %s, %s, %s, %s\n" % (phi, b1d[0][0], b1d[1][0], b1d[2][0], -eR_hat[1][2], eR_hat[0][2], -eR_hat[0][1], eOmega[0][0], eOmega[1][0], eOmega[2][0])) T = tp( ar([[ self.M[0][0], self.M[1][0] - 0.286, self.M[2][0], self.f[0][0] ]])) # dummy torque to offset visensor moment #print '---------torques are------------', T Msg = Actuators() if self.uav == 'hummingbird': c1 = tp( ar([[ 0, 2 / (self.d * self.tau_t), 0, -2 / (self.d * self.tau_t) ]])) c2 = tp( ar([[ -2 / (self.d * self.tau_t), 0, 2 / (self.d * self.tau_t), 0 ]])) c3 = tp( ar([[ 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m), 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m) ]])) c4 = tp( ar([[ 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t ]])) C = 0.25 * np.column_stack( (c1, c2, c3, c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(C, T) #w_initial = np.array([[self.w0_h**2], [self.w0_h**2], [self.w0_h**2], [self.w0_h**2]]) w = np.sqrt(np.abs(w_square)) w[w > 800.0] = 800.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] elif self.uav == 'pelican': c1 = tp( ar([[ 0, 2 / (self.d * self.tau_t), 0, -2 / (self.d * self.tau_t) ]])) c2 = tp( ar([[ -2 / (self.d * self.tau_t), 0, 2 / (self.d * self.tau_t), 0 ]])) c3 = tp( ar([[ 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m), 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m) ]])) c4 = tp( ar([[ 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t ]])) C = 0.25 * np.column_stack( (c1, c2, c3, c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(C, T) w = np.sqrt(np.abs(w_square)) w[w > 800.0] = 800.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] else: c1 = tp( ar([[ sin(pi / 6) * self.d * self.tau_t, -cos(pi / 6) * self.d * self.tau_t, -self.tau_t * self.tau_m, self.tau_t ]])) c2 = tp( ar([[ sin(pi / 2) * self.d * self.tau_t, -cos(pi / 2) * self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t ]])) c3 = tp( ar([[ sin(5 * pi / 6) * self.d * self.tau_t, -cos(5 * pi / 6) * self.d * self.tau_t, -self.tau_t * self.tau_m, self.tau_t ]])) c4 = tp( ar([[ sin(7 * pi / 6) * self.d * self.tau_t, -cos(7 * pi / 6) * self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t ]])) c5 = tp( ar([[ sin(3 * pi / 2) * self.d * self.tau_t, -cos(3 * pi / 2) * self.d * self.tau_t, -self.tau_t * self.tau_m, self.tau_t ]])) c6 = tp( ar([[ sin(11 * pi / 6) * self.d * self.tau_t, -cos(11 * pi / 6) * self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t ]])) C = np.column_stack( (c1, c2, c3, c4, c5, c6)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(np.linalg.pinv(C), T) w = np.sqrt(np.abs(w_square)) #w[w>900.0] = 900.0 Msg.angular_velocities = [ w[0][0], w[1][0], w[2][0], w[3][0], w[4][0], w[5][0] ] ff = open('motor_speeds.txt', 'a') ff.write("%s, %s, %s, %s, %s, %s, %s, %s, %s, %s\n" % (w[0][0], w[1][0], w[2][0], w[3][0], w[4][0], w[5][0], self.M[0][0], self.M[1][0] - 0.28, self.M[2][0], self.f[0][0])) #if any(Msg.angular_velocities[i] > 1.5*self.previous_angular_velocities[i] for i in range(len(Msg.angular_velocities))) == False: # print '--------------------this condition is satisfied-----------------------------------' self.pub.publish(Msg) self.previous_angular_velocities = Msg.angular_velocities self.counter += 1
def callback(self, odom, traj): """ X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) #acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z]]) V1 = _V#np.dot(R, _V); # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities """ X = ar([[odom.pose.position.x], [odom.pose.position.y], [odom.pose.position.z]]) q = Quaternion(odom.pose.orientation.w, odom.pose.orientation.x, odom.pose.orientation.y, odom.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.linear.x], [odom.twist.linear.y], [odom.twist.linear.z]]) #acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z]]) V1 = _V #np.dot(R, _V) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.angular.x], [odom.twist.angular.y], [odom.twist.angular.z]]) Omega = np.dot( R.T, Omega ) # this is needed because "vicon" package from Upenn publishes spatial angular velocities """ # next two matrices are position and orientation offset of the vi sensor imu with the center of gravity vi_pos_off_hat = ar([[0.0, 0.03, 0.0], [-0.03, 0.0, -0.1], [0.0, 0.1, 0.0]]) vi_rot_off = ar([[0.9950042, 0.0, 0.0998334], [0.0, 1.0, 0.0], [-0.0998334, 0.0, 0.9950042]]) #vi_rot_off = ar([[0.7071, 0.0, 0.7071], [0.0, 1.0, 0.0], [-0.7071, 0.0, 0.7071]]) intermediate = np.dot(vi_pos_off_hat, vi_rot_off) V1 = np.dot(vi_rot_off,_V) + np.dot(intermediate, Omega) Omega = np.dot(vi_rot_off, Omega) R = np.dot(R, vi_rot_off.T) #Omega_hat = ar([[0,-Omega[2][0], Omega[1][0]], [Omega[2][0], 0, -Omega[0][0]], [-Omega[1][0], Omega[0][0], 0]]) V1 = np.dot(R, V1) #acc = np.dot(R, acc) """ # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction Xd = ar([[traj.desired_position.x], [traj.desired_position.y], [traj.desired_position.z]]) Vd = ar([[traj.desired_velocity.x], [traj.desired_velocity.y], [traj.desired_velocity.z]]) ad = ar([[traj.desired_acceleration.x], [traj.desired_acceleration.y], [traj.desired_acceleration.z]]) #ad_dot = ar([[traj.desired_jerk.x], [traj.desired_jerk.y], [traj.desired_jerk.z]]) b1d = ar([[traj.desired_direction.x], [traj.desired_direction.y], [traj.desired_direction.z]]) #if traj.controller == 0: # phi = 0 #else: # phi = np.arctan2(b1d[1][0], b1d[0][0]) #if phi < 0: # phi = phi + 2 * np.pi #correction = np.array([[0],[0],[0.1]]) if traj.controller == 1: # velocity controller ex = ar([[0], [0], [0]]) else: ex = X - Xd ev = V1 - Vd if self.counter == 0: ei = 0 self.ei = ei else: ei = (ex + ev) * self.dt ei = self.ei + ei self.ei = ei #_b3c = -(mult(self.kx, ex) + mult(self.kv, ev)+ mult(self.ki, ei))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction _b3c = -(mult(self.kx, ex) + mult(self.kv, ev)) / self.m + self.g * self.e[:, 2][ np.newaxis].T + ad # desired direction b3c = ar(_b3c / np.linalg.norm(_b3c)) # get a normalized vector b2c = tp( np.cross(b3c.T, b1d.T) / np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix phi = np.arctan2(b2c[1][0], b2c[0][0]) #print "phi:1", phi if phi < 0: phi = phi + 2 * np.pi #print "phi:2", phi yaw = phi # yaw angle roll = np.arctan(Rc[1][0], Rc[0][0]) * 180.0 / np.pi pitch = np.arctan2(-Rc[2][0] / np.sqrt((Rc[2][1])**2 + (Rc[2][2])**2)) error = 0.5 * np.trace(self.e - np.dot(Rc.T, R)) #print error #current_time = time.time() #t = current_time - self.time #self.time_points.append(t) if self.counter != 0: phi_dot = (phi - self.phi_) / self.dt self.phi_ = phi else: #ad_dot = tp(np.zeros((1,3))); #self._ad = ad phi_dot = 0 self.phi_ = phi Omega_c = ar([[0], [0], [phi_dot]]) eR_hat = 0.5 * (np.dot(Rc.T, R) - np.dot(R.T, Rc) ) # eR skew symmetric matrix eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation eOmega = Omega - np.dot(np.dot( R.T, Rc), Omega_c) # vector that gives error in angular velocity if self.counter == 0: ei_att = 0 self.ei_att = ei_att else: ei_att = (eR + eOmega) * self.dt ei_att = self.ei_att + ei_att self.ei_att = ei_att #Z = np.dot(np.dot(Omega_hat.dot(R.T),Rc),Omega_c) - np.dot(np.dot(R.T,Rc), Omega_c_dot) #Z = np.dot(np.dot(Omega_hat,np.dot(R.T,Rc)),Omega_c) - np.dot(np.dot(R.T,Rc), Omega_c_dot) self.f = self.m * np.dot(_b3c.T, tp(R[:, 2][np.newaxis])) #self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)+mult(self.ki_att, ei_att)) + tp(np.cross(Omega.T, tp(np.dot(self.J,Omega))))# - np.dot(self.J,Z) self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp( np.cross(Omega.T, tp(np.dot(self.J, Omega)))) # - np.dot(self.J,Z) Msg = control_inputs() #Msg.header.stamp = rospy.Time.now() Msg.header.stamp = odom.header.stamp Msg.Total_thrust = self.f[0][0] Msg.Moment_x = self.M[0][0] Msg.Moment_y = self.M[1][ 0] #- 0.145 # moment corresponding to offset vi_sensor mass in y direction, slightly different them 0.13 Msg.Moment_z = self.M[2][0] """ T = tp(ar([[self.M[0][0],self.M[1][0],self.M[2][0],self.f[0][0]]])) c1 = tp(ar([[0, -self.d*self.tau_t, self.tau_t*self.tau_m, self.tau_t]])) c2 = tp(ar([[self.d*self.tau_t, 0, -self.tau_t*self.tau_m, self.tau_t]])) c3 = tp(ar([[0, self.d*self.tau_t, self.tau_t*self.tau_m, self.tau_t]])) c4 = tp(ar([[-self.d*self.tau_t, 0, -self.tau_t*self.tau_m, self.tau_t]])) C = np.column_stack((c1,c2,c3,c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(np.linalg.inv(C),T) w = np.sqrt(np.abs(w_square)) Msg = Actuators() Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] """ self.pub.publish(Msg) self.counter += 1
def callback(self, odom, accel, traj): #a = time.time() X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) V = np.dot(R, _V) _acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z - 9.8]]) acc = np.dot(R, _acc) #print acc # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) #Omega = np.dot(R, _Omega) """np.putmask(Omega, abs(Omega)<1e-9,0);""" Omega_hat = ar([[0, -Omega[2][0], Omega[1][0]], [Omega[2][0], 0, -Omega[0][0]], [-Omega[1][0], Omega[0][0], 0]]) # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction #Xd = ar([[traj.desired_position.x], [traj.desired_position.y], [traj.desired_position.z]]) Vd = ar([[traj.desired_velocity.x], [traj.desired_velocity.y], [traj.desired_velocity.z]]) ad = ar([[traj.desired_acceleration.x], [traj.desired_acceleration.y], [traj.desired_acceleration.z]]) ad_dot = ar([[traj.desired_jerk.x], [traj.desired_jerk.y], [traj.desired_jerk.z]]) b1d = ar([[traj.desired_direction.x], [traj.desired_direction.y], [traj.desired_direction.z]]) #b1d = ar([[1], [1], [0]]) b1d_dot = ar([[0], [0], [0]]) b1d_ddot = ar([[0], [0], [0]]) #correction = np.array([[0],[0],[0.1]]) ex = ar([[0], [0], [0]]) ev = V - Vd _b3c = -(mult(self.kx, ex) + mult(self.kv, ev)) / self.m + self.g * self.e[:, 2][ np.newaxis].T + ad # desired direction b3c = ar(_b3c / np.linalg.norm(_b3c)) # get a normalized vector b2c = tp( np.cross(b3c.T, b1d.T) / np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix """ if self.counter != 0: delt = time.time()-self.previous_time Rc_dot = (Rc-self.Rc_)/delt Rc_ddot = (Rc_dot-self.Rc_dot_)/delt self.Rc_ = Rc; self.Rc_dot_ = Rc_dot self.previous_time = time.time() else: Rc_dot = np.column_stack((tp(np.zeros((1,3))), tp(np.zeros((1,3))), tp(np.zeros((1,3))))); self.Rc_ = Rc Rc_ddot = np.column_stack((tp(np.zeros((1,3))), tp(np.zeros((1,3))), tp(np.zeros((1,3))))); self.Rc_dot_ = Rc_dot self.previous_time = time.time() """ """ current_time = time.time() t = current_time - self.time self.time_points.append(t) if self.counter != 0: delt = (max(self.time_points)-min(self.time_points))/self.counter+1 #self.current_time = time.time() #delt = self.current_time-self.previous_time ad_dot = (ad-self._ad)/delt; ad_ddot = (ad_dot-self._ad_dot)/delt V_ddot = (acc-self.acc_)/delt self._ad = ad; self.acc_ = acc; self._ad_dot = ad_dot self.previous_time = time.time() else: ad_dot = tp(np.zeros((1,3))); self._ad = ad V_ddot = tp(np.zeros((1,3))); self.acc_ = acc; ad_ddot = tp(np.zeros((1,3))); self._ad_dot = ad_dot self.previous_time = time.time() """ current_time = time.time() t = current_time - self.time self.time_points.append(t) if self.counter != 0: delt = (max(self.time_points) - min(self.time_points)) / self.counter + 1 #self.current_time = time.time() #delt = self.current_time-self.previous_time #ad_dot = (ad-self._ad)/delt; ad_ddot = (ad_dot - self._ad_dot) / delt V_ddot = (acc - self.acc_) / delt #self._ad = ad; self.acc_ = acc self._ad_dot = ad_dot self.previous_time = time.time() else: #ad_dot = tp(np.zeros((1,3))); #self._ad = ad V_ddot = tp(np.zeros((1, 3))) self.acc_ = acc ad_ddot = tp(np.zeros((1, 3))) self._ad_dot = ad_dot self.previous_time = time.time() f0 = open('velocity.txt', 'a') f0.write("%s, %s, %s, %s, %s, %s\n" % (V[0][0], V[1][0], V[2][0], Vd[0][0], Vd[1][0], Vd[2][0])) f1 = open('accel.txt', 'a') f1.write( "%s, %s, %s, %s, %s, %s\n" % (acc[0][0], acc[1][0], acc[2][0], ad[0][0], ad[1][0], ad[2][0])) f2 = open('position.txt', 'a') f2.write("%s, %s, %s\n" % (X[0][0], X[1][0], X[2][0])) f3 = open('jerk.txt', 'a') f3.write("%s, %s, %s, %s, %s, %s\n" % (V_ddot[0][0], V_ddot[1][0], V_ddot[2][0], ad_dot[0][0], ad_dot[1][0], ad_dot[2][0])) A = (mult(self.kx, ex) + mult( self.kv, ev)) / self.m - self.g * self.e[:, 2][np.newaxis].T - ad #ex_dot = ar([[0],[0],[0]]) ex_dot = ev ev_dot = acc - ad A_dot = (mult(self.kx, ex_dot) + mult( self.kv, ev_dot)) / self.m - ad_dot # calculate desired direction b3c_dot = -A_dot / np.linalg.norm(A) + (np.dot(A.T, A_dot) / np.linalg.norm(A)**3) * A C = tp(np.cross(b3c.T, b1d.T)) C_dot = tp(np.cross(b3c_dot.T, b1d.T) + np.cross(b3c.T, b1d_dot.T)) b2c_dot = C_dot / np.linalg.norm(C) - (np.dot(C.T, C_dot) / np.linalg.norm(C)**3) * C b1c_dot = tp(np.cross(b2c_dot.T, b3c.T) + np.cross(b2c.T, b3c_dot.T)) Rc_dot = np.column_stack( (b1c_dot, b2c_dot, b3c_dot)) # desired rotation matrix derivative #Omega_c_hat = np.dot(tp(Rc),Rc_dot) # skew-symmetric desired angular velocity Omega_c_hat = np.dot(Rc.T, Rc_dot) #Omega_c_hat = ar([[0,0,0], [0,0,0], [0, 0, 0]]) Omega_c = ar([[-Omega_c_hat[1][2]], [Omega_c_hat[0][2]], [-Omega_c_hat[0][1]]]) # ex_ddot = ar([[0],[0],[0]]) ex_ddot = ev_dot ev_ddot = V_ddot - ad_dot A_ddot = (mult(self.kx, ex_ddot) + mult(self.kv, ev_ddot)) / self.m - ad_ddot b3c_ddot = -A_ddot/np.linalg.norm(A) + 2*(np.dot(A.T,A_dot)/np.linalg.norm(A)**3)*A_dot +\ (np.linalg.norm(A_dot)**2+np.dot(A.T,A_ddot))*A/np.linalg.norm(A)**3 - 3*(np.dot(A.T,A_dot)**2/np.linalg.norm(A)**5)*A C_ddot = tp( np.cross(b3c_ddot.T, b1d.T) + 2 * np.cross(b3c_dot.T, b1d_dot.T) + np.cross(b3c.T, b1d_ddot.T)) b2c_ddot = C_ddot/np.linalg.norm(C) - 2*(np.dot(C.T,C_dot)/np.linalg.norm(C)**3)*C_dot -\ (np.linalg.norm(C_dot)**2+np.dot(C.T,C_ddot))*C/np.linalg.norm(C)**3 + 3*(np.dot(C.T,C_dot)**2/np.linalg.norm(C)**5)*C b1c_ddot = tp( np.cross(b2c_ddot.T, b3c.T) + 2 * np.cross(b2c_dot.T, b3c_dot.T) + np.cross(b2c.T, b3c_ddot.T)) Rc_ddot = np.column_stack( (b1c_ddot, b2c_ddot, b3c_ddot)) # desired rotation matrix derivative #Omega_c_dot_hat = np.dot(Rc.T,Rc_ddot) + np.dot(Omega_c_hat.T,Omega_c_hat) Omega_c_dot_hat = np.dot(Rc.T, Rc_ddot) - np.linalg.matrix_power( Omega_c_hat, 2) #Omega_c_dot_hat = ar([[0,0,0], [0,0,0], [0, 0, 0]]) Omega_c_dot = ar([[-Omega_c_dot_hat[1][2]], [Omega_c_dot_hat[0][2]], [-Omega_c_dot_hat[0][1]]]) eR_hat = 0.5 * (np.dot(Rc.T, R) - np.dot(R.T, Rc) ) # eR skew symmetric matrix eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation eOmega = Omega - np.dot(np.dot( R.T, Rc), Omega_c) # vector that gives error in angular velocity #Z = np.dot(np.dot(Omega_hat.dot(R.T),Rc),Omega_c) - np.dot(np.dot(R.T,Rc), Omega_c_dot) Z = np.dot(np.dot(Omega_hat, np.dot(R.T, Rc)), Omega_c) - np.dot( np.dot(R.T, Rc), Omega_c_dot) self.f = self.m * np.dot(_b3c.T, tp(R[:, 2][np.newaxis])) self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp( np.cross(Omega.T, tp(np.dot(self.J, Omega)))) #- np.dot(self.J,Z) Msg = control_inputs() Msg.header.stamp = rospy.Time.now() Msg.Total_thrust = self.f[0][0] Msg.Moment_x = self.M[0][0] Msg.Moment_y = self.M[1][0] Msg.Moment_z = self.M[2][0] """ T = tp(ar([[self.M[0][0],self.M[1][0],self.M[2][0],self.f[0][0]]])) c1 = tp(ar([[0, -self.d*self.tau_t, self.tau_t*self.tau_m, self.tau_t]])) c2 = tp(ar([[self.d*self.tau_t, 0, -self.tau_t*self.tau_m, self.tau_t]])) c3 = tp(ar([[0, self.d*self.tau_t, self.tau_t*self.tau_m, self.tau_t]])) c4 = tp(ar([[-self.d*self.tau_t, 0, -self.tau_t*self.tau_m, self.tau_t]])) C = np.column_stack((c1,c2,c3,c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(np.linalg.inv(C),T) w = np.sqrt(np.abs(w_square)) Msg = Actuators() Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] """ self.pub.publish(Msg) self.counter += 1
def callback(self, odom, traj): print 'i m here' X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) #acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z]]) #V1 = _V#np.dot(R, _V); V1 = np.dot(R, _V) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) #Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities """ X = ar([[odom.pose.position.x], [odom.pose.position.y], [odom.pose.position.z]]) q = Quaternion(odom.pose.orientation.w, odom.pose.orientation.x, odom.pose.orientation.y, odom.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.linear.x], [odom.twist.linear.y], [odom.twist.linear.z]]) #acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z]]) V1 = _V#np.dot(R, _V) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.angular.x], [odom.twist.angular.y], [odom.twist.angular.z]]) Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities """ """ # next two matrices are position and orientation offset of the vi sensor imu with the center of gravity vi_pos_off_hat = ar([[0.0, 0.03, 0.0], [-0.03, 0.0, -0.1], [0.0, 0.1, 0.0]]) vi_rot_off = ar([[0.9950042, 0.0, 0.0998334], [0.0, 1.0, 0.0], [-0.0998334, 0.0, 0.9950042]]) #vi_rot_off = ar([[0.7071, 0.0, 0.7071], [0.0, 1.0, 0.0], [-0.7071, 0.0, 0.7071]]) intermediate = np.dot(vi_pos_off_hat, vi_rot_off) V1 = np.dot(vi_rot_off,_V) + np.dot(intermediate, Omega) Omega = np.dot(vi_rot_off, Omega) R = np.dot(R, vi_rot_off.T) #Omega_hat = ar([[0,-Omega[2][0], Omega[1][0]], [Omega[2][0], 0, -Omega[0][0]], [-Omega[1][0], Omega[0][0], 0]]) V1 = np.dot(R, V1) #acc = np.dot(R, acc) """ # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction Xd = ar([[traj.pdes.x], [traj.pdes.y], [traj.pdes.z]]) Vd = ar([[traj.vdes.x], [traj.vdes.y], [traj.vdes.z]]) ad = ar([[traj.ades.x], [traj.ades.y], [traj.ades.z]]) #ad_dot = ar([[traj.desired_jerk.x], [traj.desired_jerk.y], [traj.desired_jerk.z]]) b1d = ar([[traj.ddes.x], [traj.ddes.y], [traj.ddes.z]]) if traj.controller == 1: # velocity controller ex = ar([[0], [0], [0]]) else: ex = X - Xd ev = V1 - Vd #if self.counter==0: # ei = 0; self.ei = ei #else: # ei = (ex+ev)*self.dt # ei = self.ei+ei # self.ei = ei #_b3c = -(mult(self.kx, ex) + mult(self.kv, ev)+ mult(self.ki, ei))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction _b3c = -(mult(self.kx, ex) + mult(self.kv, ev)) / self.m + self.g * self.e[:, 2][ np.newaxis].T + ad # desired direction b3c = ar(_b3c / np.linalg.norm(_b3c)) # get a normalized vector b2c = tp( np.cross(b3c.T, b1d.T) / np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix ff = open('b2c.txt', 'a') ff.write("%s, %s, %s, %s\n" % (b2c[0][0], b2c[1][0], b2c[2][0], np.linalg.norm(np.cross(b3c.T, b1d.T)))) phi = np.arctan2(b2c[1][0], b2c[0][0]) #phi = np.arctan2(b1d[1][0], b1d[0][0]) if phi < 0: phi = phi + 2 * np.pi error = 0.5 * np.trace(self.e - np.dot(Rc.T, R)) if self.counter != 0: phi_dot = (phi - self.phi_) / self.dt self.phi_ = phi else: phi_dot = 0 self.phi_ = phi f0 = open('phi_phidot.txt', 'a') f0.write("%s, %s\n" % (phi, phi_dot)) #f2 = open('positions.txt', 'a') #f2.write("%s, %s, %s, %s, %s, %s\n" % (X[0][0], X[1][0], X[2][0], Xd[0][0], Xd[1][0], Xd[2][0])) Omega_c = ar([[0], [0], [phi_dot]]) eR_hat = 0.5 * (np.dot(Rc.T, R) - np.dot(R.T, Rc) ) # eR skew symmetric matrix eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation eOmega = Omega - np.dot(np.dot( R.T, Rc), Omega_c) # vector that gives error in angular velocity #if self.counter==0: # ei_att = 0; self.ei_att = ei_att #else: # ei_att = (eR+eOmega)*self.dt # ei_att = self.ei_att+ei_att # self.ei_att = ei_att #Z = np.dot(np.dot(Omega_hat.dot(R.T),Rc),Omega_c) - np.dot(np.dot(R.T,Rc), Omega_c_dot) #Z = np.dot(np.dot(Omega_hat,np.dot(R.T,Rc)),Omega_c) - np.dot(np.dot(R.T,Rc), Omega_c_dot) self.f = self.m * np.dot(_b3c.T, tp(R[:, 2][np.newaxis])) #self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)+mult(self.ki_att, ei_att)) + tp(np.cross(Omega.T, tp(np.dot(self.J,Omega))))# - np.dot(self.J,Z) self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp( np.cross(Omega.T, tp(np.dot(self.J, Omega)))) # - np.dot(self.J,Z) T = tp(ar([[self.M[0][0], self.M[1][0], self.M[2][0], self.f[0][0]]])) f1 = open('forces.txt', 'a') f1.write("%s, %s, %s, %s\n" % (T[0][0], T[1][0], T[2][0], T[3][0])) #rospy.loginfo('moments and thrust is:%f, %f, %f, %f', T[0][0], T[1][0], T[2][0], T[3][0] ) Msg = Actuators() if self.uav == 'hummingbird': c1 = tp( ar([[ 0, 2 / (self.d * self.tau_t), 0, -2 / (self.d * self.tau_t) ]])) c2 = tp( ar([[ -2 / (self.d * self.tau_t), 0, 2 / (self.d * self.tau_t), 0 ]])) c3 = tp( ar([[ 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m), 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m) ]])) c4 = tp( ar([[ 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t ]])) C = 0.25 * np.column_stack( (c1, c2, c3, c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(C, T) #w_initial = np.array([[self.w0_h**2], [self.w0_h**2], [self.w0_h**2], [self.w0_h**2]]) w = np.sqrt(np.abs(w_square)) w[w > 800.0] = 800.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] elif self.uav == 'pelican': c1 = tp( ar([[ 0, 2 / (self.d * self.tau_t), 0, -2 / (self.d * self.tau_t) ]])) c2 = tp( ar([[ -2 / (self.d * self.tau_t), 0, 2 / (self.d * self.tau_t), 0 ]])) c3 = tp( ar([[ 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m), 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m) ]])) c4 = tp( ar([[ 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t ]])) C = 0.25 * np.column_stack( (c1, c2, c3, c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(C, T) #w_initial = np.array([[self.w0_p**2], [self.w0_p**2], [self.w0_p**2], [self.w0_p**2]]) #w = np.sqrt(np.abs(w_square+w_initial)) w = np.sqrt(np.abs(w_square)) w[w > 800.0] = 800.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] else: c1 = tp( ar([[ sin(pi / 6) * self.d * self.tau_t, -cos(pi / 6) * self.d * self.tau_t, -self.tau_t * self.tau_m, self.tau_t ]])) c2 = tp( ar([[ sin(pi / 2) * self.d * self.tau_t, -cos(pi / 2) * self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t ]])) c3 = tp( ar([[ sin(5 * pi / 6) * self.d * self.tau_t, -cos(5 * pi / 6) * self.d * self.tau_t, -self.tau_t * self.tau_m, self.tau_t ]])) c4 = tp( ar([[ sin(7 * pi / 6) * self.d * self.tau_t, -cos(7 * pi / 6) * self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t ]])) c5 = tp( ar([[ sin(3 * pi / 2) * self.d * self.tau_t, -cos(3 * pi / 2) * self.d * self.tau_t, -self.tau_t * self.tau_m, self.tau_t ]])) c6 = tp( ar([[ sin(11 * pi / 6) * self.d * self.tau_t, -cos(11 * pi / 6) * self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t ]])) C = np.column_stack((c1, c2, c3, c4, c5, c6)) # solving linear eq T = Cw^2 to get w^2 #inverted_matrix = np.dot(C.T, np.linalg.inv(np.dot(C, C.T))) #w_square = np.dot(inverted_matrix,T) w_square = np.dot(np.linalg.pinv(C), T) #w_initial = np.array([[self.w0_f**2], [self.w0_f**2], [self.w0_f**2], [self.w0_f**2], [self.w0_f**2], [self.w0_f**2]]) #w = np.sqrt(np.abs(w_square+w_initial)) w = np.sqrt(np.abs(w_square)) #w[w>900.0] = 900.0 Msg.angular_velocities = [ w[0][0], w[1][0], w[2][0], w[3][0], w[4][0], w[5][0] ] self.pub.publish(Msg) self.counter += 1
def odom_callback(self, odom): """does the control calculations""" self.odom_time = odom.header.stamp.to_sec() t = float(self.odom_time - self.traj_start_time) if t < self.traj_end_time[self.number_of_segments]: index = bisect.bisect(self.traj_end_time, t) - 1 else: index = bisect.bisect(self.traj_end_time, t) - 2 fff = open('traj_time_in_odom.txt', 'a') fff.write('%s, %s, %s, %s, %s\n' % (index, self.odom_time, self.traj_start_time, t, self.traj_end_time[self.number_of_segments])) if index == -1: pass else: xx = np.poly1d(self.p1[index]) vx = np.polyder(xx, 1) aax = np.polyder(xx, 2) jx = np.polyder(xx, 3) sx = np.polyder(xx, 4) yy = np.poly1d(self.p2[index]) vy = np.polyder(yy, 1) aay = np.polyder(yy, 2) jy = np.polyder(yy, 3) sy = np.polyder(yy, 4) zz = np.poly1d(self.p3[index]) vz = np.polyder(zz, 1) aaz = np.polyder(zz, 2) jz = np.polyder(zz, 3) sz = np.polyder(zz, 4) if t < self.traj_end_time[self.number_of_segments]: self.pdes = ar([[xx(t)], [yy(t)], [zz(t)]]) self.vdes = ar([[vx(t)], [vy(t)], [vz(t)]]) self.ades = ar([[aax(t)], [aay(t)], [aaz(t)]]) self.ddes = ar([[1], [0], [0]]) else: self.pdes = ar([[xx(self.traj_end_time[-1])], [yy(self.traj_end_time[-1])], [zz(self.traj_end_time[-1])]]) self.vdes = ar([[vx(self.traj_end_time[-1])], [vy(self.traj_end_time[-1])], [vz(self.traj_end_time[-1])]]) self.ades = ar([[aax(self.traj_end_time[-1])], [aay(self.traj_end_time[-1])], [aaz(self.traj_end_time[-1])]]) self.ddes = ar([[1], [0], [0]]) current_time = time.time() - self.start_time if self.counter == 0: self.initial_odom_time = current_time X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix _V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) #acc = ar([[accel.linear_acceleration.x], [accel.linear_acceleration.y], [accel.linear_acceleration.z]]) #V1 = _V#np.dot(R, _V); V1 = np.dot(R, _V) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) #Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities # Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction #index, value = min(enumerate(self.trajectory_time), key=lambda x: abs(x[1]-current_time)) #Xd = self.despos[index]; Vd = self.desvel[index]; ad = self.desacc[index]; b1d = self.desdirection[index] Xd = self.pdes Vd = self.vdes ad = self.ades b1d = self.ddes if self.controller == 1: # velocity controller ex = ar([[0], [0], [0]]) else: ex = X - Xd ev = V1 - Vd _b3c = -(mult(self.kx, ex) + mult(self.kv, ev)) / self.m + self.g * self.e[:, 2][ np.newaxis].T + ad # desired direction b3c = ar(_b3c / np.linalg.norm(_b3c)) # get a normalized vector b2c = tp( np.cross(b3c.T, b1d.T) / np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix ff = open('trajectory_subscribed.txt', 'a') ff.write("%s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s\n" % (index, self.odom_time, self.traj_start_time, Xd[0][0], Xd[1][0], Xd[2][0], Vd[0][0], Vd[1][0], Vd[2][0], ad[0][0], ad[1][0], ad[2][0])) ff = open('trajectory_followed.txt', 'a') ff.write("%s, %s, %s, %s, %s, %s\n" % (X[0][0], X[1][0], X[2][0], V1[0][0], V1[1][0], V1[2][0])) phi = np.arctan2(b2c[1][0], b2c[0][0]) if phi < 0: phi = phi + 2 * np.pi error = 0.5 * np.trace(self.e - np.dot(Rc.T, R)) if self.counter != 0: phi_dot = (phi - self.phi_) / self.dt self.phi_ = phi else: phi_dot = 0 self.phi_ = phi Omega_c = ar([[0], [0], [phi_dot]]) eR_hat = 0.5 * (np.dot(Rc.T, R) - np.dot(R.T, Rc) ) # eR skew symmetric matrix eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation eOmega = Omega - np.dot( np.dot(R.T, Rc), Omega_c) # vector that gives error in angular velocity self.f = self.m * np.dot(_b3c.T, tp(R[:, 2][np.newaxis])) self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp( np.cross(Omega.T, tp(np.dot(self.J, Omega)))) # - np.dot(self.J,Z) T = tp( ar([[self.M[0][0], self.M[1][0], self.M[2][0], self.f[0][0]]])) # dummy torque to offset visensor moment Msg = Actuators() if self.uav == 'hummingbird': c1 = tp( ar([[ 0, 2 / (self.d * self.tau_t), 0, -2 / (self.d * self.tau_t) ]])) c2 = tp( ar([[ -2 / (self.d * self.tau_t), 0, 2 / (self.d * self.tau_t), 0 ]])) c3 = tp( ar([[ 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m), 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m) ]])) c4 = tp( ar([[ 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t ]])) C = 0.25 * np.column_stack( (c1, c2, c3, c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(C, T) #w_initial = np.array([[self.w0_h**2], [self.w0_h**2], [self.w0_h**2], [self.w0_h**2]]) w = np.sqrt(np.abs(w_square)) w[w > 800.0] = 800.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] elif self.uav == 'pelican': c1 = tp( ar([[ 0, 2 / (self.d * self.tau_t), 0, -2 / (self.d * self.tau_t) ]])) c2 = tp( ar([[ -2 / (self.d * self.tau_t), 0, 2 / (self.d * self.tau_t), 0 ]])) c3 = tp( ar([[ 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m), 1 / (self.tau_t * self.m), -1 / (self.tau_t * self.m) ]])) c4 = tp( ar([[ 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t, 1 / self.tau_t ]])) C = 0.25 * np.column_stack( (c1, c2, c3, c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(C, T) w = np.sqrt(np.abs(w_square)) w[w > 800.0] = 800.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] else: c1 = tp( ar([[ sin(pi / 6) * self.d * self.tau_t, -cos(pi / 6) * self.d * self.tau_t, -self.tau_t * self.tau_m, self.tau_t ]])) c2 = tp( ar([[ sin(pi / 2) * self.d * self.tau_t, -cos(pi / 2) * self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t ]])) c3 = tp( ar([[ sin(5 * pi / 6) * self.d * self.tau_t, -cos(5 * pi / 6) * self.d * self.tau_t, -self.tau_t * self.tau_m, self.tau_t ]])) c4 = tp( ar([[ sin(7 * pi / 6) * self.d * self.tau_t, -cos(7 * pi / 6) * self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t ]])) c5 = tp( ar([[ sin(3 * pi / 2) * self.d * self.tau_t, -cos(3 * pi / 2) * self.d * self.tau_t, -self.tau_t * self.tau_m, self.tau_t ]])) c6 = tp( ar([[ sin(11 * pi / 6) * self.d * self.tau_t, -cos(11 * pi / 6) * self.d * self.tau_t, self.tau_t * self.tau_m, self.tau_t ]])) C = np.column_stack( (c1, c2, c3, c4, c5, c6)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(np.linalg.pinv(C), T) w = np.sqrt(np.abs(w_square)) #w[w>900.0] = 900.0 Msg.angular_velocities = [ w[0][0], w[1][0], w[2][0], w[3][0], w[4][0], w[5][0] ] self.pub.publish(Msg) self.counter += 1
def odom_callback(self, odom): """send command to px4""" self.odom_time = odom.header.stamp.to_sec() t = float(self.odom_time - self.traj_start_time) if t <= self.traj_end_time[self.number_of_segments]: index = bisect.bisect(self.traj_end_time, t)-1 else: index = bisect.bisect(self.traj_end_time, t)-2 #fff = open('traj_time_in_odom.txt', 'a') #fff.write('%s, %s, %s, %s, %s\n' %(index, self.odom_time, self.traj_start_time, t, self.traj_end_time[self.number_of_segments])) if index == -1: pass else: xx = np.poly1d(self.p1[index]) vx = np.polyder(xx, 1); aax = np.polyder(xx, 2) jx = np.polyder(xx, 3) yy = np.poly1d(self.p2[index]) vy = np.polyder(yy, 1); aay = np.polyder(yy, 2) jy = np.polyder(yy, 3) zz = np.poly1d(self.p3[index]) vz = np.polyder(zz, 1); aaz = np.polyder(zz, 2) jz = np.polyder(zz, 3) if t <= self.traj_end_time[self.number_of_segments]: #print ar([[xx(t)], [yy(t)], [zz(t)]]) self.pdes = ar([[xx(t)], [yy(t)], [zz(t)]]) self.vdes = ar([[vx(t)], [vy(t)], [vz(t)]]) self.ades = ar([[aax(t)], [aay(t)], [aaz(t)]]) self.jdes = ar([[jx(t)], [jy(t)], [jz(t)]]) self.ddes = self.ddir#d/np.linalg.norm(d) else: self.pdes = ar([[xx(self.traj_end_time[-1])], [yy(self.traj_end_time[-1])], [zz(self.traj_end_time[-1])]]) self.vdes = ar([[vx(self.traj_end_time[-1])], [vy(self.traj_end_time[-1])], [vz(self.traj_end_time[-1])]]) self.ades = ar([[aax(self.traj_end_time[-1])], [aay(self.traj_end_time[-1])], [aaz(self.traj_end_time[-1])]]) self.jdes = ar([[jx(self.traj_end_time[-1])], [jy(self.traj_end_time[-1])], [jz(self.traj_end_time[-1])]]) self.ddes = self.ddir if self.mode == 'hover': self.ddes = self.ddir current_time = time.time()-self.start_time if self.counter == 0: self.initial_odom_time = current_time X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]]) q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\ odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) R = q.rotation_matrix # ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]]) # CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW? Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]]) Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities Xd = self.pdes; Vd = self.vdes; ad = self.ades; b1d = self.ddes b1d_dot = ar([[0],[0],[0]]); ad_dot = self.jdes if self.controller == 1: # velocity controller ex = ar([[0],[0],[0]]) else: ex = X-Xd ev = V-Vd #_b3c = -(mult(self.kx, ex) + mult(self.kv, ev)+ mult(self.ki, ei))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction _b3c = -(mult(self.kx, ex) + mult(self.kv, ev))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction b3c = ar(_b3c/np.linalg.norm(_b3c)) # get a normalized vector b2c = tp(np.cross(b3c.T,b1d.T)/np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d b1c = tp(np.cross(b2c.T, b3c.T)) Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix #qc = self.rotmat2quat(Rc) #qc = rowan.to_matrix(Rc) qc = self.rotmat2quat(Rc) #print 'qc', qc omega_des = q.inverse*qc self.f = self.m*np.dot(_b3c.T, tp(R[:,2][np.newaxis]))/self.norm_thrust_constant # normalized thrust #self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp(np.cross(Omega.T, tp(np.dot(self.J,Omega)))) msg = AttitudeTarget() msg.header.stamp = rospy.Time.now()#odom.header.stamp msg.type_mask = 128 msg.body_rate.x = self.factor*np.sign(omega_des[0])*omega_des[1] msg.body_rate.y = self.factor*np.sign(omega_des[0])*omega_des[2] msg.body_rate.z = self.factor*np.sign(omega_des[0])*omega_des[3] msg.thrust = min(1.0, self.f[0][0]) print 'thrust:', self.f*self.norm_thrust_constant, 'thrust_factor:', min(1.0, self.f[0][0]) print 'omega_des',msg.body_rate.x, msg.body_rate.y, msg.body_rate.z print 'zheight', Xd[0][2] self.counter = self.counter+1 self.pub.publish(msg)
#Basic Python Varibales & Operators from math import sqrt as squareroot from numpy import multiply as mult print(5 + 2, ' Float div : ', 7 / 3, ' Round off quotient ', 7 // 3) wordarray = ['t', 'h', 'i', 's', ' ', 'i', 's', ' ', 't', 'e', 's', 't'] word = 'this is test' print('slice : --> ' + word[:4], ' give start -->', word[2:], 'Start end -->', word[2:5], 'Last 4 -> ', word[-4:]) print(' length : ', len(word)) print(wordarray[:5]) print(squareroot(5)) print(mult(2, 3)) print('---- Fibinacci ----') #print fibonacci limit = 10 start = 1 start, b = 0, 1 while start < limit: print(start) start, b = b, start + b