Go to the documentation of this file.
52 #include <visualization_msgs/MarkerArray.h>
65 struct CollisionSphere
72 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
84 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
91 std::vector<CollisionType>
types;
117 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
120 bool propagate_negative_distances =
false)
122 origin.
z(), max_distance, propagate_negative_distances)
132 const Eigen::Isometry3d&
getPose()
const
156 double getDistanceGradient(
double x,
double y,
double z,
double& gradient_x,
double& gradient_y,
double& gradient_z,
157 bool& in_bounds)
const
162 gx, gy, gz, in_bounds);
164 gradient_x = grad.x();
165 gradient_y = grad.y();
166 gradient_z = grad.z();
187 const CollisionType& type,
double tolerance,
bool subtract_radii,
188 double maximum_value,
bool stop_at_first_collision);
191 Eigen::Isometry3d
pose_;
202 const std::vector<CollisionSphere>& sphere_list,
204 const CollisionType& type,
double tolerance,
bool subtract_radii,
double maximum_value,
205 bool stop_at_first_collision);
208 const std::vector<CollisionSphere>& sphere_list,
213 const std::vector<CollisionSphere>& sphere_list,
215 double tolerance,
unsigned int num_coll, std::vector<unsigned int>& colls);
218 class BodyDecompositionVector;
225 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
230 double resolution,
double padding);
237 const Eigen::Isometry3d& new_relative_cylinder_pose)
282 double resolution,
double padding);
296 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
331 void updatePose(
const Eigen::Isometry3d& linkTransform);
343 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
366 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
387 void addToVector(PosedBodySphereDecompositionPtr& bd)
392 bd->getCollisionSpheres().end());
394 bd->getSphereCenters().end());
407 ROS_INFO_NAMED(
"collision_distance_field",
"No body decomposition");
413 void updatePose(
unsigned int ind,
const Eigen::Isometry3d& pose)
421 for (
unsigned int i = 0; i <
decomp_vector_[ind]->getSphereCenters().size(); i++)
428 PosedBodySphereDecompositionConstPtr
empty_ptr_;
439 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
448 for (
const PosedBodyPointDecompositionPtr& decomp :
decomp_vector_)
450 ret_points.insert(ret_points.end(), decomp->getCollisionPoints().begin(), decomp->getCollisionPoints().end());
455 void addToVector(PosedBodyPointDecompositionPtr& bd)
469 ROS_INFO_NAMED(
"collision_distance_field",
"No body decomposition");
475 void updatePose(
unsigned int ind,
const Eigen::Isometry3d& pose)
495 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
507 const PosedBodySphereDecompositionConstPtr& p2);
511 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
512 visualization_msgs::MarkerArray& arr);
515 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
516 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
517 const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr);
520 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
521 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
522 const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr);
BodyDecompositionConstPtr body_decomposition_
void updatePose(const Eigen::Isometry3d &transform)
const std::vector< double > & getSphereRadii() const
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)
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...
bodies::BodyVector bodies_
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
EigenSTL::vector_Vector3d getCollisionPoints() const
unsigned int getSize() const
std::vector< PosedBodySphereDecompositionPtr > decomp_vector_
std::vector< CollisionType > types
std::vector< CollisionSphere > determineCollisionSpheres(const bodies::Body *body, Eigen::Isometry3d &relativeTransform)
EigenSTL::vector_Vector3d gradients
const std::vector< CollisionSphere > & getCollisionSpheres() const
EigenSTL::vector_Vector3d posed_collision_spheres_
A DistanceField implementation that uses a vector propagation method. Distances propagate outward fro...
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
void addToVector(PosedBodyPointDecompositionPtr &bd)
std::vector< CollisionSphere > collision_spheres_
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...
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d relative_vec_
std::vector< double > sphere_radii
bodies::BoundingSphere relative_bounding_sphere_
const std::vector< CollisionSphere > & getCollisionSpheres() const
std::vector< double > sphere_radii_
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...
Eigen::Isometry3d getRelativeCylinderPose() const
friend class BodyDecompositionVector
Eigen::Vector3d posed_bounding_sphere_center_
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::vector< CollisionSphere > collision_spheres_
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
EigenSTL::vector_Vector3d relative_collision_points_
const EigenSTL::vector_Vector3d & getCollisionPoints() const
const std::vector< double > & getSphereRadii() const
bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr &p1, const PosedBodySphereDecompositionConstPtr &p2)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodyPointDecomposition(const BodyDecompositionConstPtr &body_decomposition)
const Body * getBody(unsigned int i) const
void updatePose(const Eigen::Isometry3d &linkTransform)
EigenSTL::vector_Vector3d posed_collision_points_
const std::vector< CollisionSphere > & getCollisionSpheres() const
std::map< unsigned int, unsigned int > sphere_index_map_
Eigen::Vector3d closest_point
void addToVector(PosedBodySphereDecompositionPtr &bd)
CollisionSphere(const Eigen::Vector3d &rel, double radius)
DistanceField is an abstract base class for computing distances from sets of 3D obstacle points....
PosedBodySphereDecompositionConstPtr getPosedBodySphereDecomposition(unsigned int i) const
#define ROS_INFO_NAMED(name,...)
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)
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)
void replaceCollisionSpheres(const std::vector< CollisionSphere > &new_collision_spheres, const Eigen::Isometry3d &new_relative_cylinder_pose)
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)
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
PosedBodyPointDecompositionConstPtr getPosedBodyDecomposition(unsigned int i) const
double getBoundingSphereRadius() const
void updatePose(const Eigen::Isometry3d &linkTransform)
unsigned int getBodiesCount()
EigenSTL::vector_Vector3d sphere_locations
void updatePose(unsigned int ind, const Eigen::Isometry3d &pose)
void init(const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &poses, double resolution, double padding)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodySphereDecomposition(const BodyDecompositionConstPtr &body_decomposition)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodySphereDecompositionVector()
std::size_t getCount() const
const bodies::Body * getBody(unsigned int i) const
Namespace for holding classes that generate distance fields.
std::vector< PosedBodyPointDecompositionPtr > decomp_vector_
unsigned int sphere_index
BodyDecompositionConstPtr body_decomposition_
std::string attached_object_name
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)
Eigen::Isometry3d relative_cylinder_pose_
const Eigen::Vector3d & getBoundingSphereCenter() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BodyDecomposition(const shapes::ShapeConstPtr &shape, double resolution, double padding=0.01)
void updatePose(unsigned int ind, const Eigen::Isometry3d &pose)
PosedBodySphereDecompositionConstPtr empty_ptr_
EigenSTL::vector_Vector3d posed_collision_points_
const bodies::BoundingSphere & getRelativeBoundingSphere() const
EigenSTL::vector_Vector3d sphere_centers_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodyPointDecompositionVector()
#define ROS_WARN_NAMED(name,...)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedDistanceField(const Eigen::Vector3d &size, const Eigen::Vector3d &origin, double resolution, double max_distance, bool propagate_negative_distances=false)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string link_name
Eigen::Vector3d closest_gradient
std::shared_ptr< const Shape > ShapeConstPtr
const EigenSTL::vector_Vector3d & getCollisionPoints() const
const std::vector< double > & getSphereRadii() const
PosedBodyPointDecompositionPtr empty_ptr_
const EigenSTL::vector_Vector3d & getSphereCenters() const
std::vector< double > sphere_radii_
const Eigen::Isometry3d & getPose() const
unsigned int getSize() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double closest_distance
const EigenSTL::vector_Vector3d & getSphereCenters() const
std::vector< double > distances
Vec3fX< details::Vec3Data< double > > Vector3d
const EigenSTL::vector_Vector3d & getCollisionPoints() const
moveit_core
Author(s): Ioan Sucan
, Sachin Chitta , Acorn Pooley
autogenerated on Thu Apr 18 2024 02:23:40