Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support for minimising the steering angle difference between previous and next angle #1

Open
wants to merge 1 commit into
base: swerve-drive
Choose a base branch
from
Open

Support for minimising the steering angle difference between previous and next angle #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 12 additions & 0 deletions swerve_controller/include/swerve_controller/swerve_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@
#include <cmath>
#include <string>
#include <boost/optional.hpp>
#include <algorithm>

#include <controller_interface/multi_interface_controller.h>
#include <hardware_interface/joint_command_interface.h>
Expand Down Expand Up @@ -148,19 +149,30 @@ namespace swerve_controller
/// Whether to publish odometry to tf or not:
bool enable_odom_tf_;

// Whether to enable minimum steering difference by checking whether the calculated angle or its
// polar opposite angle are closer to the previously set one
bool enable_min_steering_difference_;

/// Speed limiters:
CommandTwist last1_cmd_;
CommandTwist last0_cmd_;
SpeedLimiter limiter_lin_;
SpeedLimiter limiter_ang_;

double lf_steering_last;
double rf_steering_last;
double lh_steering_last;
double rh_steering_last;

private:
void updateOdometry(const ros::Time &time);

void updateCommand(const ros::Time &time, const ros::Duration &period);

void brake();

void minSteeringDifference(double &steering, double &previous_steering, double &speed);

bool clipSteeringAngle(double &steering, double &speed);

void cmdVelCallback(const geometry_msgs::Twist &command);
Expand Down
43 changes: 41 additions & 2 deletions swerve_controller/src/swerve_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,9 @@ namespace swerve_controller
max_steering_angle_(M_PI),
cmd_vel_timeout_(0.5),
base_frame_id_("base_link"),
enable_odom_tf_(true)
{
enable_odom_tf_(true),
enable_min_steering_difference_(false)
{
}

bool SwerveController::init(hardware_interface::RobotHW *robot_hw,
Expand Down Expand Up @@ -110,6 +111,10 @@ namespace swerve_controller
ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is "
<< (enable_odom_tf_ ? "enabled" : "disabled"));

controller_nh.param("enable_min_steering_difference", enable_min_steering_difference_, enable_min_steering_difference_);
ROS_INFO_STREAM_NAMED(name_, "Minimum steering difference is "
<< (enable_min_steering_difference_ ? "enabled" : "disabled"));

// Get velocity and acceleration limits from the parameter server
controller_nh.param("linear/x/has_velocity_limits",
limiter_lin_.has_velocity_limits,
Expand Down Expand Up @@ -367,10 +372,19 @@ namespace swerve_controller
!clipSteeringAngle(lh_steering, lh_speed) ||
!clipSteeringAngle(rh_steering, rh_speed))
{
ROS_WARN("Braking because clipped steering angle was impossible to reach");
brake();
return;
}

// Guarantee minimum angle difference to next steer angle by using previous steer angle
if (enable_min_steering_difference_){
minSteeringDifference(lf_steering, lf_steering_last, lf_speed);
minSteeringDifference(rf_steering, rf_steering_last, rf_speed);
minSteeringDifference(lh_steering, lh_steering_last, lh_speed);
minSteeringDifference(rh_steering, rh_steering_last, rh_speed);
}

// Set wheels velocities
if (lf_wheel_joint_ && rf_wheel_joint_ && lh_wheel_joint_ && rh_wheel_joint_)
{
Expand All @@ -388,6 +402,11 @@ namespace swerve_controller
lh_steering_joint_->setCommand(lh_steering);
rh_steering_joint_->setCommand(rh_steering);
}

lf_steering_last = lf_steering;
rf_steering_last = rf_steering;
lh_steering_last = lh_steering;
rh_steering_last = rh_steering;
}

void SwerveController::brake()
Expand Down Expand Up @@ -435,6 +454,26 @@ namespace swerve_controller
return true;
}

void SwerveController::minSteeringDifference(double &steering, double &previous_steering, double &speed)
{
double polarSteering = 0;
if (steering > 0)
{
polarSteering = steering - M_PI;
}
else
{
polarSteering = steering + M_PI;
}

// Take whichever is closer, the computed angle or its polar opposite angle
if (std::abs(previous_steering - steering) > std::abs(previous_steering - polarSteering))
{
steering = polarSteering;
speed = -speed;
}
}

void SwerveController::cmdVelCallback(const geometry_msgs::Twist &command)
{
if (isRunning())
Expand Down
9 changes: 6 additions & 3 deletions swerve_controller/test/config/swervebot_control.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,11 @@ swervebot:
rh_steering: "leg_rh_joint"

# Range of motion of steering motors
min_steering_angle: -1.58
max_steering_angle: 1.58
# min_steering_angle: -1.58
# max_steering_angle: 1.58

# Minimise steering difference between previous and next steering angle (continuous steering joints only)
# enable_min_steering_difference: true

# Other
publish_rate: 50
Expand All @@ -46,4 +49,4 @@ swervebot:
wheel_lf_joint: {p: 50.0, i: 2.0, d: 0.001}
wheel_rf_joint: {p: 50.0, i: 2.0, d: 0.001}
wheel_lh_joint: {p: 50.0, i: 2.0, d: 0.001}
wheel_rh_joint: {p: 50.0, i: 2.0, d: 0.001}
wheel_rh_joint: {p: 50.0, i: 2.0, d: 0.001}