scorpio1
/
oscillation_ctrl
forked from cesar.alejandro/oscillation_ctrl
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Fixed issues/bugs from branch merge

This commit is contained in:
cesar 2022-04-18 17:24:08 -03:00
parent 309a33c08d
commit 71f7574383
4 changed files with 55 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
launch/klausen_dampen.launch
View File

@ -42,7 +42,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
type="klausen_control.py" type="klausen_control.py"
name="klausenCtrl_node" name="klausenCtrl_node"
output="screen" output="screen"
/> />
<node <node
pkg="oscillation_ctrl" pkg="oscillation_ctrl"

16
src/klausen_control.py
View File

@ -55,10 +55,10 @@ class Main:
self.has_run = 0 # Bool to keep track of first run instance self.has_run = 0 # Bool to keep track of first run instance
# Col1 = theta, theta dot; Col2 = phi, phidot for self.PHI # Col1 = theta, theta dot; Col2 = phi, phidot for self.PHI
self.PHI = np.array([[0,0],[0,0]]) self.PHI = np.array([[0,0],[0,0]])
self.dr_vel = np.zeros([3,1]) self.dr_vel = np.zeros([3,1])
self.dr_angVel = np.zeros([3,1]) self.dr_angVel = np.zeros([3,1])
self.dr_acc = np.zeros([3,1]) #self.imu_data.linear_acceleration self.dr_acc = np.zeros([3,1]) #self.imu_data.linear_acceleration
# Tether var - Check if current method is used # Tether var - Check if current method is used
# Get tether length # Get tether length
@ -74,8 +74,8 @@ class Main:
break break
# Tuning gains # Tuning gains
self.K1 = np.identity(3) self.K1 = np.identity(3)
self.K2 = np.identity(5) self.K2 = np.identity(5)
# Values which require updates. *_p = prior # Values which require updates. *_p = prior
self.z1_p = np.zeros([3,1]) self.z1_p = np.zeros([3,1])
@ -258,8 +258,8 @@ class Main:
self.a45_buff = odeint(self.statespace,self.a45_0,self.t,args=(Ka,Kb,M_aI)) self.a45_buff = odeint(self.statespace,self.a45_0,self.t,args=(Ka,Kb,M_aI))
# Update a45_0 to new a45. Need to transpose to column vector # Update a45_0 to new a45. Need to transpose to column vector
self.a45_0 = self.a45_buff[1,:] self.a45_0 = self.a45_buff[1,:]
self.a45 = np.array([[self.a45_0[0]],[self.a45_0[1]]]) self.a45 = np.array([[self.a45_0[0]],[self.a45_0[1]]])
# Get alphadot_4:5 # Get alphadot_4:5
self.a45dot = self.statespace(self.a45_0,1,Ka,Kb,M_aI) # Do not need 't' and that's why it is a 1 self.a45dot = self.statespace(self.a45_0,1,Ka,Kb,M_aI) # Do not need 't' and that's why it is a 1
@ -316,7 +316,7 @@ class Main:
def publisher(self,pub_time): def publisher(self,pub_time):
self.control() self.control()
self.pub_q.publish(self.quaternion) self.pub_q.publish(self.quaternion)
# rospy.loginfo('roll: %.2f\npitch: %.2f\n',self.EulerAng[0],self.EulerAng[1]) #rospy.loginfo('roll: %.2f\npitch: %.2f\n',self.EulerAng[0],self.EulerAng[1])
# --------------------------------------------------------------------------------# # --------------------------------------------------------------------------------#
# ALGORITHM # ALGORITHM

62
src/path_follow.cpp
View File

@ -120,10 +120,10 @@ int main(int argc, char **argv)
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool> ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming"); ("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode> ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode"); ("mavros/set_mode");
ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL> ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
("mavros/cmd/takeoff"); ("mavros/cmd/takeoff");
ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack> ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
("status/waypoint_tracker"); ("status/waypoint_tracker");
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
@ -132,31 +132,39 @@ int main(int argc, char **argv)
// wait for FCU connection // wait for FCU connection
while(ros::ok() && !current_state.connected){ while(ros::ok() && !current_state.connected){
ros::spinOnce(); ros::spinOnce();
ROS_INFO("Waiting for FCU connection"); ROS_INFO("Waiting for FCU connection");
rate.sleep(); rate.sleep();
} }
if (current_state.connected){
ROS_INFO("Connected to FCU");
} else {
ROS_INFO("Never Connected");
}
/*********** Initiate variables ************************/ /*********** Initiate variables ************************/
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
ros::Rate rate_pub(75.0);
// Needed for attitude command // Needed for attitude command
mavros_msgs::AttitudeTarget attitude; mavros_msgs::AttitudeTarget attitude;
attitude.header.frame_id = "map"; // NED Ref Frame(?) #TODO attitude.header.frame_id = "map"; // NED Ref Frame(?) #TODO
attitude.type_mask = 1|2|4; // Ignore body rates attitude.type_mask = 1|2|4; // Ignore body rates
// Retrieve desired waypoints // Retrieve desired waypoints
oscillation_ctrl::WaypointTrack wpoint_srv; oscillation_ctrl::WaypointTrack wpoint_srv;
wpoint_srv.request.query = false; wpoint_srv.request.query = false;
if (waypoint_cl.call(wpoint_srv)) if (waypoint_cl.call(wpoint_srv))
{ {
ROS_INFO("Waypoints received"); ROS_INFO("Waypoints received");
} }
// populate desired waypoints - this is only for original hover as // populate desired waypoints - this is only for original hover as
// a change of waypoints is handled by ref_signal.py // a change of waypoints is handled by ref_signal.py
buff_pose.pose.position.x = wpoint_srv.response.xd.x; buff_pose.pose.position.x = wpoint_srv.response.xd.x;
buff_pose.pose.position.y = wpoint_srv.response.xd.y; buff_pose.pose.position.y = wpoint_srv.response.xd.y;
buff_pose.pose.position.z = wpoint_srv.response.xd.z; buff_pose.pose.position.z = wpoint_srv.response.xd.z;
double alt_des = buff_pose.pose.position.z; // Desired height double alt_des = buff_pose.pose.position.z; // Desired height
// Desired mode is offboard // Desired mode is offboard
mavros_msgs::SetMode offb_set_mode; mavros_msgs::SetMode offb_set_mode;
@ -177,6 +185,7 @@ int main(int argc, char **argv)
for(int i = 100; ros::ok() && i > 0; --i){ for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(buff_pose); local_pos_pub.publish(buff_pose);
ros::spinOnce(); ros::spinOnce();
ROS_INFO("Publishing position setpoints");
rate.sleep(); rate.sleep();
} }
@ -199,21 +208,16 @@ int main(int argc, char **argv)
eulAng.commanded = K_ang; eulAng.commanded = K_ang;
eulAng.desired = J_ang; eulAng.desired = J_ang;
if(current_state.mode != "OFFBOARD" && if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
(ros::Time::now() - last_request > ros::Duration(5.0))){ if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
if( set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO("Offboard enabled");
} else { } else {
ROS_INFO("Could not enter offboard mode"); ROS_INFO("Could not enter offboard mode");
} }
last_request = ros::Time::now(); //last_request = ros::Time::now();
} else { } else {
if( !current_state.armed && if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){
(ros::Time::now() - last_request > ros::Duration(3.0))){ if( arming_client.call(arm_cmd) && arm_cmd.response.success){
if( arming_client.call(arm_cmd) && ROS_INFO("Vehicle armed");
arm_cmd.response.success){
ROS_INFO("Vehicle armed");
} }
last_request = ros::Time::now(); last_request = ros::Time::now();
} }
@ -229,17 +233,17 @@ int main(int argc, char **argv)
attitude.thrust = thrust.thrust; attitude.thrust = thrust.thrust;
// Determine which messages to send // Determine which messages to send
if(ros::Time::now() - tkoff_req > ros::Duration(22.0) && takeoff){ if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){
attitude.orientation = q_des; attitude.orientation = q_des;
attitude.header.stamp = ros::Time::now(); attitude.header.stamp = ros::Time::now();
att_targ.publish(attitude); att_targ.publish(attitude);
att_cmds = true; att_cmds = true;
ROS_INFO("Attitude Ctrl"); ROS_INFO("ATTITUDE CTRL");
ROS_INFO("Des Altitude: %.2f", alt_des); ROS_INFO("Des Altitude: %.2f", alt_des);
ROS_INFO("Cur Altitude: %.2f", current_altitude); ROS_INFO("Cur Altitude: %.2f", current_altitude);
} else { } else {
local_pos_pub.publish(buff_pose); local_pos_pub.publish(buff_pose);
ROS_INFO("Position Ctrl"); ROS_INFO("POSITION CTRL");
ROS_INFO("Des Altitude: %.2f", alt_des); ROS_INFO("Des Altitude: %.2f", alt_des);
ROS_INFO("Cur Altitude: %.2f", current_altitude); ROS_INFO("Cur Altitude: %.2f", current_altitude);
} }
@ -248,7 +252,7 @@ int main(int argc, char **argv)
euler_pub.publish(eulAng); euler_pub.publish(eulAng);
ros::spinOnce(); ros::spinOnce();
rate.sleep(); rate_pub.sleep();
} }
} }

24
src/ref_signalGen.py
View File

@ -324,12 +324,12 @@ class Main:
# SOLVE ODE (get ref pos, vel, accel) # SOLVE ODE (get ref pos, vel, accel)
self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,)) self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,))
self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd.y,)) self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd.y,))
self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd.z,)) #self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd.z,))
for i in range(1,len(self.y0)): for i in range(1,len(self.y0)):
self.x[:,i] = np.clip(self.x[:,i], a_min = -self.max[i], a_max = self.max[i]) self.x[:,i] = np.clip(self.x[:,i], a_min = -self.max[i], a_max = self.max[i])
self.y[:,i] = np.clip(self.y[:,i], a_min = -self.max[i], a_max = self.max[i]) self.y[:,i] = np.clip(self.y[:,i], a_min = -self.max[i], a_max = self.max[i])
self.z[:,i] = np.clip(self.z[:,i], a_min = -self.max[i], a_max = self.max[i]) #self.z[:,i] = np.clip(self.z[:,i], a_min = -self.max[i], a_max = self.max[i])
for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z
@ -338,11 +338,11 @@ class Main:
if self.SF_idx < len(self.SF_delay_x[0]): if self.SF_idx < len(self.SF_delay_x[0]):
self.EPS_I[3*j] = self.x[1,j] self.EPS_I[3*j] = self.x[1,j]
self.EPS_I[3*j+1] = self.y[1,j] self.EPS_I[3*j+1] = self.y[1,j]
self.EPS_I[3*j+2] = self.z[1,j] #self.EPS_I[3*j+2] = self.z[1,j]
else: else:
self.EPS_I[3*j] = self.A1*self.x[1,j] + self.A2*self.SF_delay_x[j,0] # Determine convolution (x) self.EPS_I[3*j] = self.A1*self.x[1,j] + self.A2*self.SF_delay_x[j,0] # Determine convolution (x)
self.EPS_I[3*j+1] = self.A1*self.y[1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y) self.EPS_I[3*j+1] = self.A1*self.y[1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y)
self.EPS_I[3*j+2] = self.z[1,j] # No need to convolute z-dim #self.EPS_I[3*j+2] = self.z[1,j] # No need to convolute z-dim
self.delay(1,feedBack) # Detemine feedback array self.delay(1,feedBack) # Detemine feedback array
@ -358,18 +358,22 @@ class Main:
self.ref_sig.type_mask = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin self.ref_sig.type_mask = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin
self.ref_sig.position.x = self.EPS_F[0] self.ref_sig.position.x = self.EPS_F[0]
self.ref_sig.position.y = self.EPS_F[1] self.ref_sig.position.y = self.EPS_F[1]
#self.ref_sig.position.z = self.EPS_F[2]
self.ref_sig.position.z = 5.0
self.ref_sig.velocity.x = self.EPS_F[3] self.ref_sig.velocity.x = self.EPS_F[3]
self.ref_sig.velocity.y = self.EPS_F[4] self.ref_sig.velocity.y = self.EPS_F[4]
self.ref_sig.velocity.z = self.EPS_F[5]
self.ref_sig.acceleration.x = self.EPS_F[6] self.ref_sig.acceleration.x = self.EPS_F[6]
self.ref_sig.acceleration.y = self.EPS_F[7] self.ref_sig.acceleration.y = self.EPS_F[7]
self.ref_sig.acceleration.z = self.EPS_F[8]
# Do not need to evaluate z
self.ref_sig.position.z = 5.0
self.ref_sig.velocity.z = 0
self.ref_sig.acceleration.z = 0
#self.ref_sig.position.z = self.EPS_F[2]
#self.ref_sig.velocity.z = self.EPS_F[5]
#self.ref_sig.acceleration.z = self.EPS_F[8]
self.x0 = [self.dr_pos.position.x, self.x[1,1], self.x[1,2], self.x[1,3]] self.x0 = [self.dr_pos.position.x, self.x[1,1], self.x[1,2], self.x[1,3]]
self.y0 = [self.dr_pos.position.y, self.y[1,1], self.y[1,2], self.y[1,3]] self.y0 = [self.dr_pos.position.y, self.y[1,1], self.y[1,2], self.y[1,3]]
self.z0 = [self.dr_pos.position.z, self.z[1,1], self.z[1,2], self.z[1,3]] #self.z0 = [self.dr_pos.position.z, self.z[1,1], self.z[1,2], self.z[1,3]]
self.SF_idx += 1 self.SF_idx += 1
self.FB_idx += 1 self.FB_idx += 1
@ -410,7 +414,7 @@ class Main:
self.pub_ref.publish(self.ref_sig) self.pub_ref.publish(self.ref_sig)
# Give user feedback on published message: # Give user feedback on published message:
self.screen_output() #self.screen_output()
if __name__=="__main__": if __name__=="__main__":