38 #include <ompl/base/spaces/SE3StateSpace.h>
45 constexpr
char LOGNAME[] =
"pose_model_state_space";
60 for (
const auto& it : m)
61 poses_.emplace_back(it.first, it.second);
65 "PoseModelStateSpace");
74 const ompl::base::State* state2)
const
77 for (std::size_t i = 0; i < poses_.size(); ++i)
85 for (
const auto& pose : poses_)
86 total += pose.state_space_->getMaximumExtent();
94 new double[variable_count_];
95 state->poses =
new ompl::base::SE3StateSpace::StateType*[poses_.size()];
96 for (std::size_t i = 0; i < poses_.size(); ++i)
97 state->poses[i] = poses_[i].state_space_->allocState()->as<ompl::base::SE3StateSpace::StateType>();
103 for (std::size_t i = 0; i < poses_.size(); ++i)
104 poses_[i].state_space_->freeState(state->as<
StateType>()->
poses[i]);
110 const ompl::base::State* source)
const
115 for (std::size_t i = 0; i < poses_.size(); ++i)
119 computeStateK(destination);
124 ModelBasedStateSpace::sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(),
125 ~ompl::base::StateSpace::STATESPACE_TRIANGLE_INEQUALITY);
129 const double t, ompl::base::State* state)
const
140 for (std::size_t i = 0; i < poses_.size(); ++i)
145 state->as<
StateType>()->setPoseComputed(
true);
157 if (computeStateIK(state))
164 if (d_from + d_to > std::max(0.2, dj))
170 double minZ,
double maxZ)
173 ompl::base::RealVectorBounds b(3);
180 for (
auto& pose : poses_)
181 pose.state_space_->as<ompl::base::SE3StateSpace>()->setBounds(b);
186 : subgroup_(subgroup), kinematics_solver_(k.allocator_(subgroup)), bijection_(k.bijection_)
188 state_space_ = std::make_shared<ompl::base::SE3StateSpace>();
198 std::vector<double>
values(bijection_.size());
199 for (
unsigned int i = 0; i < bijection_.size(); ++i)
203 std::vector<geometry_msgs::Pose> poses;
204 if (!kinematics_solver_->getPositionFK(fk_link_,
values, poses))
208 ompl::base::SE3StateSpace::StateType* se3_state = full_state->
poses[idx];
209 se3_state->setXYZ(poses[0].position.x, poses[0].position.y, poses[0].position.z);
210 ompl::base::SO3StateSpace::StateType& so3_state = se3_state->rotation();
211 so3_state.x = poses[0].orientation.x;
212 so3_state.y = poses[0].orientation.y;
213 so3_state.z = poses[0].orientation.z;
214 so3_state.w = poses[0].orientation.w;
222 std::vector<double> seed_values(bijection_.size());
223 for (std::size_t i = 0; i < bijection_.size(); ++i)
224 seed_values[i] = full_state->
values[bijection_[i]];
234 geometry_msgs::Pose pose;
235 const ompl::base::SE3StateSpace::StateType* se3_state = full_state->
poses[idx];
236 pose.position.x = se3_state->getX();
237 pose.position.y = se3_state->getY();
238 pose.position.z = se3_state->getZ();
239 const ompl::base::SO3StateSpace::StateType& so3_state = se3_state->rotation();
240 pose.orientation.x = so3_state.x;
241 pose.orientation.y = so3_state.y;
242 pose.orientation.z = so3_state.z;
243 pose.orientation.w = so3_state.w;
246 std::vector<double> solution(bijection_.size());
247 moveit_msgs::MoveItErrorCodes err_code;
248 if (!kinematics_solver_->getPositionIK(pose, seed_values, solution, err_code))
250 if (err_code.val != moveit_msgs::MoveItErrorCodes::TIMED_OUT ||
251 !kinematics_solver_->searchPositionIK(pose, seed_values, kinematics_solver_->getDefaultTimeout() * 2.0,
256 for (std::size_t i = 0; i < bijection_.size(); ++i)
257 full_state->
values[bijection_[i]] = solution[i];
266 for (std::size_t i = 0; i <
poses_.size(); ++i)
272 state->as<
StateType>()->setPoseComputed(
true);
280 for (std::size_t i = 0; i <
poses_.size(); ++i)
286 state->as<
StateType>()->setJointsComputed(
true);
304 class PoseModelStateSampler :
public ompl::base::StateSampler
307 PoseModelStateSampler(
const ompl::base::StateSpace* space, ompl::base::StateSamplerPtr sampler)
308 : ompl::base::StateSampler(space), sampler_(std::move(sampler))
312 void sampleUniform(ompl::base::State* state)
override
314 sampler_->sampleUniform(state);
315 afterStateSample(state);
318 void sampleUniformNear(ompl::base::State* state,
const ompl::base::State* near,
const double distance)
override
320 sampler_->sampleUniformNear(state, near,
distance);
321 afterStateSample(state);
324 void sampleGaussian(ompl::base::State* state,
const ompl::base::State* mean,
const double stdDev)
override
326 sampler_->sampleGaussian(state, mean, stdDev);
327 afterStateSample(state);
331 void afterStateSample(ompl::base::State* sample)
const
333 sample->as<
StateType>()->setJointsComputed(
true);
334 sample->as<
StateType>()->setPoseComputed(
false);
338 ompl::base::StateSamplerPtr sampler_;
341 return ompl::base::StateSamplerPtr(
static_cast<ompl::base::StateSampler*
>(
349 state->as<
StateType>()->setJointsComputed(
true);
350 state->as<
StateType>()->setPoseComputed(
false);