Skip to content

Commit

Permalink
improve obstacle avoidance planner
Browse files Browse the repository at this point in the history
  • Loading branch information
@makeecat
makeecat committed Aug 31, 2024
1 parent a43d1aa commit ac781ae
Show file tree
Hide file tree
Showing 3 changed files with 59 additions and 29 deletions.
20 changes: 11 additions & 9 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,19 +94,21 @@ The markdown lines are used for generating the documentation.
```markdown
===============================================================================
Language Files Lines Code Comments Blanks
SVG 2 492 480 0 12
===============================================================================
SVG 1 1 1 0 0
TOML 1 31 30 0 1
YAML 1 94 86 0 8
YAML 1 103 95 0 8
-------------------------------------------------------------------------------
Markdown 4 273 0 207 66
|- Markdown 1 17 0 17 0
(Total) 290 0 224 66
Markdown 4 267 0 201 66
|- BASH 1 6 6 0 0
|- Markdown 1 16 0 16 0
(Total) 289 6 217 66
-------------------------------------------------------------------------------
Rust 3 1487 1438 13 36
|- Markdown 1 300 0 300 0
(Total) 1787 1438 313 36
Rust 3 1503 1455 12 36
|- Markdown 1 303 0 303 0
(Total) 1806 1455 315 36
===============================================================================
Total 11 2377 2034 220 123
Total 10 1905 1581 213 111
===============================================================================
```

Expand Down
17 changes: 13 additions & 4 deletions config/quad.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,17 +67,26 @@ planner_schedule:
center: [0.5, 0.5, 1.0]
radius: 0.5
angular_velocity: 1.0
duration: 8.0
duration: 5.0
ramp_time: 2.0
- step: 37000
- step: 32000
planner_type: MinimumJerkLine
params:
end_position: [-2.5, 0.0, 1.0]
end_yaw: 0.0
duration: 3.0
- step: 35000
planner_type: ObstacleAvoidance
params:
target_position: [1.5, 1.0, 0.5]
target_position: [2.5, 1.0, 0.5]
duration: 10.0
end_yaw: 0.0
k_att: 0.03
k_rep: 0.02
k_rep: 0.01
k_vortex: 0.005
d0: 0.5
d_target: 0.5
max_speed: 0.1
- step: 45000
planner_type: MinimumSnapWaypoint
params:
Expand Down
51 changes: 35 additions & 16 deletions src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -670,22 +670,31 @@ pub struct ObstacleAvoidancePlanner {
k_att: f32,
/// Repulsive force gain
k_rep: f32,
/// Vortex force gain
k_vortex: f32,
/// Influence distance of obstacles
d0: f32,
/// Influence distance of target
d_target: f32,
/// Maximum speed of the quadrotor
max_speed: f32,
}

impl Planner for ObstacleAvoidancePlanner {
fn plan(
&self,
current_position: Vector3<f32>,
_current_velocity: Vector3<f32>,
current_velocity: Vector3<f32>,
time: f32,
) -> (Vector3<f32>, Vector3<f32>, f32) {
let t = ((time - self.start_time) / self.duration).clamp(0.0, 1.0);
// Attractive force towards the goal
let f_att = self.k_att * (self.target_position - current_position);
let distance_to_target = (self.target_position - current_position).norm();
let f_att = self.k_att
* self.smooth_attractive_force(distance_to_target)
* (self.target_position - current_position).normalize();
// Repulsive force from obstacles
let mut f_rep = Vector3::zeros();
let mut f_vortex = Vector3::zeros();
for obstacle in &self.obstacles {
let diff = current_position - obstacle.position;
let distance = diff.norm();
Expand All @@ -694,11 +703,12 @@ impl Planner for ObstacleAvoidancePlanner {
* (1.0 / distance - 1.0 / self.d0)
* (1.0 / distance.powi(2))
* diff.normalize();
f_vortex +=
self.k_vortex * current_velocity.cross(&diff).normalize() / distance.powi(2);
}
}
let f_total = f_att + f_rep;
let max_speed: f32 = 1.0;
let desired_velocity = f_total.normalize() * max_speed.min(f_total.norm());
let f_total = f_att + f_rep + f_vortex;
let desired_velocity = f_total.normalize() * self.max_speed.min(f_total.norm());
let desired_position = current_position + desired_velocity * self.duration * (1.0 - t);
let desired_yaw = self.start_yaw + (self.end_yaw - self.start_yaw) * t;
(desired_position, desired_velocity, desired_yaw)
Expand All @@ -710,6 +720,15 @@ impl Planner for ObstacleAvoidancePlanner {
}
}

impl ObstacleAvoidancePlanner {
fn smooth_attractive_force(&self, distance: f32) -> f32 {
if distance <= self.d_target {
distance
} else {
self.d_target + (distance - self.d_target).tanh()
}
}
}
/// Waypoint planner that generates a minimum snap trajectory between waypoints
/// # Arguments
/// * `waypoints` - A list of waypoints to follow
Expand Down Expand Up @@ -777,11 +796,9 @@ impl MinimumSnapWaypointPlanner {
j as f32 * (j - 1) as f32 * (j - 2) as f32 * duration.powi(j as i32 - 3);
}
}
let coeffs = a.lu().solve(&b).ok_or_else(|| {
SimulationError::NalgebraError(
"Failed to solve for coefficients in MinimumSnapWaypointPlanner".to_string(),
)
})?;
let coeffs = a.lu().solve(&b).ok_or(SimulationError::NalgebraError(
"Failed to solve for coefficients in MinimumSnapWaypointPlanner".to_string(),
))?;
self.coefficients.push(
(0..8)
.map(|j| Vector3::new(coeffs[(j, 0)], coeffs[(j, 1)], coeffs[(j, 2)]))
Expand Down Expand Up @@ -979,7 +996,10 @@ fn create_planner(
obstacles: obstacles.clone(),
k_att: parse_f32(params, "k_att")?,
k_rep: parse_f32(params, "k_rep")?,
k_vortex: parse_f32(params, "k_vortex")?,
d0: parse_f32(params, "d0")?,
d_target: parse_f32(params, "d_target")?,
max_speed: parse_f32(params, "max_speed")?,
})),
"MinimumSnapWaypoint" => {
let mut waypoints = vec![quad.position];
Expand All @@ -997,22 +1017,20 @@ fn create_planner(
coords.get(2)?.as_f64()? as f32,
))
})
.ok_or_else(|| {
SimulationError::OtherError("Invalid waypoint".to_string())
})
.ok_or(SimulationError::OtherError("Invalid waypoint".to_string()))
})
.collect::<Result<Vec<_>, _>>()?,
);
let mut yaws = vec![quad.orientation.euler_angles().2];
yaws.extend(
params["yaws"]
.as_sequence()
.ok_or_else(|| SimulationError::OtherError("Invalid yaws".to_string()))?
.ok_or(SimulationError::OtherError("Invalid yaws".to_string()))?
.iter()
.map(|y| {
y.as_f64()
.map(|v| v as f32)
.ok_or_else(|| SimulationError::OtherError("Invalid yaw".to_string()))
.ok_or(SimulationError::OtherError("Invalid yaw".to_string()))
})
.collect::<Result<Vec<_>, _>>()?,
);
Expand Down Expand Up @@ -1528,6 +1546,7 @@ pub fn log_depth_image(
/// * `gray` - The gray value in the range [0, 255]
/// # Returns
/// * The RGB color value in the range [0, 255]
#[inline]
pub fn color_map_fn(gray: f32) -> (u8, u8, u8) {
let x = gray / 255.0;
let r = (34.61
Expand Down

0 comments on commit ac781ae

Please sign in to comment.