Skip to content

Commit

Permalink
PR Review:
Browse files Browse the repository at this point in the history 
  * Adaption of reported continuous results
  * Removed parent classes of collision managers
  * Removed typedef for ObjectKeyPair
  * Changed member name in CastHullShape
  * clang-format
  * removing unnecessary asserts
  • Loading branch information
@j-petit
j-petit committed Jul 4, 2019
1 parent ba0552f commit 3ab813e
Show file tree
Hide file tree
Showing 16 changed files with 385 additions and 531 deletions.
7 changes: 2 additions & 5 deletions moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,14 +104,11 @@ struct Contact
/** \brief The type of the second body involved in the contact */
BodyType body_type_2;

/** \brief The time until collision */
double cc_time;
/** \brief The distance percentage between casted poses until collision */
double percent_interpolation;

/** \brief The continous collision type */
ContinuousCollisionType cc_type;

Eigen::Vector3d nearest_points[2];
Eigen::Vector3d cc_nearest_points[2];
};

/** \brief When collision costs are computed, this structure contains information about the partial cost incurred in a
Expand Down
3 changes: 1 addition & 2 deletions ...collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,8 +90,7 @@ class CollisionRobotBt : public CollisionRobot

void updatedPaddingOrScaling(const std::vector<std::string>& links) override;

void addAttachedOjects(const robot_state::RobotState& state,
std::vector<tesseract::COWPtr>& cows) const;
void addAttachedOjects(const robot_state::RobotState& state, std::vector<tesseract::COWPtr>& cows) const;

void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
const AllowedCollisionMatrix* acm) const;
Expand Down
195 changes: 162 additions & 33 deletions ...tion_bullet/include/moveit/collision_detection_bullet/tesseract/bullet_cast_bvh_manager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,81 +43,211 @@
#define TESSERACT_COLLISION_BULLET_CAST_BVH_MANAGERS_H

#include <moveit/collision_detection_bullet/tesseract/bullet_utils.h>
#include <moveit/collision_detection_bullet/tesseract/continuous_contact_manager_base.h>
#include <moveit/macros/class_forward.h>

namespace tesseract
{
MOVEIT_CLASS_FORWARD(BulletCastBVHManager)

/** @brief A BVH implementaiton of a tesseract contact manager */
class BulletCastBVHManager : public ContinuousContactManagerBase
class BulletCastBVHManager
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

/** \brief Constructor */
BulletCastBVHManager();
~BulletCastBVHManager() override;

ContinuousContactManagerBasePtr clone() const override;
~BulletCastBVHManager();

/**
* @brief Clone the manager
*
* This is to be used for multi threaded application. A user should
* make a cone for each thread.
*/
BulletCastBVHManagerPtr clone() const;

/**
* @brief Add a collision object to the checker
*
* All objects are added should initially be added as static objects. Use the
* setContactRequest method of defining which collision objects are moving.
*
* @param name The name of the object, must be unique.
* @param mask_id User defined id which gets stored in the results structure.
* @param shapes A vector of shapes that make up the collision object.
* @param shape_poses A vector of poses for each shape, must be same length as shapes
* @param shape_types A vector of shape types for encode the collision object. If the vector is of length 1 it is
* used for all shapes.
* @param collision_object_types A int identifying a conversion mode for the object. (ex. convert meshes to
* convex_hulls)
* @param enabled Indicate if the object is enabled for collision checking.
* @return true if successfully added, otherwise false.
*/
bool addCollisionObject(const std::string& name, const collision_detection::BodyType& mask_id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const AlignedVector<Eigen::Isometry3d>& shape_poses,
const std::vector<CollisionObjectType>& collision_object_types, bool enabled = true) override;
const std::vector<CollisionObjectType>& collision_object_types, bool enabled = true);

bool hasCollisionObject(const std::string& name) const override;
/**
* @brief Find if a collision object already exists
* @param name The name of the collision object
* @return true if it exists, otherwise false.
*/
bool hasCollisionObject(const std::string& name) const;

bool removeCollisionObject(const std::string& name) override;
/**
* @brief Remove an object from the checker
* @param name The name of the object
* @return true if successfully removed, otherwise false.
*/
bool removeCollisionObject(const std::string& name);

bool enableCollisionObject(const std::string& name) override;
/**
* @brief Enable an object
* @param name The name of the object
*/
bool enableCollisionObject(const std::string& name);

bool disableCollisionObject(const std::string& name) override;
/**
* @brief Disable an object
* @param name The name of the object
*/
bool disableCollisionObject(const std::string& name);

void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) override;
/**
* @brief Set a single static collision object's tansforms
* @param name The name of the object
* @param pose The tranformation in world
*/
void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose);

/**
* @brief Set a series of static collision object's tranforms
* @param names The name of the object
* @param poses The tranformation in world
*/
void setCollisionObjectsTransform(const std::vector<std::string>& names,
const AlignedVector<Eigen::Isometry3d>& poses) override;
const AlignedVector<Eigen::Isometry3d>& poses);

void setCollisionObjectsTransform(const AlignedMap<std::string, Eigen::Isometry3d>& transforms) override;
/**
* @brief Set a series of static collision object's tranforms
* @param transforms A transform map <name, pose>
*/
void setCollisionObjectsTransform(const AlignedMap<std::string, Eigen::Isometry3d>& transforms);

/**
* @brief Set a single cast(moving) collision object's tansforms
*
* This should only be used for moving objects. Use the base
* class methods for static objects.
*
* @param name The name of the object
* @param pose1 The start tranformation in world
* @param pose2 The end tranformation in world
*/
void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose1,
const Eigen::Isometry3d& pose2) override;
const Eigen::Isometry3d& pose2);

/**
* @brief Set a series of cast(moving) collision object's tranforms
*
* This should only be used for moving objects. Use the base
* class methods for static objects.
*
* @param names The name of the object
* @param pose1 The start tranformations in world
* @param pose2 The end tranformations in world
*/
void setCollisionObjectsTransform(const std::vector<std::string>& names,
const AlignedVector<Eigen::Isometry3d>& pose1,
const AlignedVector<Eigen::Isometry3d>& pose2) override;
const AlignedVector<Eigen::Isometry3d>& pose2);

/**
* @brief Set a series of cast(moving) collision object's tranforms
*
* This should only be used for moving objects. Use the base
* class methods for static objects.
*
* @param pose1 A start transform map <name, pose>
* @param pose2 A end transform map <name, pose>
*/
void setCollisionObjectsTransform(const AlignedMap<std::string, Eigen::Isometry3d>& pose1,
const AlignedMap<std::string, Eigen::Isometry3d>& pose2) override;
const AlignedMap<std::string, Eigen::Isometry3d>& pose2);

void setActiveCollisionObjects(const std::vector<std::string>& names) override;
/**
* @brief Set which collision objects can move
* @param names A vector of collision object names
*/
void setActiveCollisionObjects(const std::vector<std::string>& names);

const std::vector<std::string>& getActiveCollisionObjects() const override;
/**
* @brief Get which collision objects can move
* @return A list of collision object names
*/
const std::vector<std::string>& getActiveCollisionObjects() const;

void setContactDistanceThreshold(double contact_distance) override;
/**
* @brief Set the contact distance threshold for which collision should be considered.
* @param contact_distance The contact distance
*/
void setContactDistanceThreshold(double contact_distance);

double getContactDistanceThreshold() const override;
/**
* @brief Get the contact distance threshold
* @return The contact distance
*/
double getContactDistanceThreshold() const;

void setIsContactAllowedFn(IsContactAllowedFn fn) override;
/** @brief Set the active function for determining if two links are allowed to be in collision */
void setIsContactAllowedFn(IsContactAllowedFn fn);

IsContactAllowedFn getIsContactAllowedFn() const override;
/** @brief Get the active function for determining if two links are allowed to be in collision */
IsContactAllowedFn getIsContactAllowedFn() const;

/**
* @brief Perform a contact test for all objects based
* @param collisions The Contact results data
* @param req The collision request data
* @param acm The allowed collision matrix
*/
void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req,
const collision_detection::AllowedCollisionMatrix* acm) override;
const collision_detection::AllowedCollisionMatrix* acm);

/**
* @brief A a bullet collision object to the manager
* @param cow The tesseract bullet collision object
*/
void addCollisionObject(const COWPtr& cow);

private:
std::vector<std::string> active_; /**< @brief A list of the active collision objects */
double contact_distance_; /**< @brief The contact distance threshold */
IsContactAllowedFn fn_; /**< @brief The is allowed collision function */
/**< @brief A list of the active collision objects */
std::vector<std::string> active_;

/**< @brief The contact distance threshold */
double contact_distance_;

/**< @brief The is allowed collision function */
IsContactAllowedFn fn_;

/**< @brief The bullet collision dispatcher used for getting object to object collison algorithm */
std::unique_ptr<btCollisionDispatcher> dispatcher_;

/**< @brief The bullet collision dispatcher configuration information */
btDispatcherInfo dispatch_info_;

/**< @brief The bullet collision configuration */
btDefaultCollisionConfiguration coll_config_;

/**< @brief The bullet broadphase interface */
std::unique_ptr<btBroadphaseInterface> broadphase_;

/**< @brief A map of collision objects being managed */
std::map<std::string, COWPtr> link2cow_;

std::unique_ptr<btCollisionDispatcher> dispatcher_; /**< @brief The bullet collision dispatcher used for getting
object to object collison algorithm */
btDispatcherInfo dispatch_info_; /**< @brief The bullet collision dispatcher configuration information */
btDefaultCollisionConfiguration coll_config_; /**< @brief The bullet collision configuration */
std::unique_ptr<btBroadphaseInterface> broadphase_; /**< @brief The bullet broadphase interface */
std::map<std::string, COWPtr> link2cow_; /**< @brief A map of collision objects being managed */
std::map<std::string, COWPtr> link2castcow_; /**< @brief A map of cast collision objects being managed. */
/**< @brief A map of cast collision objects being managed. */
std::map<std::string, COWPtr> link2castcow_;

/**
* @brief Perform a contact test for the provided object which is not part of the manager
Expand All @@ -126,7 +256,6 @@ class BulletCastBVHManager : public ContinuousContactManagerBase
*/
void contactTest(const COWPtr& cow, ContactTestData& collisions);
};
typedef std::shared_ptr<BulletCastBVHManager> BulletCastBVHManagerPtr;
}

#endif // TESSERACT_COLLISION_BULLET_CAST_BVH_MANAGERS_H
Loading

0 comments on commit 3ab813e

Please sign in to comment.