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
