37 #ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_DISTANCE_FIELD_TYPES_ 38 #define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_DISTANCE_FIELD_TYPES_ 54 #include <visualization_msgs/MarkerArray.h> 93 std::vector<CollisionType>
types;
99 closest_distance = DBL_MAX;
101 sphere_locations.clear();
104 sphere_radii.clear();
122 bool propagate_negative_distances =
false)
123 :
distance_field::PropagationDistanceField(size.
x(), size.
y(), size.
z(), resolution, origin.
x(), origin.
y(),
124 origin.
z(), max_distance, propagate_negative_distances)
125 ,
pose_(Eigen::Affine3d::Identity())
158 double getDistanceGradient(
double x,
double y,
double z,
double& gradient_x,
double& gradient_y,
double& gradient_z,
159 bool& in_bounds)
const 161 Eigen::Vector3d rel_pos =
pose_.inverse(Eigen::Isometry) * Eigen::Vector3d(x, y, z);
164 gx, gy, gz, in_bounds);
165 Eigen::Vector3d grad =
pose_ * Eigen::Vector3d(gx, gy, gz);
166 gradient_x = grad.x();
167 gradient_y = grad.y();
168 gradient_z = grad.z();
189 const CollisionType& type,
double tolerance,
bool subtract_radii,
190 double maximum_value,
bool stop_at_first_collision);
204 const std::vector<CollisionSphere>& sphere_list,
206 const CollisionType& type,
double tolerance,
bool subtract_radii,
double maximum_value,
207 bool stop_at_first_collision);
210 const std::vector<CollisionSphere>& sphere_list,
215 const std::vector<CollisionSphere>& sphere_list,
217 double tolerance,
unsigned int num_coll, std::vector<unsigned int>& colls);
220 class BodyDecompositionVector;
224 friend class BodyDecompositionVector;
239 const Eigen::Affine3d& new_relative_cylinder_pose)
243 collision_spheres_ = new_collision_spheres;
244 relative_cylinder_pose_ = new_relative_cylinder_pose;
249 return collision_spheres_;
254 return sphere_radii_;
259 return relative_collision_points_;
264 return bodies_.getBody(i);
269 return bodies_.getCount();
274 return relative_cylinder_pose_;
279 return relative_bounding_sphere_;
304 return body_decomposition_->getCollisionSpheres();
309 return sphere_centers_;
314 return posed_collision_points_;
319 return body_decomposition_->getSphereRadii();
323 return posed_bounding_sphere_center_;
328 return body_decomposition_->getRelativeBoundingSphere().radius;
333 void updatePose(
const Eigen::Affine3d& linkTransform);
349 PosedBodyPointDecomposition(
const BodyDecompositionConstPtr& body_decomposition,
const Eigen::Affine3d& pose);
351 PosedBodyPointDecomposition(std::shared_ptr<const octomap::OcTree> octree);
355 return posed_collision_points_;
358 void updatePose(
const Eigen::Affine3d& linkTransform);
376 return collision_spheres_;
381 return posed_collision_spheres_;
386 return sphere_radii_;
391 sphere_index_map_[decomp_vector_.size()] = collision_spheres_.size();
392 decomp_vector_.push_back(bd);
393 collision_spheres_.insert(collision_spheres_.end(), bd->getCollisionSpheres().begin(),
394 bd->getCollisionSpheres().end());
395 posed_collision_spheres_.insert(posed_collision_spheres_.end(), bd->getSphereCenters().begin(),
396 bd->getSphereCenters().end());
397 sphere_radii_.insert(sphere_radii_.end(), bd->getSphereRadii().begin(), bd->getSphereRadii().end());
402 return decomp_vector_.size();
407 if (i >= decomp_vector_.size())
409 ROS_INFO_NAMED(
"collision_distance_field",
"No body decomposition");
412 return decomp_vector_[i];
415 void updatePose(
unsigned int ind,
const Eigen::Affine3d& pose)
417 if (ind >= decomp_vector_.size())
422 decomp_vector_[ind]->updatePose(pose);
423 for (
unsigned int i = 0; i < decomp_vector_[ind]->getSphereCenters().size(); i++)
425 posed_collision_spheres_[sphere_index_map_[ind] + i] = decomp_vector_[ind]->getSphereCenters()[i];
450 for (
unsigned int i = 0; i < decomp_vector_.size(); i++)
452 ret_points.insert(ret_points.end(), decomp_vector_[i]->getCollisionPoints().begin(),
453 decomp_vector_[i]->getCollisionPoints().end());
460 decomp_vector_.push_back(bd);
465 return decomp_vector_.size();
470 if (i >= decomp_vector_.size())
472 ROS_INFO_NAMED(
"collision_distance_field",
"No body decomposition");
475 return decomp_vector_[i];
478 void updatePose(
unsigned int ind,
const Eigen::Affine3d& pose)
480 if (ind < decomp_vector_.size())
482 decomp_vector_[ind]->updatePose(pose);
510 const PosedBodySphereDecompositionConstPtr& p2);
514 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
515 visualization_msgs::MarkerArray& arr);
518 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
519 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
520 const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr);
523 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
524 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
525 const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr);
const EigenSTL::vector_Vector3d & getCollisionPoints() const
void getProximityGradientMarkers(const std::string &frame_id, const std::string &ns, const ros::Duration &dur, const std::vector< PosedBodySphereDecompositionPtr > &posed_decompositions, const std::vector< PosedBodySphereDecompositionVectorPtr > &posed_vector_decompositions, const std::vector< GradientInfo > &gradients, visualization_msgs::MarkerArray &arr)
std::map< unsigned int, unsigned int > sphere_index_map_
std::vector< CollisionSphere > determineCollisionSpheres(const bodies::Body *body, Eigen::Affine3d &relativeTransform)
#define ROS_INFO_NAMED(name,...)
const EigenSTL::vector_Vector3d & getSphereCenters() const
PosedBodySphereDecompositionConstPtr empty_ptr_
void updatePose(const Eigen::Affine3d &transform)
bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr &p1, const PosedBodySphereDecompositionConstPtr &p2)
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
EigenSTL::vector_Vector3d relative_collision_points_
#define ROS_WARN_NAMED(name,...)
const bodies::Body * getBody(unsigned int i) const
unsigned int sphere_index
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
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)
void init(const M_string &remappings)
const std::vector< double > & getSphereRadii() const
void updatePose(unsigned int ind, const Eigen::Affine3d &pose)
std::vector< PosedBodyPointDecompositionPtr > decomp_vector_
const bodies::BoundingSphere & getRelativeBoundingSphere() const
Eigen::Affine3d getRelativeCylinderPose() const
Eigen::Vector3d closest_gradient
EigenSTL::vector_Vector3d sphere_centers_
BodyDecompositionConstPtr body_decomposition_
const std::vector< CollisionSphere > & getCollisionSpheres() const
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
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...
void replaceCollisionSpheres(const std::vector< CollisionSphere > &new_collision_spheres, const Eigen::Affine3d &new_relative_cylinder_pose)
EigenSTL::vector_Vector3d getCollisionPoints() const
EigenSTL::vector_Vector3d posed_collision_points_
const EigenSTL::vector_Vector3d & getCollisionPoints() const
EigenSTL::vector_Vector3d gradients
const std::vector< double > & getSphereRadii() const
EigenSTL::vector_Vector3d sphere_locations
Eigen::Vector3d closest_point
Generic interface to collision detection.
std::vector< CollisionSphere > collision_spheres_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedDistanceField(const Eigen::Vector3d &size, const Eigen::Vector3d &origin, double resolution, double max_distance, bool propagate_negative_distances=false)
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...
double getBoundingSphereRadius() const
std::vector< CollisionType > types
DistanceField is an abstract base class for computing distances from sets of 3D obstacle points...
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double closest_distance
static const double resolution
PosedBodyPointDecompositionPtr empty_ptr_
const std::vector< CollisionSphere > & getCollisionSpheres() const
const EigenSTL::vector_Vector3d & getCollisionPoints() const
std::vector< double > sphere_radii
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
std::vector< double > sphere_radii_
std::vector< double > distances
void getCollisionSphereMarkers(const std_msgs::ColorRGBA &color, const std::string &frame_id, const std::string &ns, const ros::Duration &dur, const std::vector< PosedBodySphereDecompositionPtr > &posed_decompositions, visualization_msgs::MarkerArray &arr)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string link_name
unsigned int getSize() const
std::vector< CollisionSphere > collision_spheres_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d relative_vec_
void addToVector(PosedBodyPointDecompositionPtr &bd)
PosedBodySphereDecompositionConstPtr getPosedBodySphereDecomposition(unsigned int i) const
const std::vector< double > & getSphereRadii() const
Namespace for holding classes that generate distance fields.
EigenSTL::vector_Vector3d posed_collision_points_
Eigen::Vector3d posed_bounding_sphere_center_
unsigned int getBodiesCount()
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodyPointDecompositionVector()
void updatePose(unsigned int ind, const Eigen::Affine3d &pose)
const geometry_msgs::Pose * pose_
BodyDecompositionConstPtr body_decomposition_
void addToVector(PosedBodySphereDecompositionPtr &bd)
unsigned int getSize() const
std::vector< PosedBodySphereDecompositionPtr > decomp_vector_
const Eigen::Affine3d & getPose() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodySphereDecompositionVector()
EigenSTL::vector_Vector3d posed_collision_spheres_
Eigen::Affine3d relative_cylinder_pose_
PosedBodyPointDecompositionConstPtr getPosedBodyDecomposition(unsigned int i) const
bodies::BodyVector bodies_
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)
std::string attached_object_name
const std::vector< CollisionSphere > & getCollisionSpheres() const
bodies::BoundingSphere relative_bounding_sphere_
std::vector< double > sphere_radii_
const Eigen::Vector3d & getBoundingSphereCenter() const
void getCollisionMarkers(const std::string &frame_id, const std::string &ns, const ros::Duration &dur, const std::vector< PosedBodySphereDecompositionPtr > &posed_decompositions, const std::vector< PosedBodySphereDecompositionVectorPtr > &posed_vector_decompositions, const std::vector< GradientInfo > &gradients, visualization_msgs::MarkerArray &arr)
CollisionSphere(const Eigen::Vector3d &rel, double radius)
std::shared_ptr< const Shape > ShapeConstPtr
const EigenSTL::vector_Vector3d & getSphereCenters() const