// HERE IS THE BIG DEAL: USE DESIRED AND ACTUAL STATE TO COMPUTE AND PUBLISH CMD_VEL void SteeringController::lin_steering_algorithm() { double controller_speed; double controller_omega; //Eigen::Vector2d pos_err_xy_vec_; //Eigen::Vector2d t_vec; //tangent of desired path //Eigen::Vector2d n_vec; //normal to desired path, pointing to the "left" double tx = cos(des_state_phi_); double ty = sin(des_state_phi_); double nx = -ty; double ny = tx; double heading_err; double lateral_err; double trip_dist_err; // error is scheduling...are we ahead or behind? // have access to: des_state_vel_, des_state_omega_, des_state_x_, des_state_y_, des_state_phi_ and corresponding odom values double dx = des_state_x_- odom_x_; double dy = des_state_y_ - odom_y_; //pos_err_xy_vec_ = des_xy_vec_ - odom_xy_vec_; // vector pointing from odom x-y to desired x-y //lateral_err = n_vec.dot(pos_err_xy_vec_); //signed scalar lateral offset error; if positive, then desired state is to the left of odom lateral_err = dx*nx + dy*ny; trip_dist_err = dx*tx + dy*ty; //trip_dist_err = t_vec.dot(pos_err_xy_vec_); // progress error: if positive, then we are behind schedule heading_err = min_dang(des_state_phi_ - odom_phi_); // if positive, should rotate +omega to align with desired heading // DEBUG OUTPUT... // ROS_INFO("des_state_phi, odom_phi, heading err = %f, %f, %f", des_state_phi_,odom_phi_,heading_err); // ROS_INFO("lateral err, trip dist err = %f, %f",lateral_err,trip_dist_err); // DEFINITELY COMMENT OUT ALL cout<< OPERATIONS FOR REAL-TIME CODE //std::cout<<des_xy_vec_<<std::endl; //std::cout<<odom_xy_vec_<<std::endl; // let's put these in a message to publish, for rqt_plot to display //steering_errs_.data.clear(); //steering_errs_.data.push_back(lateral_err); //steering_errs_.data.push_back(heading_err); //steering_errs_.data.push_back(trip_dist_err); //steering_errs_publisher_.publish(steering_errs_); // suitable for plotting w/ rqt_plot //END OF DEBUG STUFF // do something clever with this information controller_speed = des_state_vel_ + K_TRIP_DIST*trip_dist_err; //you call that clever ?!?!?!? should speed up/slow down to null out //controller_omega = des_state_omega_; //ditto controller_omega = des_state_omega_ + K_PHI*heading_err + K_DISP*lateral_err; controller_omega = MAX_OMEGA*sat(controller_omega/MAX_OMEGA); // saturate omega command at specified limits // send out our very clever speed/spin commands: twist_cmd_.linear.x = controller_speed; twist_cmd_.angular.z = controller_omega; twist_cmd2_.twist = twist_cmd_; // copy the twist command into twist2 message twist_cmd2_.header.stamp = ros::Time::now(); // look up the time and put it in the header cmd_publisher_.publish(twist_cmd_); cmd_publisher2_.publish(twist_cmd2_); }
//here are the main traj-builder fncs: //for spin-in-place motion that would hit maximum angular velocity, construct // trapezoidal angular velocity profile void TrajBuilder::build_trapezoidal_spin_traj(geometry_msgs::PoseStamped start_pose, geometry_msgs::PoseStamped end_pose, std::vector<nav_msgs::Odometry> &vec_of_states) { double x_start = start_pose.pose.position.x; double y_start = start_pose.pose.position.y; double x_end = end_pose.pose.position.x; double y_end = end_pose.pose.position.y; double dx = x_end - x_start; double dy = y_end - y_start; double psi_start = convertPlanarQuat2Psi(start_pose.pose.orientation); double psi_end = convertPlanarQuat2Psi(end_pose.pose.orientation); double dpsi = min_dang(psi_end - psi_start); double t_ramp = omega_max_/ alpha_max_; double ramp_up_dist = 0.5 * alpha_max_ * t_ramp*t_ramp; double cruise_distance = fabs(dpsi) - 2.0 * ramp_up_dist; //delta-angle to spin at omega_max int npts_ramp = round(t_ramp / dt_); nav_msgs::Odometry des_state; des_state.header = start_pose.header; //really, want to copy the frame_id des_state.pose.pose = start_pose.pose; //start from here des_state.twist.twist = halt_twist_; // insist on starting from rest //ramp up omega (positive or negative); double t = 0.0; double accel = sgn(dpsi) * alpha_max_; double omega_des = 0.0; double psi_des = psi_start; for (int i = 0; i < npts_ramp; i++) { t += dt_; omega_des = accel*t; des_state.twist.twist.angular.z = omega_des; //update rotation rate //update orientation psi_des = psi_start + 0.5 * accel * t*t; des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des); vec_of_states.push_back(des_state); } //now cruise for distance cruise_distance at const omega omega_des = sgn(dpsi)*omega_max_; des_state.twist.twist.angular.z = sgn(dpsi)*omega_max_; double t_cruise = cruise_distance / omega_max_; int npts_cruise = round(t_cruise / dt_); for (int i = 0; i < npts_cruise; i++) { //Euler one-step integration psi_des += omega_des*dt_; //Euler one-step integration des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des); vec_of_states.push_back(des_state); } //ramp down omega to halt: for (int i = 0; i < npts_ramp; i++) { omega_des -= accel*dt_; //Euler one-step integration des_state.twist.twist.angular.z = omega_des; psi_des += omega_des*dt_; //Euler one-step integration des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des); vec_of_states.push_back(des_state); } //make sure the last state is precisely where desired, and at rest: des_state.pose.pose = end_pose.pose; // des_state.twist.twist = halt_twist_; // insist on full stop vec_of_states.push_back(des_state); }
void TrajBuilder::build_triangular_spin_traj(geometry_msgs::PoseStamped start_pose, geometry_msgs::PoseStamped end_pose, std::vector<nav_msgs::Odometry> &vec_of_states) { nav_msgs::Odometry des_state; des_state.header = start_pose.header; //really, want to copy the frame_id des_state.pose.pose = start_pose.pose; //start from here des_state.twist.twist = halt_twist_; // insist on starting from rest vec_of_states.push_back(des_state); double psi_start = convertPlanarQuat2Psi(start_pose.pose.orientation); double psi_end = convertPlanarQuat2Psi(end_pose.pose.orientation); double dpsi = min_dang(psi_end - psi_start); ROS_INFO("spin traj: psi_start = %f; psi_end = %f; dpsi= %f", psi_start, psi_end, dpsi); double t_ramp = sqrt(fabs(dpsi) / alpha_max_); int npts_ramp = round(t_ramp / dt_); double psi_des = psi_start; //start from here double omega_des = 0.0; // assumes spin starts from rest; // position of des_state will not change; only orientation and twist double t = 0.0; double accel = sgn(dpsi) * alpha_max_; //watch out for sign: CW vs CCW rotation //ramp up; for (int i = 0; i < npts_ramp; i++) { t += dt_; omega_des = accel*t; des_state.twist.twist.angular.z = omega_des; //update rotation rate //update orientation psi_des = psi_start + 0.5 * accel * t*t; des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des); vec_of_states.push_back(des_state); } //ramp down: for (int i = 0; i < npts_ramp; i++) { omega_des -= accel*dt_; //Euler one-step integration des_state.twist.twist.angular.z = omega_des; psi_des += omega_des*dt_; //Euler one-step integration des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des); vec_of_states.push_back(des_state); } //make sure the last state is precisely where requested, and at rest: des_state.pose.pose = end_pose.pose; //start from here des_state.twist.twist = halt_twist_; // insist on starting from rest vec_of_states.push_back(des_state); }
// HERE IS THE STEERING ALGORITHM: USE DESIRED AND ACTUAL STATE TO COMPUTE AND PUBLISH CMD_VEL void SteeringController::mobot_nl_steering() { double controller_speed; double controller_omega; // have access to: des_state_vel_, des_state_omega_, des_state_x_, des_state_y_, // des_state_phi_ and corresponding robot state values double tx = cos(des_state_psi_); // [tx,ty] is tangent of desired path double ty = sin(des_state_psi_); double nx = -ty; //components [nx, ny] of normal to path, points to left of desired heading double ny = tx; double dx = state_x_ - des_state_x_; //x-error relative to desired path double dy = state_y_ - des_state_y_; //y-error lateral_err_ = dx*nx+dy*ny; //lateral error is error vector dotted with path normal // lateral offset error is positive if robot is to the left of the path double trip_dist_err = dx*tx+dy*ty; // progress error: if positive, then we are ahead of schedule //heading error: if positive, should rotate -omega to align with desired heading double heading_err = min_dang(state_psi_ - des_state_psi_); double strategy_psi = psi_strategy(lateral_err_); //heading command, based on NL algorithm controller_omega = omega_cmd_fnc(strategy_psi, state_psi_, des_state_psi_); controller_speed = MAX_SPEED; //default...should speed up/slow down appropriately // send out our speed/spin commands: twist_cmd_.linear.x = controller_speed; twist_cmd_.angular.z = controller_omega; cmd_publisher_.publish(twist_cmd_); // DEBUG OUTPUT... ROS_INFO("des_state_phi, heading err = %f, %f", des_state_psi_,heading_err); ROS_INFO("lateral err, trip dist err = %f, %f",lateral_err_,trip_dist_err); std_msgs::Float32 float_msg; float_msg.data = lateral_err_; lat_err_publisher_.publish(float_msg); float_msg.data = state_psi_; heading_publisher_.publish(float_msg); float_msg.data = psi_cmd_; heading_cmd_publisher_.publish(float_msg); //END OF DEBUG OUTPUT }
//upper-level function to construct a spin-in-place trajectory // this function decides if triangular or trapezoidal angular-velocity // profile is needed void TrajBuilder::build_spin_traj(geometry_msgs::PoseStamped start_pose, geometry_msgs::PoseStamped end_pose, std::vector<nav_msgs::Odometry> &vec_of_states) { //decide if triangular or trapezoidal profile: double x_start = start_pose.pose.position.x; double y_start = start_pose.pose.position.y; double x_end = end_pose.pose.position.x; double y_end = end_pose.pose.position.y; double dx = x_end - x_start; double dy = y_end - y_start; double psi_start = convertPlanarQuat2Psi(start_pose.pose.orientation); double psi_end = convertPlanarQuat2Psi(end_pose.pose.orientation); double dpsi = min_dang(psi_end - psi_start); ROS_INFO("rotational spin distance = %f", dpsi); double ramp_up_time = omega_max_/ alpha_max_; double ramp_up_dist = 0.5 * alpha_max_ * ramp_up_time*ramp_up_time; //decide on triangular vs trapezoidal: if (fabs(dpsi) < 2.0 * ramp_up_dist) { //delta-angle is too short for trapezoid build_triangular_spin_traj(start_pose, end_pose, vec_of_states); } else { build_trapezoidal_spin_traj(start_pose, end_pose, vec_of_states); } }