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 continuous (unbounded) joints properly #59

Merged
merged 4 commits into from
Nov 15, 2023
Merged

Support continuous (unbounded) joints properly #59

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
95bf319
Support continuous (unbounded) joints properly
sea-bass Sep 29, 2023
c5051b5
Bugfixes from testing with Kinova Gen3
sea-bass Sep 29, 2023
6c3f99f
Remove unnecessary copies
sea-bass Oct 2, 2023
9cf7eb7
Abstract variable bounds functions into variable, fix memetic initial…
sea-bass Nov 13, 2023
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
15 changes: 10 additions & 5 deletions include/pick_ik/robot.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,15 @@ namespace pick_ik {

struct Robot {
struct Variable {
double clip_min, clip_max;
/// @brief Min and max position values of the variable.
double min, max;

/// @brief Whether the variable's position is bounded.
bool bounded;

/// @brief The span (min - max) of the variable, or a default value if unbounded.
double span;
double min;
double max;

double max_velocity_rcp;
double minimal_displacement_factor;
};
Expand All @@ -27,8 +32,8 @@ struct Robot {
moveit::core::JointModelGroup const* jmg,
std::vector<size_t> tip_link_indices) -> Robot;

/** @brief Returns a random valid configuration. */
auto get_random_valid_configuration() const -> std::vector<double>;
/** @brief Sets a variable vector to a random valid configuration. */
sea-bass marked this conversation as resolved.
Show resolved Hide resolved
auto set_random_valid_configuration(std::vector<double>& config) const -> void;

/** @brief Check is a configuration is valid. */
auto is_valid_configuration(std::vector<double> const& config) const -> bool;
Expand Down
4 changes: 2 additions & 2 deletions src/goal.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ auto make_center_joints_cost_fn(Robot robot) -> CostFn {
assert(robot.variables.size() == active_positions.size());
for (size_t i = 0; i < active_positions.size(); ++i) {
auto const& variable = robot.variables[i];
if (variable.clip_max == std::numeric_limits<double>::max()) {
if (!variable.bounded) {
continue;
}

Expand All @@ -113,7 +113,7 @@ auto make_avoid_joint_limits_cost_fn(Robot robot) -> CostFn {
assert(robot.variables.size() == active_positions.size());
for (size_t i = 0; i < active_positions.size(); ++i) {
auto const& variable = robot.variables[i];
if (variable.clip_max == std::numeric_limits<double>::max()) {
if (!variable.bounded) {
continue;
}

Expand Down
15 changes: 9 additions & 6 deletions src/ik_gradient.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@ auto step(GradientIk& self, Robot const& robot, CostFn const& cost_fn, double st
auto const count = self.local.size();

// compute gradient direction
self.working = self.local;
for (size_t i = 0; i < count; ++i) {
// test negative displacement
self.working[i] = self.local[i] - step_size;
Expand Down Expand Up @@ -55,8 +54,6 @@ auto step(GradientIk& self, Robot const& robot, CostFn const& cost_fn, double st
[&](auto value) { return value * f; });

// initialize line search
self.working = self.local;

for (size_t i = 0; i < count; ++i) {
self.working[i] = self.local[i] - self.gradient[i];
}
Expand All @@ -78,9 +75,15 @@ auto step(GradientIk& self, Robot const& robot, CostFn const& cost_fn, double st
// apply optimization step
// (move along gradient direction by estimated step size)
for (size_t i = 0; i < count; ++i) {
self.working[i] = std::clamp(self.local[i] - self.gradient[i] * joint_diff,
robot.variables[i].clip_min,
robot.variables[i].clip_max);
auto const& var = robot.variables[i];
auto updated_value = self.local[i] - self.gradient[i] * joint_diff;
if (var.bounded) {
self.working[i] = std::clamp(updated_value, var.min, var.max);
} else {
self.working[i] = std::clamp(updated_value,
self.local[i] - var.span / 2.0,
self.local[i] + var.span / 2.0);
}
}

// Always accept the solution and continue
Expand Down
15 changes: 12 additions & 3 deletions src/ik_memetic.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,10 @@ void MemeticIk::initPopulation(Robot const& robot,
std::vector<double> const zero_grad(robot.variables.size(), 0.0);
population_.resize(params_.population_size);
for (size_t i = 0; i < params_.elite_size; ++i) {
auto const genotype = (i == 0) ? initial_guess : robot.get_random_valid_configuration();
auto genotype = initial_guess;
if (i > 0) {
robot.set_random_valid_configuration(genotype);
}
population_[i] = Individual{genotype, cost_fn(genotype), 1.0, zero_grad};
}

Expand Down Expand Up @@ -153,7 +156,13 @@ void MemeticIk::reproduce(Robot const& robot, CostFn const& cost_fn) {
}

// Clamp to valid joint values
gene = std::clamp(gene, joint.clip_min, joint.clip_max);
if (joint.bounded) {
gene = std::clamp(gene, joint.min, joint.max);
} else {
gene = std::clamp(gene,
original_gene - joint.span / 2.0,
original_gene + joint.span / 2.0);
}

// Approximate gradient
population_[i].gradient[j_idx] = gene - original_gene;
Expand All @@ -173,7 +182,7 @@ void MemeticIk::reproduce(Robot const& robot, CostFn const& cost_fn) {

} else {
// If the mating pool is empty, roll a new population member randomly.
population_[i].genes = robot.get_random_valid_configuration();
robot.set_random_valid_configuration(population_[i].genes);
population_[i].fitness = cost_fn(population_[i].genes);
for (auto& g : population_[i].gradient) {
g = 0.0;
Expand Down
8 changes: 2 additions & 6 deletions src/pick_ik_plugin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -179,7 +179,7 @@ class PickIKPlugin : public kinematics::KinematicsBase {
RCLCPP_WARN(
LOGGER,
"Initial guess exceeds joint limits. Regenerating a random valid configuration.");
init_state = robot_.get_random_valid_configuration();
robot_.set_random_valid_configuration(init_state);
}

// Optimize until a valid solution is found or we have timed out.
Expand Down Expand Up @@ -309,15 +309,11 @@ class PickIKPlugin : public kinematics::KinematicsBase {
if (found_valid_solution || timeout_elapsed) {
done_optimizing = true;
} else {
init_state = robot_.get_random_valid_configuration();
robot_.set_random_valid_configuration(init_state);
remaining_timeout -= total_optim_time.count();
}
}

if (!robot_.is_valid_configuration(solution)) {
std::cout << "INVALID SOLUTION!" << std::endl;
}

return found_valid_solution;
}

Expand Down
37 changes: 22 additions & 15 deletions src/robot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,11 @@
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

namespace {
constexpr double kUnboundedVariableSpan = 2.0 * M_PI;
constexpr double kUnboundedJointSampleSpread = M_PI;
} // namespace

namespace pick_ik {

auto Robot::from(std::shared_ptr<moveit::core::RobotModel const> const& model,
Expand All @@ -33,17 +38,11 @@ auto Robot::from(std::shared_ptr<moveit::core::RobotModel const> const& model,

auto var = Variable{};

bool bounded = bounds.position_bounded_;

var.bounded = bounds.position_bounded_;
var.min = bounds.min_position_;
var.max = bounds.max_position_;

var.clip_min = bounded ? var.min : std::numeric_limits<double>::lowest();
var.clip_max = bounded ? var.max : std::numeric_limits<double>::max();

var.span = var.max - var.min;

if (!(var.span >= 0 && var.span < std::numeric_limits<double>::max())) var.span = 1;
var.span = var.bounded ? var.max - var.min : kUnboundedVariableSpan;
Copy link
Contributor

Choose a reason for hiding this comment

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

Should var.span just be a function?

Copy link
Collaborator Author

@sea-bass sea-bass Nov 10, 2023 •
edited
Loading

Choose a reason for hiding this comment

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

I think we want this to be a variable so it doesn't have to be needlessly recalculated multiple times... but with your next comment, I like the idea of providing abstractions to clip variables to limits / generate random values, and then no other code besides these functions would touch the span member variable.


auto const max_velocity = bounds.max_velocity_;
var.max_velocity_rcp = max_velocity > 0.0 ? 1.0 / max_velocity : 0.0;
Expand All @@ -64,22 +63,30 @@ auto Robot::from(std::shared_ptr<moveit::core::RobotModel const> const& model,
return robot;
}

auto Robot::get_random_valid_configuration() const -> std::vector<double> {
std::vector<double> config;
auto Robot::set_random_valid_configuration(std::vector<double>& config) const -> void {
auto const num_vars = variables.size();
config.reserve(num_vars);
if (config.size() != num_vars) {
config.resize(num_vars);
}
for (size_t idx = 0; idx < num_vars; ++idx) {
auto const var = variables[idx];
config.push_back(rsl::uniform_real(var.clip_min, var.clip_max));
auto const& var = variables[idx];
if (var.bounded) {
sea-bass marked this conversation as resolved.
Show resolved Hide resolved
config[idx] = rsl::uniform_real(var.min, var.max);
} else {
config[idx] = rsl::uniform_real(config[idx] - kUnboundedJointSampleSpread,
config[idx] + kUnboundedJointSampleSpread);
}
}
return config;
}

auto Robot::is_valid_configuration(std::vector<double> const& config) const -> bool {
auto const num_vars = variables.size();
for (size_t idx = 0; idx < num_vars; ++idx) {
auto const var = variables[idx];
if (config[idx] > var.clip_max || config[idx] < var.clip_min) {
if (!var.bounded) {
continue;
}
if (config[idx] > var.max || config[idx] < var.min) {
return false;
}
}
Expand Down