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

Fix constrained-based planning / PoseModelStateSpace #2910

Merged
merged 1 commit into from
Jul 19, 2024
Merged

Fix constrained-based planning / PoseModelStateSpace #2910

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
1 change: 0 additions & 1 deletion ...erface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -145,6 +145,5 @@ class PoseModelStateSpace : public ModelBasedStateSpace
};

std::vector<PoseComponent> poses_;
double jump_factor_;
};
} // namespace ompl_interface
42 changes: 2 additions & 40 deletions ...t_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,6 @@ const std::string PoseModelStateSpace::PARAMETERIZATION_TYPE = "PoseModel";

PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec) : ModelBasedStateSpace(spec)
{
jump_factor_ = 3; // \todo make this a param

if (spec.joint_model_group_->getGroupKinematics().first)
{
poses_.emplace_back(spec.joint_model_group_, spec.joint_model_group_->getGroupKinematics().first);
Expand All @@ -82,10 +80,7 @@ PoseModelStateSpace::~PoseModelStateSpace() = default;

double PoseModelStateSpace::distance(const ompl::base::State* state1, const ompl::base::State* state2) const
{
double total = 0;
for (std::size_t i = 0; i < poses_.size(); ++i)
total += poses_[i].state_space_->distance(state1->as<StateType>()->poses[i], state2->as<StateType>()->poses[i]);
return total;
return ModelBasedStateSpace::distance(state1, state2);
}

double PoseModelStateSpace::getMaximumExtent() const
Expand Down Expand Up @@ -136,42 +131,9 @@ void PoseModelStateSpace::sanityChecks() const
void PoseModelStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t,
ompl::base::State* state) const
{
// we want to interpolate in Cartesian space; we do not have a guarantee that from and to
// have their poses computed, but this is very unlikely to happen (depends how the planner gets its input states)

// interpolate in joint space
ModelBasedStateSpace::interpolate(from, to, t, state);

// interpolate SE3 components
for (std::size_t i = 0; i < poses_.size(); ++i)
{
poses_[i].state_space_->interpolate(from->as<StateType>()->poses[i], to->as<StateType>()->poses[i], t,
state->as<StateType>()->poses[i]);
}

// the call above may reset all flags for state; but we know the pose we want flag should be set
state->as<StateType>()->setPoseComputed(true);

/*
std::cout << "*********** interpolate\n";
printState(from, std::cout);
printState(to, std::cout);
printState(state, std::cout);
std::cout << "\n\n";
*/

// after interpolation we cannot be sure about the joint values (we use them as seed only)
// so we recompute IK if needed
if (computeStateIK(state))
{
double dj = jump_factor_ * ModelBasedStateSpace::distance(from, to);
double d_from = ModelBasedStateSpace::distance(from, state);
double d_to = ModelBasedStateSpace::distance(state, to);

// if the joint value jumped too much
if (d_from + d_to > std::max(0.2, dj)) // \todo make 0.2 a param
state->as<StateType>()->markInvalid();
}
computeStateFK(state);
}

void PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ)
Expand Down
Loading