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.

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

adding gamma term on noise smoothing to base implementation (ready for review) #99

Merged
merged 59 commits into from
Dec 22, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
59 commits
Select commit Hold shift + click to select a range
c757f43
adding gamma term on noise smoothing
SteveMacenski Oct 6, 2022
5f27dc7
updating default for temp
SteveMacenski Oct 6, 2022
bcfa066
add squared terms to control cost
SteveMacenski Oct 11, 2022
2a2051d
adding updated smoother obstacles critic
SteveMacenski Oct 11, 2022
b3b03a4
fix normalization term
SteveMacenski Oct 12, 2022
dc64589
minor optimizations in visualizer code
SteveMacenski Oct 12, 2022
7b5fcc1
fixing a math typo
SteveMacenski Oct 12, 2022
b92c837
integrated cost for obstacle critic + normalizing by trajectory length
SteveMacenski Oct 13, 2022
10b8866
adding moved control clamping + constraint critic
SteveMacenski Oct 13, 2022
743dc48
adding last bits
SteveMacenski Oct 14, 2022
2c6e775
minor updates to constraint critic
SteveMacenski Oct 14, 2022
a516c3e
update readme with new critic
SteveMacenski Oct 15, 2022
c72845f
Merge branch 'dev' into infomatics
SteveMacenski Oct 15, 2022
cf16369
using floats to manage cost, not unsigned chars, since we're adding t…
SteveMacenski Oct 16, 2022
f437871
Merge branch 'infomatics' of github.com:artofnothingness/mppic into i…
SteveMacenski Oct 16, 2022
f8fec2e
distance-based obstacle critic
SteveMacenski Nov 17, 2022
84963f9
completed obstacle critic
SteveMacenski Nov 17, 2022
2280b51
update tuned defaults
SteveMacenski Nov 17, 2022
ae6a9a7
adding Savitzky-Golay filter on control output
SteveMacenski Nov 18, 2022
1b04bae
Merge pull request #102 from artofnothingness/sgf
SteveMacenski Nov 28, 2022
f8509d5
lint + doxygen
SteveMacenski Nov 29, 2022
a12836c
adding copyright headers
SteveMacenski Nov 29, 2022
6173f34
adding header gaurds to critics
SteveMacenski Nov 29, 2022
bafa9f2
rename controller / plugin to be less abstract
SteveMacenski Nov 30, 2022
9b74143
fix test
SteveMacenski Nov 30, 2022
73f06ab
add in refactors to readme
SteveMacenski Nov 30, 2022
9ee8b76
remove deps, going to merge into 22.04
SteveMacenski Nov 30, 2022
b1116ab
feature bullet
SteveMacenski Nov 30, 2022
73a9a71
adding a ton of unit testing
SteveMacenski Dec 2, 2022
178c0db
complete parameter dynamic handler tests
SteveMacenski Dec 2, 2022
c1eadc9
adding motion model tests + fix to ackermann
SteveMacenski Dec 3, 2022
6076a3f
adding unit testing for trajectory visualization and partial coverage…
SteveMacenski Dec 6, 2022
36da4c8
Fixing comparison for finding path point closest to final trajectories
SteveMacenski Dec 6, 2022
0740ec9
adding more tests
SteveMacenski Dec 7, 2022
462a651
moooooooor unit tests
SteveMacenski Dec 8, 2022
5f9aec1
adding path angle nand prefer forward tests
SteveMacenski Dec 8, 2022
c045b9d
Merge pull request #105 from artofnothingness/SteveMacenski-patch-1
SteveMacenski Dec 8, 2022
c2c8511
half of the required optimizer tests
SteveMacenski Dec 9, 2022
66f13da
completed testing!
SteveMacenski Dec 9, 2022
fcae96e
Merge pull request #104 from artofnothingness/doxygen
SteveMacenski Dec 9, 2022
7f33773
adding path align to smoke test
SteveMacenski Dec 9, 2022
b684cb7
fixing test
SteveMacenski Dec 10, 2022
d5520be
major updates to improve path tracking and obstacle avoidance INCOMPLETE
SteveMacenski Dec 16, 2022
e43e6a7
obstacle changes complete
SteveMacenski Dec 16, 2022
6d75601
adding docs
SteveMacenski Dec 16, 2022
bb0d4ea
adding discussion on cost values
SteveMacenski Dec 16, 2022
8dcbe77
change to else if
SteveMacenski Dec 20, 2022
9c738ca
add path follow validity check
SteveMacenski Dec 20, 2022
b007539
removing crashing code temp
SteveMacenski Dec 20, 2022
dd3d990
completed for dynamic obstacles
SteveMacenski Dec 20, 2022
2f02b02
make prune utilize things within costmap with better gaurentees
SteveMacenski Dec 21, 2022
050947a
dynamic obstacle fully supported
SteveMacenski Dec 21, 2022
a856d4a
Merge pull request #106 from artofnothingness/major_updates
SteveMacenski Dec 21, 2022
a7b2674
updating defaults
SteveMacenski Dec 21, 2022
056418a
linting and error handling
SteveMacenski Dec 21, 2022
996a93a
add additional unit tests
SteveMacenski Dec 21, 2022
13ca43d
typo fix
SteveMacenski Dec 21, 2022
0807e22
update defaults
SteveMacenski Dec 21, 2022
ce2e8d6
adding a couple of line overlooks
SteveMacenski Dec 21, 2022
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
3 changes: 2 additions & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ add_library(critics SHARED
src/critics/path_angle_critic.cpp
src/critics/prefer_forward_critic.cpp
src/critics/twirling_critic.cpp
src/critics/constraint_critic.cpp
)

set(libraries mppic critics)
Expand Down Expand Up @@ -80,7 +81,7 @@ if(BUILD_TESTING)
set(ament_cmake_copyright_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
add_subdirectory(test)
add_subdirectory(benchmark)
# add_subdirectory(benchmark)
endif()

ament_export_libraries(${libraries})
Expand Down
270 changes: 158 additions & 112 deletions README.md

Large diffs are not rendered by default.

15 changes: 13 additions & 2 deletions benchmark/controller_benchmark.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,16 @@
// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <benchmark/benchmark.h>
#include <string>
Expand Down Expand Up @@ -34,7 +46,6 @@ void prepareAndRunBenchmark(
bool consider_footprint, std::string motion_model,
std::vector<std::string> critics, benchmark::State & state)
{

bool visualize = false;

int batch_size = 300;
Expand Down
15 changes: 13 additions & 2 deletions benchmark/optimizer_benchmark.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,16 @@
// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <benchmark/benchmark.h>
#include <string>
Expand Down Expand Up @@ -79,7 +91,6 @@ void prepareAndRunBenchmark(
for (auto _ : state) {
optimizer->evalControl(pose, velocity, path, dummy_goal_checker);
}

}

static void BM_DiffDrivePointFootprint(benchmark::State & state)
Expand Down
3 changes: 3 additions & 0 deletions critics.xml
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,9 @@
<description>mppi critic for preventing twirling behavior when using omnidirectional models</description>
</class>

<class type="mppi::critics::ConstraintCritic" base_class_type="mppi::critics::CriticFunction">
<description>mppi critic for incentivizing moving within kinematic and dynamic bounds</description>
</class>

</library>
</class_libraries>
70 changes: 66 additions & 4 deletions include/mppic/controller.hpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,17 @@
// Copyright 2022 FastSense, Samsung Research
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MPPIC__CONTROLLER_HPP_
#define MPPIC__CONTROLLER_HPP_

Expand All @@ -18,30 +31,79 @@
namespace mppi
{

class Controller : public nav2_core::Controller
/**
* @class mppi::MPPIController
* @brief Main plugin controller for MPPI Controller
*/
class MPPIController : public nav2_core::Controller
{
public:
Controller() = default;

/**
* @brief Constructor for mppi::MPPIController
*/
MPPIController() = default;

/**
* @brief Configure controller on bringup
* @param parent WeakPtr to node
* @param name Name of plugin
* @param tf TF buffer to use
* @param costmap_ros Costmap2DROS object of environment
*/
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, const std::shared_ptr<tf2_ros::Buffer> tf,
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;

/**
* @brief Cleanup resources
*/
void cleanup() override;

/**
* @brief Activate controller
*/
void activate() override;

/**
* @brief Deactivate controller
*/
void deactivate() override;

/**
* @brief Reset the controller state between tasks
*/
void reset() override;

/**
* @brief Main method to compute velocities using the optimizer
* @param robot_pose Robot pose
* @param robot_speed Robot speed
* @param goal_checker Pointer to the goal checker for awareness if completed task
*/
geometry_msgs::msg::TwistStamped computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & robot_pose,
const geometry_msgs::msg::Twist & robot_speed,
nav2_core::GoalChecker * goal_checker) override;

/**
* @brief Set new reference path to track
* @param path Path to track
*/
void setPlan(const nav_msgs::msg::Path & path) override;

/**
* @brief Set new speed limit from callback
* @param speed_limit Speed limit to use
* @param percentage Bool if the speed limit is absolute or relative
*/
void setSpeedLimit(const double & speed_limit, const bool & percentage) override;

protected:
/**
* @brief Visualize trajectories
* @param transformed_plan Transformed input plan
*/
void visualize(nav_msgs::msg::Path transformed_plan);

std::string name_;
Expand Down
35 changes: 30 additions & 5 deletions include/mppic/critic_data.hpp
Original file line number Diff line number Diff line change
@@ -1,19 +1,40 @@
// Copyright 2022 @artofnothingness Aleksei Budyakov, Samsung Research

#pragma once

// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MPPIC__CRITIC_DATA_HPP_
#define MPPIC__CRITIC_DATA_HPP_

#include <memory>
#include <vector>
#include <xtensor/xtensor.hpp>

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_core/goal_checker.hpp"
#include "mppic/models/state.hpp"
#include "mppic/models/trajectories.hpp"
#include "mppic/models/path.hpp"
#include "mppic/motion_models.hpp"


namespace mppi
{

/**
* @struct mppi::CriticData
* @brief Data to pass to critics for scoring, including state, trajectories, path, costs, and
* important parameters to share
*/
struct CriticData
{
const models::State & state;
Expand All @@ -25,7 +46,11 @@ struct CriticData

bool fail_flag;
nav2_core::GoalChecker * goal_checker;
std::shared_ptr<MotionModel> motion_model;
std::optional<std::vector<bool>> path_pts_valid;
std::optional<size_t> furthest_reached_path_point;
};

} // namespace mppi::models
} // namespace mppi

#endif // MPPIC__CRITIC_DATA_HPP_
59 changes: 57 additions & 2 deletions include/mppic/critic_function.hpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,17 @@
// Copyright 2022 artofnothingness Alexey Budyakov, Samsung Research
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MPPIC__CRITIC_FUNCTION_HPP_
#define MPPIC__CRITIC_FUNCTION_HPP_

Expand All @@ -13,21 +26,53 @@

namespace mppi::critics
{

/**
* @class mppi::critics::CollisionCost
* @brief Utility for storing cost information
*/
struct CollisionCost
{
float cost{0};
bool using_footprint{false};
};

/**
* @class mppi::critics::CriticFunction
* @brief Abstract critic objective function to score trajectories
*/
class CriticFunction
{
public:
/**
* @brief Constructor for mppi::critics::CriticFunction
*/
CriticFunction() = default;

/**
* @brief Destructor for mppi::critics::CriticFunction
*/
virtual ~CriticFunction() = default;

/**
* @brief Configure critic on bringup
* @param parent WeakPtr to node
* @param parent_name name of the controller
* @param name Name of plugin
* @param costmap_ros Costmap2DROS object of environment
* @param dynamic_parameter_handler Parameter handler object
*/
void on_configure(
rclcpp_lifecycle::LifecycleNode::WeakPtr parent,
const std::string & parent_name,
const std::string & name,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
ParametersHandler * param_handler)
{
parent_ = parent;
logger_ = parent_.lock()->get_logger();
name_ = name;
parent_name_ = parent_name;
costmap_ros_ = costmap_ros;
costmap_ = costmap_ros_->getCostmap();
parameters_handler_ = param_handler;
Expand All @@ -38,18 +83,28 @@ class CriticFunction
initialize();
}

/**
* @brief Main function to score trajectory
* @param data Critic data to use in scoring
*/
virtual void score(CriticData & data) = 0;

/**
* @brief Initialize critic
*/
virtual void initialize() = 0;

/**
* @brief Get name of critic
*/
std::string getName()
{
return name_;
}

protected:
bool enabled_;
std::string name_;
std::string name_, parent_name_;
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
nav2_costmap_2d::Costmap2D * costmap_{nullptr};
Expand Down
Loading