Skip to content

Base u18 #4

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

Open
shohinm wants to merge 9 commits into
base: master
Choose a base branch
from
Open

Base u18 #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
5d54bb0
Fixed resizing issue for voxel state versions vec
shohinm Dec 15, 2020
484f616
changes for migration to melodic
May 20, 2020
1fb2afb
added collision model and launch example for franka
May 21, 2020
bf66eee
added catkin ignores
Jun 9, 2020
a3b6403
Added franka collision models
Sep 7, 2020
16dc8de
Removed CATKIN_IGNORE of smpl_moveit_interface
Sep 15, 2020
f17c907
Fixed behaviour of SelfCollisionModel::updateAllowedCollisionMatrix
shohinm Dec 20, 2020
7088297
Added attached object visualization in visulization of robot collisio…
shohinm Dec 20, 2020
f732162
Joining the bfs3d search thread so it can be reassigned
shohinm Feb 19, 2021
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
12 changes: 6 additions & 6 deletions sbpl_collision_checking/include/sbpl_collision_checking/attached_bodies_collision_model.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ class AttachedBodiesCollisionModel
bool attachBody(
const std::string& id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms,
const Isometry3dVector& transforms,
const std::string& link_name,
bool create_voxels_model = true,
bool create_spheres_model = true);
Expand Down Expand Up @@ -143,29 +143,29 @@ class AttachedBodiesCollisionModel
int abidx,
const std::string& id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms);
const Isometry3dVector& transforms);

CollisionVoxelsModel* createVoxelsModel(
int abidx,
const std::string& id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms);
const Isometry3dVector& transforms);

void generateSpheresModel(
const std::string& id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms,
const Isometry3dVector& transforms,
CollisionSpheresModelConfig& spheres_model);

void generateVoxelsModel(
const std::string& id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms,
const Isometry3dVector& transforms,
CollisionVoxelModelConfig& voxels_models);

bool voxelizeAttachedBody(
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms,
const Isometry3dVector& transforms,
CollisionVoxelsModel& model) const;
};

Expand Down
2 changes: 1 addition & 1 deletion sbpl_collision_checking/include/sbpl_collision_checking/collision_space.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ class CollisionSpace : public CollisionChecker
bool attachObject(
const std::string& id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms,
const Isometry3dVector& transforms,
const std::string& link_name);

bool detachObject(const std::string& id);
Expand Down
2 changes: 2 additions & 0 deletions sbpl_collision_checking/include/sbpl_collision_checking/types.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,8 @@ using AlignedVector = std::vector<T, Eigen::aligned_allocator<T>>;

using Affine3dVector = AlignedVector<Eigen::Affine3d>;

using Isometry3dVector = AlignedVector<Eigen::Isometry3d>;

template <
class Key,
class T,
Expand Down
12 changes: 6 additions & 6 deletions sbpl_collision_checking/src/attached_bodies_collision_model.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ AttachedBodiesCollisionModel::AttachedBodiesCollisionModel(
bool AttachedBodiesCollisionModel::attachBody(
const std::string& id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms,
const Isometry3dVector& transforms,
const std::string& link_name,
bool create_voxels_model,
bool create_spheres_model)
Expand Down Expand Up @@ -203,7 +203,7 @@ CollisionSpheresModel* AttachedBodiesCollisionModel::createSpheresModel(
int abidx,
const std::string& id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms)
const Isometry3dVector& transforms)
{
ROS_DEBUG_NAMED(ABM_LOGGER, " Generate spheres model");

Expand Down Expand Up @@ -233,7 +233,7 @@ CollisionVoxelsModel* AttachedBodiesCollisionModel::createVoxelsModel(
int abidx,
const std::string& id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms)
const Isometry3dVector& transforms)
{
ROS_DEBUG_NAMED(ABM_LOGGER, " Generate voxels model");

Expand Down Expand Up @@ -264,7 +264,7 @@ CollisionVoxelsModel* AttachedBodiesCollisionModel::createVoxelsModel(
void AttachedBodiesCollisionModel::generateSpheresModel(
const std::string& id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms,
const Isometry3dVector& transforms,
CollisionSpheresModelConfig& spheres_model)
{
assert(std::all_of(shapes.begin(), shapes.end(),
Expand Down Expand Up @@ -315,7 +315,7 @@ void AttachedBodiesCollisionModel::generateSpheresModel(
void AttachedBodiesCollisionModel::generateVoxelsModel(
const std::string& id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms,
const Isometry3dVector& transforms,
CollisionVoxelModelConfig& voxels_model)
{
ROS_DEBUG_NAMED(ABM_LOGGER, "Generate voxels model configuration");
Expand All @@ -324,7 +324,7 @@ void AttachedBodiesCollisionModel::generateVoxelsModel(

bool AttachedBodiesCollisionModel::voxelizeAttachedBody(
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms,
const Isometry3dVector& transforms,
CollisionVoxelsModel& model) const
{
if (shapes.size() != transforms.size()) {
Expand Down
4 changes: 4 additions & 0 deletions sbpl_collision_checking/src/attached_bodies_collision_state.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,10 @@ bool AttachedBodiesCollisionState::updateVoxelsState(int vsidx)
}

state.voxels = std::move(new_voxels);

if (m_voxels_state_versions.size()<=vsidx)
m_voxels_state_versions.resize(vsidx+1);

m_voxels_state_versions[vsidx] = attachedBodyTransformVersion(bidx);
return true;
}
Expand Down
13 changes: 12 additions & 1 deletion sbpl_collision_checking/src/collision_space.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -200,7 +200,7 @@ bool CollisionSpace::removeShapes(const CollisionObject* object)
bool CollisionSpace::attachObject(
const std::string& id,
const std::vector<shapes::ShapeConstPtr>& shapes,
const Affine3dVector& transforms,
const Isometry3dVector& transforms,
const std::string& link_name)
{
return m_abcm->attachBody(id, shapes, transforms, link_name);
Expand Down Expand Up @@ -256,6 +256,17 @@ auto CollisionSpace::getCollisionRobotVisualization()
for (auto& m : markers.markers) {
m.header.frame_id = m_grid->getReferenceFrame();
}

// Add the attached collision object spheres
for (int ssidx : m_abcs->groupSpheresStateIndices(m_gidx)) {
m_abcs->updateSphereStates(ssidx);
}
auto markers_attached = m_abcs->getVisualization(m_gidx);
for (auto& m : markers_attached.markers) {
m.header.frame_id = m_grid->getReferenceFrame();
markers.markers.push_back(m);
}

return markers;
}

Expand Down
12 changes: 6 additions & 6 deletions sbpl_collision_checking/src/conversions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -253,7 +253,7 @@ void ConvertCollisionObjectMsgToWorldObject(
continue;
}

Eigen::Affine3d transform;
Eigen::Isometry3d transform;
tf::poseMsgToEigen(pose, transform);

o.shapes_.push_back(sp);
Expand All @@ -270,7 +270,7 @@ void ConvertCollisionObjectMsgToWorldObject(
continue;
}

Eigen::Affine3d transform;
Eigen::Isometry3d transform;
tf::poseMsgToEigen(pose, transform);

o.shapes_.push_back(sp);
Expand All @@ -287,7 +287,7 @@ void ConvertCollisionObjectMsgToWorldObject(
continue;
}

Eigen::Affine3d transform;
Eigen::Isometry3d transform;
tf::poseMsgToEigen(pose, transform);

o.shapes_.push_back(sp);
Expand Down Expand Up @@ -324,7 +324,7 @@ void ConvertCollisionObjectMsgToCollisionObject(
continue;
}

Eigen::Affine3d transform;
Eigen::Isometry3d transform;
tf::poseMsgToEigen(pose, transform);

o.shapes.push_back(sp);
Expand All @@ -341,7 +341,7 @@ void ConvertCollisionObjectMsgToCollisionObject(
continue;
}

Eigen::Affine3d transform;
Eigen::Isometry3d transform;
tf::poseMsgToEigen(pose, transform);

o.shapes.push_back(sp);
Expand All @@ -358,7 +358,7 @@ void ConvertCollisionObjectMsgToCollisionObject(
continue;
}

Eigen::Affine3d transform;
Eigen::Isometry3d transform;
tf::poseMsgToEigen(pose, transform);

o.shapes.push_back(sp);
Expand Down
4 changes: 2 additions & 2 deletions sbpl_collision_checking/src/robot_collision_model.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -140,11 +140,11 @@ bool RobotCollisionModel::initRobotModel(

// breadth-first traversal of all links in the robot

std::stack<boost::shared_ptr<const ::urdf::Link>> links;
std::stack<std::shared_ptr<const ::urdf::Link>> links;
links.push(root_link);

while (!links.empty()) {
boost::shared_ptr<const ::urdf::Link> link;
std::shared_ptr<const ::urdf::Link> link;
link = links.top();
links.pop();

Expand Down
7 changes: 5 additions & 2 deletions sbpl_collision_checking/src/self_collision_model.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -170,12 +170,15 @@ void SelfCollisionModel::updateAllowedCollisionMatrix(
const std::string& entry1 = all_entries[i];
const std::string& entry2 = all_entries[j];
if (acm.getEntry(entry1, entry2, type)) {
if (type != collision_detection::AllowedCollision::NEVER) {
if (type == collision_detection::AllowedCollision::NEVER)
{
m_acm.setEntry(entry1, entry2, false);
}
else {
else if (type == collision_detection::AllowedCollision::ALWAYS || type == collision_detection::AllowedCollision::CONDITIONAL)
{
m_acm.setEntry(entry1, entry2, true);
}

}
}
}
Expand Down
155 changes: 155 additions & 0 deletions sbpl_collision_checking_test/config/collision_model_franka.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,155 @@
# occupancy grid information
world_collision_model:
size_x: 1.9
size_y: 2.4
size_z: 3.0
origin_x: -0.4
origin_y: -1.2
origin_z: 0.0
res_m: 0.02
max_distance_m: 0.2

robot_collision_model:
world_joint:
name: map
type: floating # fixed, floating
voxels_models:
- { link_name: panda_link0, res: 0.01 }
- { link_name: panda_link1, res: 0.01 }
- { link_name: panda_link2, res: 0.01 }
- { link_name: panda_link3, res: 0.01 }
- { link_name: panda_link4, res: 0.01 }
- { link_name: panda_link5, res: 0.01 }
- { link_name: panda_link6, res: 0.01 }
- { link_name: panda_link7, res: 0.01 }
- { link_name: panda_hand, res: 0.01 }
- { link_name: panda_leftfinger, res: 0.01 }
- { link_name: panda_rightfinger, res: 0.01 }
spheres_models:
- link_name: panda_link0
auto: false
spheres:
- { name: l00, x: 0.0, y: 0.000, z: 0.050, radius: 0.080, priority: 5 }
- link_name: panda_link1
auto: false
spheres:
- { name: l10, x: 0.0, y: -0.080, z: 0.000, radius: 0.060, priority: 5 }
- { name: l11, x: 0.0, y: -0.030, z: 0.000, radius: 0.060, priority: 5 }
- { name: l12, x: 0.0, y: 0.000, z: -0.120, radius: 0.060, priority: 5 }
- { name: l13, x: 0.0, y: 0.000, z: -0.170, radius: 0.060, priority: 5 }
- link_name: panda_link2
auto: false
spheres:
- { name: l20, x: 0.0, y: 0.000, z: 0.030, radius: 0.060, priority: 5 }
- { name: l21, x: 0.0, y: 0.000, z: 0.080, radius: 0.060, priority: 5 }
- { name: l22, x: 0.0, y: -0.120, z: 0.000, radius: 0.060, priority: 5 }
- { name: l23, x: 0.0, y: -0.170, z: 0.000, radius: 0.060, priority: 5 }
- link_name: panda_link3
auto: false
spheres:
- { name: l30, x: 0.0, y: 0.000, z: -0.06, radius: 0.050, priority: 5 }
- { name: l31, x: 0.0, y: 0.000, z: -0.100, radius: 0.060, priority: 5 }
- { name: l32, x: 0.08, y: 0.060, z: 0.000, radius: 0.055, priority: 5 }
- { name: l33, x: 0.08, y: 0.020, z: 0.000, radius: 0.055, priority: 5 }
- link_name: panda_link4
auto: false
spheres:
- { name: l40, x: 0.0, y: 0.000, z: 0.020, radius: 0.055, priority: 5 }
- { name: l41, x: 0.0, y: 0.000, z: 0.060, radius: 0.055, priority: 5 }
- { name: l42, x: -0.08, y: 0.095, z: 0.000, radius: 0.060, priority: 5 }
- { name: l43, x: -0.08, y: 0.060, z: 0.000, radius: 0.055, priority: 5 }
- link_name: panda_link5
auto: false
spheres:
- { name: l500, x: 0.0, y: 0.055, z: 0.000, radius: 0.060, priority: 5 }
- { name: l501, x: 0.0, y: 0.075, z: 0.000, radius: 0.060, priority: 5 }
- { name: l502, x: 0.0, y: 0.000, z: -0.22, radius: 0.060, priority: 5 }
- { name: l503, x: 0.0, y: 0.05, z: -0.18, radius: 0.050, priority: 5 }
# small spheres
- { name: l504, x: 0.01, y: 0.08, z: -0.14, radius: 0.025, priority: 5 }
- { name: l505, x: 0.01, y: 0.085, z: -0.11, radius: 0.025, priority: 5 }
- { name: l506, x: 0.01, y: 0.09, z: -0.08, radius: 0.025, priority: 5 }
- { name: l507, x: 0.01, y: 0.095, z: -0.05, radius: 0.025, priority: 5 }
- { name: l508, x: -0.01, y: 0.08, z: -0.14, radius: 0.025, priority: 5 }
- { name: l509, x: -0.01, y: 0.085, z: -0.11, radius: 0.025, priority: 5 }
- { name: l510, x: -0.01, y: 0.09, z: -0.08, radius: 0.025, priority: 5 }
- { name: l511, x: -0.01, y: 0.095, z: -0.05, radius: 0.025, priority: 5 }
- link_name: panda_link6
auto: false
spheres:
- { name: l60, x: 0.0, y: 0.000, z: 0.000, radius: 0.060, priority: 5 }
- { name: l61, x: 0.08, y: 0.030, z: 0.000, radius: 0.060, priority: 5 }
- { name: l62, x: 0.08, y: -0.010, z: 0.000, radius: 0.060, priority: 5 }
- link_name: panda_link7
auto: false
spheres:
- { name: l70, x: 0.0, y: 0.000, z: 0.070, radius: 0.05, priority: 5 }
# small spheres
- { name: l71, x: 0.02, y: 0.04, z: 0.08, radius: 0.025, priority: 5 }
- { name: l72, x: 0.04, y: 0.02, z: 0.08, radius: 0.025, priority: 5 }
- { name: l73, x: 0.04, y: 0.06, z: 0.085, radius: 0.02, priority: 5 }
- { name: l74, x: 0.06, y: 0.04, z: 0.085, radius: 0.02, priority: 5 }
- link_name: panda_hand
auto: false
spheres:
- { name: h00, x: 0.00, y: -0.075, z: 0.01, radius: 0.028, priority: 5 }
- { name: h01, x: 0.00, y: -0.045, z: 0.01, radius: 0.028, priority: 5 }
- { name: h02, x: 0.00, y: -0.015, z: 0.01, radius: 0.028, priority: 5 }
- { name: h03, x: 0.00, y: 0.015, z: 0.01, radius: 0.028, priority: 5 }
- { name: h04, x: 0.00, y: 0.045, z: 0.01, radius: 0.028, priority: 5 }
- { name: h05, x: 0.00, y: 0.075, z: 0.01, radius: 0.028, priority: 5 }
##
- { name: h10, x: 0.00, y: -0.075, z: 0.03, radius: 0.026, priority: 5 }
- { name: h11, x: 0.00, y: -0.045, z: 0.03, radius: 0.026, priority: 5 }
- { name: h12, x: 0.00, y: -0.015, z: 0.03, radius: 0.026, priority: 5 }
- { name: h13, x: 0.00, y: 0.015, z: 0.03, radius: 0.026, priority: 5 }
- { name: h14, x: 0.00, y: 0.045, z: 0.03, radius: 0.026, priority: 5 }
- { name: h15, x: 0.00, y: 0.075, z: 0.03, radius: 0.026, priority: 5 }
##
- { name: h20, x: 0.00, y: -0.075, z: 0.05, radius: 0.024, priority: 5 }
- { name: h21, x: 0.00, y: -0.045, z: 0.05, radius: 0.024, priority: 5 }
- { name: h22, x: 0.00, y: -0.015, z: 0.05, radius: 0.024, priority: 5 }
- { name: h23, x: 0.00, y: 0.015, z: 0.05, radius: 0.024, priority: 5 }
- { name: h24, x: 0.00, y: 0.045, z: 0.05, radius: 0.024, priority: 5 }
- { name: h25, x: 0.00, y: 0.075, z: 0.05, radius: 0.024, priority: 5 }
# - link_name: panda_rightfinger
# auto: true
# radius: 0.01
# - link_name: panda_leftfinger
# auto: true
# radius: 0.01
- link_name: panda_rightfinger
auto: false
spheres:
- { name: rf01, x: 0.00, y: -0.02, z: 0.02, radius: 0.01, priority: 5 }
- { name: rf02, x: 0.00, y: -0.02, z: 0.03, radius: 0.01, priority: 5 }
- { name: rf03, x: 0.00, y: -0.02, z: 0.04, radius: 0.01, priority: 5 }
- { name: rf04, x: 0.00, y: -0.02, z: 0.05, radius: 0.01, priority: 5 }
- link_name: panda_leftfinger
auto: false
spheres:
- { name: lf01, x: 0.00, y: 0.02, z: 0.02, radius: 0.01, priority: 5 }
- { name: lf02, x: 0.00, y: 0.02, z: 0.03, radius: 0.01, priority: 5 }
- { name: lf03, x: 0.00, y: 0.02, z: 0.04, radius: 0.01, priority: 5 }
- { name: lf04, x: 0.00, y: 0.02, z: 0.05, radius: 0.01, priority: 5 }
# voxels_models:
# - link_name: world
collision_groups:
- name: manipulator
# chains:
# - base: panda_link0
# tip: panda_link6
links:
- name: panda_link0
- name: panda_link1
- name: panda_link2
- name: panda_link3
- name: panda_link4
- name: panda_link5
- name: panda_link6
- name: panda_link7
- name: panda_hand
- name: panda_rightfinger
- name: panda_leftfinger
# allowed_collisions:
# - { sphere1: l70, sphere2: l81 }
Loading