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, 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]])
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 = X - Xd - correction
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
#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
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()
#f4= open('times.txt', 'a')
#f4.write("%s,%s,%s\n" % (self.current_time, self.previous_time,delt))
else:
#self.current_time = time.time()
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()
"""
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)) / 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)) / 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
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 = -(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.03)
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