forked from cesar.alejandro/oscillation_ctrl
Fixed issues/bugs from branch merge
This commit is contained in:
parent
309a33c08d
commit
71f7574383
|
|
@ -42,7 +42,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
type="klausen_control.py"
|
||||
name="klausenCtrl_node"
|
||||
output="screen"
|
||||
|
||||
/>
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
|
|
|
|||
|
|
@ -55,10 +55,10 @@ class Main:
|
|||
self.has_run = 0 # Bool to keep track of first run instance
|
||||
|
||||
# Col1 = theta, theta dot; Col2 = phi, phidot for self.PHI
|
||||
self.PHI = np.array([[0,0],[0,0]])
|
||||
self.dr_vel = np.zeros([3,1])
|
||||
self.PHI = np.array([[0,0],[0,0]])
|
||||
self.dr_vel = 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
|
||||
# Get tether length
|
||||
|
|
@ -74,8 +74,8 @@ class Main:
|
|||
break
|
||||
|
||||
# Tuning gains
|
||||
self.K1 = np.identity(3)
|
||||
self.K2 = np.identity(5)
|
||||
self.K1 = np.identity(3)
|
||||
self.K2 = np.identity(5)
|
||||
|
||||
# Values which require updates. *_p = prior
|
||||
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))
|
||||
|
||||
# Update a45_0 to new a45. Need to transpose to column vector
|
||||
self.a45_0 = self.a45_buff[1,:]
|
||||
self.a45 = np.array([[self.a45_0[0]],[self.a45_0[1]]])
|
||||
self.a45_0 = self.a45_buff[1,:]
|
||||
self.a45 = np.array([[self.a45_0[0]],[self.a45_0[1]]])
|
||||
|
||||
# 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
|
||||
|
|
@ -316,7 +316,7 @@ class Main:
|
|||
def publisher(self,pub_time):
|
||||
self.control()
|
||||
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
|
||||
|
|
|
|||
|
|
@ -120,10 +120,10 @@ int main(int argc, char **argv)
|
|||
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
|
||||
("mavros/cmd/arming");
|
||||
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>
|
||||
("mavros/cmd/takeoff");
|
||||
ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
|
||||
ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
|
||||
("status/waypoint_tracker");
|
||||
|
||||
//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
|
||||
while(ros::ok() && !current_state.connected){
|
||||
ros::spinOnce();
|
||||
ROS_INFO("Waiting for FCU connection");
|
||||
ROS_INFO("Waiting for FCU connection");
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
if (current_state.connected){
|
||||
ROS_INFO("Connected to FCU");
|
||||
} else {
|
||||
ROS_INFO("Never Connected");
|
||||
}
|
||||
|
||||
/*********** 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
|
||||
mavros_msgs::AttitudeTarget attitude;
|
||||
|
||||
attitude.header.frame_id = "map"; // NED Ref Frame(?) #TODO
|
||||
attitude.type_mask = 1|2|4; // Ignore body rates
|
||||
|
||||
// Retrieve desired waypoints
|
||||
oscillation_ctrl::WaypointTrack wpoint_srv;
|
||||
wpoint_srv.request.query = false;
|
||||
if (waypoint_cl.call(wpoint_srv))
|
||||
{
|
||||
ROS_INFO("Waypoints received");
|
||||
}
|
||||
// Retrieve desired waypoints
|
||||
oscillation_ctrl::WaypointTrack wpoint_srv;
|
||||
wpoint_srv.request.query = false;
|
||||
if (waypoint_cl.call(wpoint_srv))
|
||||
{
|
||||
ROS_INFO("Waypoints received");
|
||||
}
|
||||
|
||||
// populate desired waypoints - this is only for original hover as
|
||||
// a change of waypoints is handled by ref_signal.py
|
||||
// populate desired waypoints - this is only for original hover as
|
||||
// a change of waypoints is handled by ref_signal.py
|
||||
buff_pose.pose.position.x = wpoint_srv.response.xd.x;
|
||||
buff_pose.pose.position.y = wpoint_srv.response.xd.y;
|
||||
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
|
||||
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){
|
||||
local_pos_pub.publish(buff_pose);
|
||||
ros::spinOnce();
|
||||
ROS_INFO("Publishing position setpoints");
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
|
|
@ -199,21 +208,16 @@ int main(int argc, char **argv)
|
|||
eulAng.commanded = K_ang;
|
||||
eulAng.desired = J_ang;
|
||||
|
||||
if(current_state.mode != "OFFBOARD" &&
|
||||
(ros::Time::now() - last_request > ros::Duration(5.0))){
|
||||
if( set_mode_client.call(offb_set_mode) &&
|
||||
offb_set_mode.response.mode_sent){
|
||||
ROS_INFO("Offboard enabled");
|
||||
if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
|
||||
if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
|
||||
} 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 {
|
||||
if( !current_state.armed &&
|
||||
(ros::Time::now() - last_request > ros::Duration(3.0))){
|
||||
if( arming_client.call(arm_cmd) &&
|
||||
arm_cmd.response.success){
|
||||
ROS_INFO("Vehicle armed");
|
||||
if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){
|
||||
if( arming_client.call(arm_cmd) && arm_cmd.response.success){
|
||||
ROS_INFO("Vehicle armed");
|
||||
}
|
||||
last_request = ros::Time::now();
|
||||
}
|
||||
|
|
@ -229,17 +233,17 @@ int main(int argc, char **argv)
|
|||
attitude.thrust = thrust.thrust;
|
||||
|
||||
// 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.header.stamp = ros::Time::now();
|
||||
att_targ.publish(attitude);
|
||||
att_cmds = true;
|
||||
ROS_INFO("Attitude Ctrl");
|
||||
ROS_INFO("ATTITUDE CTRL");
|
||||
ROS_INFO("Des Altitude: %.2f", alt_des);
|
||||
ROS_INFO("Cur Altitude: %.2f", current_altitude);
|
||||
} else {
|
||||
local_pos_pub.publish(buff_pose);
|
||||
ROS_INFO("Position Ctrl");
|
||||
ROS_INFO("POSITION CTRL");
|
||||
ROS_INFO("Des Altitude: %.2f", alt_des);
|
||||
ROS_INFO("Cur Altitude: %.2f", current_altitude);
|
||||
}
|
||||
|
|
@ -248,7 +252,7 @@ int main(int argc, char **argv)
|
|||
euler_pub.publish(eulAng);
|
||||
|
||||
ros::spinOnce();
|
||||
rate.sleep();
|
||||
rate_pub.sleep();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -324,12 +324,12 @@ class Main:
|
|||
# SOLVE ODE (get ref pos, vel, accel)
|
||||
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.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)):
|
||||
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.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
|
||||
|
||||
|
|
@ -338,11 +338,11 @@ class Main:
|
|||
if self.SF_idx < len(self.SF_delay_x[0]):
|
||||
self.EPS_I[3*j] = self.x[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:
|
||||
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+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
|
||||
|
||||
|
|
@ -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.position.x = self.EPS_F[0]
|
||||
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.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.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.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.FB_idx += 1
|
||||
|
|
@ -410,7 +414,7 @@ class Main:
|
|||
self.pub_ref.publish(self.ref_sig)
|
||||
|
||||
# Give user feedback on published message:
|
||||
self.screen_output()
|
||||
#self.screen_output()
|
||||
|
||||
|
||||
if __name__=="__main__":
|
||||
|
|
|
|||
Loading…
Reference in New Issue