Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

switch from using affine3d to isometry3d #20

Merged
merged 2 commits into from
Aug 31, 2018
Merged

switch from using affine3d to isometry3d #20

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8fa58d2
switch from using affine3d to isometry3d
Levi-Armstrong Aug 30, 2018
6af213f
Convert to use templated using for vectors and maps of eigen types
Levi-Armstrong Aug 30, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32 changes: 16 additions & 16 deletions tesseract_collision/include/tesseract_collision/bullet/bullet_cast_managers.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ class BulletCastSimpleManager : public ContinuousContactManagerBase
bool addCollisionObject(const std::string& name,
const int& mask_id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const EigenSTL::vector_Affine3d& shape_poses,
const VectorIsometry3d& shape_poses,
const CollisionObjectTypeVector& collision_object_types,
bool enabled = true) override;

Expand All @@ -70,20 +70,20 @@ class BulletCastSimpleManager : public ContinuousContactManagerBase

bool disableCollisionObject(const std::string& name) override;

void setCollisionObjectsTransform(const std::string& name, const Eigen::Affine3d& pose) override;
void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) override;

void setCollisionObjectsTransform(const std::vector<std::string>& names,
const EigenSTL::vector_Affine3d& poses) override;
const VectorIsometry3d& poses) override;

void setCollisionObjectsTransform(const TransformMap& transforms) override;

void setCollisionObjectsTransform(const std::string& name,
const Eigen::Affine3d& pose1,
const Eigen::Affine3d& pose2) override;
const Eigen::Isometry3d& pose1,
const Eigen::Isometry3d& pose2) override;

void setCollisionObjectsTransform(const std::vector<std::string>& names,
const EigenSTL::vector_Affine3d& pose1,
const EigenSTL::vector_Affine3d& pose2) override;
const VectorIsometry3d& pose1,
const VectorIsometry3d& pose2) override;

void setCollisionObjectsTransform(const TransformMap& pose1, const TransformMap& pose2) override;

Expand Down Expand Up @@ -165,7 +165,7 @@ class BulletCastBVHManager : public ContinuousContactManagerBase
bool addCollisionObject(const std::string& name,
const int& mask_id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const EigenSTL::vector_Affine3d& shape_poses,
const VectorIsometry3d& shape_poses,
const CollisionObjectTypeVector& collision_object_types,
bool enabled = true) override;

Expand All @@ -177,20 +177,20 @@ class BulletCastBVHManager : public ContinuousContactManagerBase

bool disableCollisionObject(const std::string& name) override;

void setCollisionObjectsTransform(const std::string& name, const Eigen::Affine3d& pose) override;
void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) override;

void setCollisionObjectsTransform(const std::vector<std::string>& names,
const EigenSTL::vector_Affine3d& poses) override;
const VectorIsometry3d& poses) override;

void setCollisionObjectsTransform(const TransformMap& transforms) override;

void setCollisionObjectsTransform(const std::string& name,
const Eigen::Affine3d& pose1,
const Eigen::Affine3d& pose2) override;
const Eigen::Isometry3d& pose1,
const Eigen::Isometry3d& pose2) override;

void setCollisionObjectsTransform(const std::vector<std::string>& names,
const EigenSTL::vector_Affine3d& pose1,
const EigenSTL::vector_Affine3d& pose2) override;
const VectorIsometry3d& pose1,
const VectorIsometry3d& pose2) override;

void setCollisionObjectsTransform(const TransformMap& pose1, const TransformMap& pose2) override;

Expand Down Expand Up @@ -286,8 +286,8 @@ void constructCastContactManager(T& manager,
else if (btBroadphaseProxy::isCompound(new_cow->getCollisionShape()->getShapeType()))
{
btCompoundShape* compound = static_cast<btCompoundShape*>(new_cow->getCollisionShape());
const Eigen::Affine3d& tf1 = it1->second;
const Eigen::Affine3d& tf2 = it2->second;
const Eigen::Isometry3d& tf1 = it1->second;
const Eigen::Isometry3d& tf2 = it2->second;

btCompoundShape* new_compound = new btCompoundShape(/*dynamicAABBtree=*/false);

Expand Down
12 changes: 6 additions & 6 deletions tesseract_collision/include/tesseract_collision/bullet/bullet_discrete_managers.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ class BulletDiscreteSimpleManager : public DiscreteContactManagerBase
bool addCollisionObject(const std::string& name,
const int& mask_id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const EigenSTL::vector_Affine3d& shape_poses,
const VectorIsometry3d& shape_poses,
const CollisionObjectTypeVector& collision_object_types,
bool enabled = true) override;

Expand All @@ -68,10 +68,10 @@ class BulletDiscreteSimpleManager : public DiscreteContactManagerBase

bool disableCollisionObject(const std::string& name) override;

void setCollisionObjectsTransform(const std::string& name, const Eigen::Affine3d& pose) override;
void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) override;

void setCollisionObjectsTransform(const std::vector<std::string>& names,
const EigenSTL::vector_Affine3d& poses) override;
const VectorIsometry3d& poses) override;

void setCollisionObjectsTransform(const TransformMap& transforms) override;

Expand Down Expand Up @@ -157,7 +157,7 @@ class BulletDiscreteBVHManager : public DiscreteContactManagerBase
bool addCollisionObject(const std::string& name,
const int& mask_id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const EigenSTL::vector_Affine3d& shape_poses,
const VectorIsometry3d& shape_poses,
const CollisionObjectTypeVector& collision_object_types,
bool enabled = true) override;

Expand All @@ -169,10 +169,10 @@ class BulletDiscreteBVHManager : public DiscreteContactManagerBase

bool disableCollisionObject(const std::string& name) override;

void setCollisionObjectsTransform(const std::string& name, const Eigen::Affine3d& pose) override;
void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) override;

void setCollisionObjectsTransform(const std::vector<std::string>& names,
const EigenSTL::vector_Affine3d& poses) override;
const VectorIsometry3d& poses) override;

void setCollisionObjectsTransform(const TransformMap& transforms) override;

Expand Down
10 changes: 5 additions & 5 deletions tesseract_collision/include/tesseract_collision/bullet/bullet_utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ inline btMatrix3x3 convertEigenToBt(const Eigen::Matrix3d& r)
return btMatrix3x3(r(0, 0), r(0, 1), r(0, 2), r(1, 0), r(1, 1), r(1, 2), r(2, 0), r(2, 1), r(2, 2));
}

inline btTransform convertEigenToBt(const Eigen::Affine3d& t)
inline btTransform convertEigenToBt(const Eigen::Isometry3d& t)
{
const Eigen::Matrix3d& rot = t.matrix().block<3, 3>(0, 0);
const Eigen::Vector3d& tran = t.translation();
Expand All @@ -92,7 +92,7 @@ class CollisionObjectWrapper : public btCollisionObject
CollisionObjectWrapper(const std::string& name,
const int& type_id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const EigenSTL::vector_Affine3d& shape_poses,
const VectorIsometry3d& shape_poses,
const CollisionObjectTypeVector& collision_object_types);

short int m_collisionFilterGroup;
Expand Down Expand Up @@ -142,14 +142,14 @@ class CollisionObjectWrapper : public btCollisionObject
CollisionObjectWrapper(const std::string& name,
const int& type_id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const EigenSTL::vector_Affine3d& shape_poses,
const VectorIsometry3d& shape_poses,
const CollisionObjectTypeVector& collision_object_types,
const std::vector<std::shared_ptr<void>>& data);

std::string m_name; /**< @brief The name of the collision object */
int m_type_id; /**< @brief A user defined type id */
const std::vector<shapes::ShapeConstPtr>& m_shapes; /**< @brief The shapes that define the collison object */
const EigenSTL::vector_Affine3d& m_shape_poses; /**< @brief The shpaes poses information */
const VectorIsometry3d& m_shape_poses; /**< @brief The shpaes poses information */
const CollisionObjectTypeVector& m_collision_object_types; /**< @brief The shape collision object type to be used */

std::vector<std::shared_ptr<void>>
Expand Down Expand Up @@ -722,7 +722,7 @@ inline void updateCollisionObjectWithRequest(const ContactRequest& req, COW& cow
inline COWPtr createCollisionObject(const std::string& name,
const int& type_id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const EigenSTL::vector_Affine3d& shape_poses,
const VectorIsometry3d& shape_poses,
const CollisionObjectTypeVector& collision_object_types,
bool enabled = true)
{
Expand Down
28 changes: 14 additions & 14 deletions tesseract_collision/src/bullet/bullet_cast_managers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -143,7 +143,7 @@ ContinuousContactManagerBasePtr BulletCastSimpleManager::clone() const
bool BulletCastSimpleManager::addCollisionObject(const std::string& name,
const int& mask_id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const EigenSTL::vector_Affine3d& shape_poses,
const VectorIsometry3d& shape_poses,
const CollisionObjectTypeVector& collision_object_types,
bool enabled)
{
Expand Down Expand Up @@ -226,7 +226,7 @@ bool BulletCastSimpleManager::disableCollisionObject(const std::string& name)
return disabled;
}

void BulletCastSimpleManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Affine3d& pose)
void BulletCastSimpleManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose)
{
// TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with
// geometry
Expand All @@ -237,7 +237,7 @@ void BulletCastSimpleManager::setCollisionObjectsTransform(const std::string& na
}

void BulletCastSimpleManager::setCollisionObjectsTransform(const std::vector<std::string>& names,
const EigenSTL::vector_Affine3d& poses)
const VectorIsometry3d& poses)
{
assert(names.size() == poses.size());
for (auto i = 0u; i < names.size(); ++i)
Expand All @@ -251,8 +251,8 @@ void BulletCastSimpleManager::setCollisionObjectsTransform(const TransformMap& t
}

void BulletCastSimpleManager::setCollisionObjectsTransform(const std::string& name,
const Eigen::Affine3d& pose1,
const Eigen::Affine3d& pose2)
const Eigen::Isometry3d& pose1,
const Eigen::Isometry3d& pose2)
{
// TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with
// geometry
Expand All @@ -270,8 +270,8 @@ void BulletCastSimpleManager::setCollisionObjectsTransform(const std::string& na
}

void BulletCastSimpleManager::setCollisionObjectsTransform(const std::vector<std::string>& names,
const EigenSTL::vector_Affine3d& pose1,
const EigenSTL::vector_Affine3d& pose2)
const VectorIsometry3d& pose1,
const VectorIsometry3d& pose2)
{
assert(names.size() == pose1.size() == pose2.size());
for (auto i = 0u; i < names.size(); ++i)
Expand Down Expand Up @@ -525,7 +525,7 @@ ContinuousContactManagerBasePtr BulletCastBVHManager::clone() const
bool BulletCastBVHManager::addCollisionObject(const std::string& name,
const int& mask_id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const EigenSTL::vector_Affine3d& shape_poses,
const VectorIsometry3d& shape_poses,
const CollisionObjectTypeVector& collision_object_types,
bool enabled)
{
Expand Down Expand Up @@ -624,7 +624,7 @@ bool BulletCastBVHManager::disableCollisionObject(const std::string& name)
return disabled;
}

void BulletCastBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Affine3d& pose)
void BulletCastBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose)
{
// TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with
// geometry
Expand All @@ -637,7 +637,7 @@ void BulletCastBVHManager::setCollisionObjectsTransform(const std::string& name,
}

void BulletCastBVHManager::setCollisionObjectsTransform(const std::vector<std::string>& names,
const EigenSTL::vector_Affine3d& poses)
const VectorIsometry3d& poses)
{
assert(names.size() == poses.size());
for (auto i = 0u; i < names.size(); ++i)
Expand All @@ -651,8 +651,8 @@ void BulletCastBVHManager::setCollisionObjectsTransform(const TransformMap& tran
}

void BulletCastBVHManager::setCollisionObjectsTransform(const std::string& name,
const Eigen::Affine3d& pose1,
const Eigen::Affine3d& pose2)
const Eigen::Isometry3d& pose1,
const Eigen::Isometry3d& pose2)
{
// TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with
// geometry
Expand Down Expand Up @@ -704,8 +704,8 @@ void BulletCastBVHManager::setCollisionObjectsTransform(const std::string& name,
}

void BulletCastBVHManager::setCollisionObjectsTransform(const std::vector<std::string>& names,
const EigenSTL::vector_Affine3d& pose1,
const EigenSTL::vector_Affine3d& pose2)
const VectorIsometry3d& pose1,
const VectorIsometry3d& pose2)
{
assert(names.size() == pose1.size() == pose2.size());
for (auto i = 0u; i < names.size(); ++i)
Expand Down
12 changes: 6 additions & 6 deletions tesseract_collision/src/bullet/bullet_discrete_managers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ DiscreteContactManagerBasePtr BulletDiscreteSimpleManager::clone() const
bool BulletDiscreteSimpleManager::addCollisionObject(const std::string& name,
const int& mask_id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const EigenSTL::vector_Affine3d& shape_poses,
const VectorIsometry3d& shape_poses,
const CollisionObjectTypeVector& collision_object_types,
bool enabled)
{
Expand Down Expand Up @@ -140,7 +140,7 @@ bool BulletDiscreteSimpleManager::disableCollisionObject(const std::string& name
return false;
}

void BulletDiscreteSimpleManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Affine3d& pose)
void BulletDiscreteSimpleManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose)
{
// TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with
// geometry
Expand All @@ -150,7 +150,7 @@ void BulletDiscreteSimpleManager::setCollisionObjectsTransform(const std::string
}

void BulletDiscreteSimpleManager::setCollisionObjectsTransform(const std::vector<std::string>& names,
const EigenSTL::vector_Affine3d& poses)
const VectorIsometry3d& poses)
{
assert(names.size() == poses.size());
for (auto i = 0u; i < names.size(); ++i)
Expand Down Expand Up @@ -329,7 +329,7 @@ DiscreteContactManagerBasePtr BulletDiscreteBVHManager::clone() const
bool BulletDiscreteBVHManager::addCollisionObject(const std::string& name,
const int& mask_id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const EigenSTL::vector_Affine3d& shape_poses,
const VectorIsometry3d& shape_poses,
const CollisionObjectTypeVector& collision_object_types,
bool enabled)
{
Expand Down Expand Up @@ -393,7 +393,7 @@ bool BulletDiscreteBVHManager::disableCollisionObject(const std::string& name)
return false;
}

void BulletDiscreteBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Affine3d& pose)
void BulletDiscreteBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose)
{
// TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with
// geometry
Expand Down Expand Up @@ -428,7 +428,7 @@ void BulletDiscreteBVHManager::setCollisionObjectsTransform(const std::string& n
}

void BulletDiscreteBVHManager::setCollisionObjectsTransform(const std::vector<std::string>& names,
const EigenSTL::vector_Affine3d& poses)
const VectorIsometry3d& poses)
{
assert(names.size() == poses.size());
for (auto i = 0u; i < names.size(); ++i)
Expand Down
Loading