45#include <geometric_shapes/shapes.h>
46#include <geometric_shapes/bodies.h>
47#include <octomap/OcTree.h>
52#include <visualization_msgs/msg/marker_array.hpp>
53#include <rclcpp/duration.hpp>
54#include <rclcpp/logging.hpp>
55#include <rclcpp/clock.hpp>
56#include <rclcpp/rclcpp.hpp>
57#include <rclcpp/time.hpp>
58#include <rclcpp/utilities.hpp>
97 std::vector<CollisionType>
types;
126 PosedDistanceField(
const Eigen::Vector3d& size,
const Eigen::Vector3d& origin,
double resolution,
double max_distance,
127 bool propagate_negative_distances =
false)
129 origin.z(), max_distance, propagate_negative_distances)
130 ,
pose_(Eigen::Isometry3d::Identity())
163 double getDistanceGradient(
double x,
double y,
double z,
double& gradient_x,
double& gradient_y,
double& gradient_z,
164 bool& in_bounds)
const
167 Eigen::Vector3d rel_pos =
pose_.linear().transpose() * Eigen::Vector3d(x, y, z);
170 gx, gy, gz, in_bounds);
171 Eigen::Vector3d grad =
pose_ * Eigen::Vector3d(gx, gy, gz);
172 gradient_x = grad.x();
173 gradient_y = grad.y();
174 gradient_z = grad.z();
194 const EigenSTL::vector_Vector3d& sphere_centers,
GradientInfo& gradient,
195 const CollisionType& type,
double tolerance,
bool subtract_radii,
196 double maximum_value,
bool stop_at_first_collision);
210 const std::vector<CollisionSphere>& sphere_list,
211 const EigenSTL::vector_Vector3d& sphere_centers,
GradientInfo& gradient,
212 const CollisionType& type,
double tolerance,
bool subtract_radii,
double maximum_value,
213 bool stop_at_first_collision);
216 const std::vector<CollisionSphere>& sphere_list,
217 const EigenSTL::vector_Vector3d& sphere_centers,
double maximum_value,
221 const std::vector<CollisionSphere>& sphere_list,
222 const EigenSTL::vector_Vector3d& sphere_centers,
double maximum_value,
223 double tolerance,
unsigned int num_coll, std::vector<unsigned int>& colls);
226class BodyDecompositionVector;
235 BodyDecomposition(
const shapes::ShapeConstPtr& shape,
double resolution,
double padding = 0.01);
238 double resolution,
double padding);
245 const Eigen::Isometry3d& new_relative_cylinder_pose)
268 const bodies::Body*
getBody(
unsigned int i)
const
289 void init(
const std::vector<shapes::ShapeConstPtr>&
shapes,
const EigenSTL::vector_Isometry3d& poses,
290 double resolution,
double padding);
339 void updatePose(
const Eigen::Isometry3d& linkTransform);
364 void updatePose(
const Eigen::Isometry3d& linkTransform);
377 : logger_(
moveit::getLogger(
"moveit.core.posed_body_sphere_decomposition_vector"))
383 return collision_spheres_;
388 return posed_collision_spheres_;
393 return sphere_radii_;
398 sphere_index_map_[decomp_vector_.size()] = collision_spheres_.size();
399 decomp_vector_.push_back(bd);
400 collision_spheres_.insert(collision_spheres_.end(), bd->getCollisionSpheres().begin(),
401 bd->getCollisionSpheres().end());
402 posed_collision_spheres_.insert(posed_collision_spheres_.end(), bd->getSphereCenters().begin(),
403 bd->getSphereCenters().end());
404 sphere_radii_.insert(sphere_radii_.end(), bd->getSphereRadii().begin(), bd->getSphereRadii().end());
409 return decomp_vector_.size();
414 if (i >= decomp_vector_.size())
416 RCLCPP_INFO(logger_,
"No body decomposition");
419 return decomp_vector_[i];
422 void updatePose(
unsigned int ind,
const Eigen::Isometry3d& pose)
424 if (ind >= decomp_vector_.size())
426 RCLCPP_WARN(logger_,
"Can't update pose");
429 decomp_vector_[ind]->updatePose(pose);
430 for (
unsigned int i = 0; i < decomp_vector_[ind]->getSphereCenters().size(); ++i)
432 posed_collision_spheres_[sphere_index_map_[ind] + i] = decomp_vector_[ind]->getSphereCenters()[i];
437 rclcpp::Logger logger_;
438 PosedBodySphereDecompositionConstPtr empty_ptr_;
439 std::vector<PosedBodySphereDecompositionPtr> decomp_vector_;
440 std::vector<CollisionSphere> collision_spheres_;
441 EigenSTL::vector_Vector3d posed_collision_spheres_;
442 std::vector<double> sphere_radii_;
443 std::map<unsigned int, unsigned int> sphere_index_map_;
457 EigenSTL::vector_Vector3d ret_points;
458 for (
const PosedBodyPointDecompositionPtr& decomp : decomp_vector_)
460 ret_points.insert(ret_points.end(), decomp->getCollisionPoints().begin(), decomp->getCollisionPoints().end());
467 decomp_vector_.push_back(bd);
472 return decomp_vector_.size();
477 if (i >= decomp_vector_.size())
479 RCLCPP_INFO(
logger_,
"No body decomposition");
482 return decomp_vector_[i];
485 void updatePose(
unsigned int ind,
const Eigen::Isometry3d& pose)
487 if (ind < decomp_vector_.size())
489 decomp_vector_[ind]->updatePose(pose);
493 RCLCPP_WARN(
logger_,
"Can't update pose");
502 PosedBodyPointDecompositionPtr empty_ptr_;
503 std::vector<PosedBodyPointDecompositionPtr> decomp_vector_;
520 const PosedBodySphereDecompositionConstPtr& p2);
523 const std::string& ns,
const rclcpp::Duration& dur,
524 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
525 visualization_msgs::msg::MarkerArray& arr);
528 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
529 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
530 const std::vector<GradientInfo>& gradients, visualization_msgs::msg::MarkerArray& arr);
532void getCollisionMarkers(
const std::string& frame_id,
const std::string& ns,
const rclcpp::Duration& dur,
533 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
534 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
535 const std::vector<GradientInfo>& gradients, visualization_msgs::msg::MarkerArray& arr);
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
#define MOVEIT_CLASS_FORWARD(C)
const EigenSTL::vector_Vector3d & getCollisionPoints() const
const bodies::BoundingSphere & getRelativeBoundingSphere() const
std::vector< CollisionSphere > collision_spheres_
void init(const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &poses, double resolution, double padding)
bodies::BoundingSphere relative_bounding_sphere_
friend class BodyDecompositionVector
const bodies::Body * getBody(unsigned int i) const
Eigen::Isometry3d relative_cylinder_pose_
EigenSTL::vector_Vector3d relative_collision_points_
const std::vector< double > & getSphereRadii() const
unsigned int getBodiesCount()
Eigen::Isometry3d getRelativeCylinderPose() const
const std::vector< CollisionSphere > & getCollisionSpheres() const
bodies::BodyVector bodies_
void replaceCollisionSpheres(const std::vector< CollisionSphere > &new_collision_spheres, const Eigen::Isometry3d &new_relative_cylinder_pose)
std::vector< double > sphere_radii_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodyPointDecompositionVector()
void addToVector(PosedBodyPointDecompositionPtr &bd)
PosedBodyPointDecompositionConstPtr getPosedBodyDecomposition(unsigned int i) const
unsigned int getSize() const
void updatePose(unsigned int ind, const Eigen::Isometry3d &pose)
EigenSTL::vector_Vector3d getCollisionPoints() const
void updatePose(const Eigen::Isometry3d &linkTransform)
const EigenSTL::vector_Vector3d & getCollisionPoints() const
EigenSTL::vector_Vector3d posed_collision_points_
BodyDecompositionConstPtr body_decomposition_
const std::vector< double > & getSphereRadii() const
void addToVector(PosedBodySphereDecompositionPtr &bd)
unsigned int getSize() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodySphereDecompositionVector()
void updatePose(unsigned int ind, const Eigen::Isometry3d &pose)
const std::vector< CollisionSphere > & getCollisionSpheres() const
PosedBodySphereDecompositionConstPtr getPosedBodySphereDecomposition(unsigned int i) const
const EigenSTL::vector_Vector3d & getSphereCenters() const
const Eigen::Vector3d & getBoundingSphereCenter() const
Eigen::Vector3d posed_bounding_sphere_center_
const EigenSTL::vector_Vector3d & getCollisionPoints() const
const std::vector< double > & getSphereRadii() const
EigenSTL::vector_Vector3d posed_collision_points_
BodyDecompositionConstPtr body_decomposition_
void updatePose(const Eigen::Isometry3d &linkTransform)
const EigenSTL::vector_Vector3d & getSphereCenters() const
const std::vector< CollisionSphere > & getCollisionSpheres() const
EigenSTL::vector_Vector3d sphere_centers_
double getBoundingSphereRadius() const
const Eigen::Isometry3d & getPose() const
double getDistanceGradient(double x, double y, double z, double &gradient_x, double &gradient_y, double &gradient_z, bool &in_bounds) const
Gets not only the distance to the nearest cell but the gradient direction. The point defined by the x...
void updatePose(const Eigen::Isometry3d &transform)
bool getCollisionSphereGradients(const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, GradientInfo &gradient, const CollisionType &type, double tolerance, bool subtract_radii, double maximum_value, bool stop_at_first_collision)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedDistanceField(const Eigen::Vector3d &size, const Eigen::Vector3d &origin, double resolution, double max_distance, bool propagate_negative_distances=false)
DistanceField is an abstract base class for computing distances from sets of 3D obstacle points....
double getDistanceGradient(double x, double y, double z, double &gradient_x, double &gradient_y, double &gradient_z, bool &in_bounds) const
Gets not only the distance to the nearest cell but the gradient direction. The gradient is computed a...
A DistanceField implementation that uses a vector propagation method. Distances propagate outward fro...
PropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z, double max_distance, bool propagate_negative_distances=false)
Constructor that initializes entire distance field to empty - all cells will be assigned maximum dist...
void getProximityGradientMarkers(const std::string &frame_id, const std::string &ns, const rclcpp::Duration &dur, const std::vector< PosedBodySphereDecompositionPtr > &posed_decompositions, const std::vector< PosedBodySphereDecompositionVectorPtr > &posed_vector_decompositions, const std::vector< GradientInfo > &gradients, visualization_msgs::msg::MarkerArray &arr)
std::vector< CollisionSphere > determineCollisionSpheres(const bodies::Body *body, Eigen::Isometry3d &relativeTransform)
void getCollisionMarkers(const std::string &frame_id, const std::string &ns, const rclcpp::Duration &dur, const std::vector< PosedBodySphereDecompositionPtr > &posed_decompositions, const std::vector< PosedBodySphereDecompositionVectorPtr > &posed_vector_decompositions, const std::vector< GradientInfo > &gradients, visualization_msgs::msg::MarkerArray &arr)
void getCollisionSphereMarkers(const std_msgs::msg::ColorRGBA &color, const std::string &frame_id, const std::string &ns, const rclcpp::Duration &dur, const std::vector< PosedBodySphereDecompositionPtr > &posed_decompositions, visualization_msgs::msg::MarkerArray &arr)
bool getCollisionSphereCollision(const distance_field::DistanceField *distance_field, const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, double maximum_value, double tolerance)
bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr &p1, const PosedBodySphereDecompositionConstPtr &p2)
bool getCollisionSphereGradients(const distance_field::DistanceField *distance_field, const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, GradientInfo &gradient, const CollisionType &type, double tolerance, bool subtract_radii, double maximum_value, bool stop_at_first_collision)
Namespace for holding classes that generate distance fields.
Main namespace for MoveIt.
CollisionSphere(const Eigen::Vector3d &rel, double radius)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d relative_vec_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double closest_distance
EigenSTL::vector_Vector3d gradients
std::vector< double > distances
std::vector< double > sphere_radii
EigenSTL::vector_Vector3d sphere_locations
std::vector< CollisionType > types
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string link_name
Eigen::Vector3d closest_gradient
std::string attached_object_name
unsigned int sphere_index
Eigen::Vector3d closest_point