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

Jump to bottom

Nonholonomic slotcar turning fixes, add turning multiplier offset parameter, rename compute_ds variables #41

Merged
merged 6 commits into from
Sep 1, 2021
Merged

Nonholonomic slotcar turning fixes, add turning multiplier offset parameter, rename compute_ds variables #41

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
ae827d7
fix naming of 'velocities' to displacements
ddengster Aug 26, 2021
81f6a27
add target velocity to velocity computation
ddengster Aug 30, 2021
bd898ca
fix turning radius computation
ddengster Aug 31, 2021
919bbd8
remove unneeded code
ddengster Aug 31, 2021
9f135ce
Merge branch 'main' into fix/motion
ddengster Aug 31, 2021
d55cfac
uncrustify
ddengster Aug 31, 2021
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
28 changes: 17 additions & 11 deletions rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,11 +54,6 @@ struct NonHolonomicTrajectory
Eigen::Vector2d v1;

bool turning = false;
float turning_radius = 0.0f;
float turn_arc_radians = 0.0f;
float turn_arclength = 0.0f;
Eigen::Vector2d turn_circle_center;
Eigen::Vector2d turn_target_heading;
};

// Edit reference of parameter for template type deduction
Expand Down Expand Up @@ -145,25 +140,28 @@ class SlotcarCommon
const std::vector<Eigen::Vector3d>& obstacle_positions,
const double time);

std::pair<double, double> update_nonholonomic(Eigen::Isometry3d& pose);
std::pair<double, double> update_nonholonomic(Eigen::Isometry3d& pose,
double& target_linear_velocity);

bool emergency_stop(const std::vector<Eigen::Vector3d>& obstacle_positions,
const Eigen::Vector3d& current_heading);

std::array<double, 2> calculate_control_signals(const std::array<double,
2>& curr_velocities,
const std::pair<double, double>& velocities,
const double dt) const;
const std::pair<double, double>& displacements,
const double dt,
const double target_linear_velocity = 0.0) const;

std::array<double, 2> calculate_joint_control_signals(
const std::array<double, 2>& w_tire,
const std::pair<double, double>& velocities,
const std::pair<double, double>& displacements,
const double dt) const;

std::array<double, 2> calculate_model_control_signals(
const std::array<double, 2>& curr_velocities,
const std::pair<double, double>& velocities,
const double dt) const;
const std::pair<double, double>& displacements,
const double dt,
const double target_linear_velocity = 0.0) const;

void charge_state_cb(const std::string& name, bool selected);

Expand Down Expand Up @@ -271,6 +269,7 @@ class SlotcarCommon
double _stop_radius = 1.0;

double _min_turning_radius = -1.0; // minimum turning radius, will use a formula if negative
double _turning_right_angle_mul_offset = 1.0; // if _min_turning_radius is computed, this value multiplies it

PowerParams _params;
bool _enable_charge = true;
Expand Down Expand Up @@ -397,6 +396,13 @@ void SlotcarCommon::read_sdf(SdfPtrT& sdf)
"Setting minimum turning radius to: %f",
_min_turning_radius);

get_element_val_if_present<SdfPtrT, double>(sdf,
"turning_right_angle_mul_offset", this->_turning_right_angle_mul_offset);
RCLCPP_INFO(
logger(),
"Setting turning right angle multiplier offset to: %f",
_turning_right_angle_mul_offset);

get_element_val_if_present<SdfPtrT, double>(sdf,
"stop_distance", this->_stop_distance);
RCLCPP_INFO(logger(), "Setting stop distance to: %f", _stop_distance);
Expand Down
3 changes: 2 additions & 1 deletion rmf_robot_sim_common/include/rmf_robot_sim_common/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,8 @@ double compute_ds(
const double v_max,
const double accel_nom,
const double accel_max,
const double dt);
const double dt,
const double v_target = 0.0);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nit, there was some discussion here about the compute_ds and compute_desired_rate_of_change being basically the same function, can we take the chance to remove one of the two?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There's a line that differs between functions by double the divisor at https://github.com/open-rmf/rmf_simulation/pull/41/files/d55cfacfe9310deef0b6012e1a81ab2c7bb51394#diff-3bd4615902d61050ebeb9f584b212fc5d5ef07c1eed12cb5fa7814dbed7d59a4R41
but you're right let me remove it and test it with the other robots

Copy link
Contributor Author

@ddengster ddengster Sep 1, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

change delayed, will have to tweak the turning_right_angle_mul_offset values again #42


struct MotionParams
{
Expand Down
Loading