From 5d54bb0879fa56d1963d2e6fcf036cafa59f3d65 Mon Sep 17 00:00:00 2001 From: Shohin Mukherjee Date: Tue, 15 Dec 2020 10:43:13 -0500 Subject: [PATCH 1/9] Fixed resizing issue for voxel state versions vec --- .../src/attached_bodies_collision_state.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sbpl_collision_checking/src/attached_bodies_collision_state.cpp b/sbpl_collision_checking/src/attached_bodies_collision_state.cpp index 1b512326..7f311161 100644 --- a/sbpl_collision_checking/src/attached_bodies_collision_state.cpp +++ b/sbpl_collision_checking/src/attached_bodies_collision_state.cpp @@ -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; } From 484f616836f462c68b506a148d4c54d1e36debd8 Mon Sep 17 00:00:00 2001 From: Fahad Islam Date: Wed, 20 May 2020 11:45:53 -0400 Subject: [PATCH 2/9] changes for migration to melodic --- .../attached_bodies_collision_model.h | 12 ++++++------ .../sbpl_collision_checking/collision_space.h | 2 +- .../include/sbpl_collision_checking/types.h | 2 ++ .../src/attached_bodies_collision_model.cpp | 12 ++++++------ sbpl_collision_checking/src/collision_space.cpp | 2 +- sbpl_collision_checking/src/conversions.cpp | 12 ++++++------ .../src/robot_collision_model.cpp | 4 ++-- .../src/test_collision_model.cpp | 4 ++-- .../{package.xml => package_inactive.xml} | 0 smpl_test/CMakeLists.txt | 12 ++++++------ smpl_test/src/collision_space_scene.cpp | 8 ++++---- .../include/smpl_urdf_robot_model/robot_model.h | 1 + 12 files changed, 37 insertions(+), 34 deletions(-) rename smpl_moveit_interface/{package.xml => package_inactive.xml} (100%) diff --git a/sbpl_collision_checking/include/sbpl_collision_checking/attached_bodies_collision_model.h b/sbpl_collision_checking/include/sbpl_collision_checking/attached_bodies_collision_model.h index 14d0db36..570e2cc7 100644 --- a/sbpl_collision_checking/include/sbpl_collision_checking/attached_bodies_collision_model.h +++ b/sbpl_collision_checking/include/sbpl_collision_checking/attached_bodies_collision_model.h @@ -59,7 +59,7 @@ class AttachedBodiesCollisionModel bool attachBody( const std::string& id, const std::vector& shapes, - const Affine3dVector& transforms, + const Isometry3dVector& transforms, const std::string& link_name, bool create_voxels_model = true, bool create_spheres_model = true); @@ -143,29 +143,29 @@ class AttachedBodiesCollisionModel int abidx, const std::string& id, const std::vector& shapes, - const Affine3dVector& transforms); + const Isometry3dVector& transforms); CollisionVoxelsModel* createVoxelsModel( int abidx, const std::string& id, const std::vector& shapes, - const Affine3dVector& transforms); + const Isometry3dVector& transforms); void generateSpheresModel( const std::string& id, const std::vector& shapes, - const Affine3dVector& transforms, + const Isometry3dVector& transforms, CollisionSpheresModelConfig& spheres_model); void generateVoxelsModel( const std::string& id, const std::vector& shapes, - const Affine3dVector& transforms, + const Isometry3dVector& transforms, CollisionVoxelModelConfig& voxels_models); bool voxelizeAttachedBody( const std::vector& shapes, - const Affine3dVector& transforms, + const Isometry3dVector& transforms, CollisionVoxelsModel& model) const; }; diff --git a/sbpl_collision_checking/include/sbpl_collision_checking/collision_space.h b/sbpl_collision_checking/include/sbpl_collision_checking/collision_space.h index b80434a5..479fd80e 100644 --- a/sbpl_collision_checking/include/sbpl_collision_checking/collision_space.h +++ b/sbpl_collision_checking/include/sbpl_collision_checking/collision_space.h @@ -116,7 +116,7 @@ class CollisionSpace : public CollisionChecker bool attachObject( const std::string& id, const std::vector& shapes, - const Affine3dVector& transforms, + const Isometry3dVector& transforms, const std::string& link_name); bool detachObject(const std::string& id); diff --git a/sbpl_collision_checking/include/sbpl_collision_checking/types.h b/sbpl_collision_checking/include/sbpl_collision_checking/types.h index 320c8b66..a671e551 100644 --- a/sbpl_collision_checking/include/sbpl_collision_checking/types.h +++ b/sbpl_collision_checking/include/sbpl_collision_checking/types.h @@ -65,6 +65,8 @@ using AlignedVector = std::vector>; using Affine3dVector = AlignedVector; +using Isometry3dVector = AlignedVector; + template < class Key, class T, diff --git a/sbpl_collision_checking/src/attached_bodies_collision_model.cpp b/sbpl_collision_checking/src/attached_bodies_collision_model.cpp index bfacc020..0b5fc6b7 100644 --- a/sbpl_collision_checking/src/attached_bodies_collision_model.cpp +++ b/sbpl_collision_checking/src/attached_bodies_collision_model.cpp @@ -70,7 +70,7 @@ AttachedBodiesCollisionModel::AttachedBodiesCollisionModel( bool AttachedBodiesCollisionModel::attachBody( const std::string& id, const std::vector& shapes, - const Affine3dVector& transforms, + const Isometry3dVector& transforms, const std::string& link_name, bool create_voxels_model, bool create_spheres_model) @@ -203,7 +203,7 @@ CollisionSpheresModel* AttachedBodiesCollisionModel::createSpheresModel( int abidx, const std::string& id, const std::vector& shapes, - const Affine3dVector& transforms) + const Isometry3dVector& transforms) { ROS_DEBUG_NAMED(ABM_LOGGER, " Generate spheres model"); @@ -233,7 +233,7 @@ CollisionVoxelsModel* AttachedBodiesCollisionModel::createVoxelsModel( int abidx, const std::string& id, const std::vector& shapes, - const Affine3dVector& transforms) + const Isometry3dVector& transforms) { ROS_DEBUG_NAMED(ABM_LOGGER, " Generate voxels model"); @@ -264,7 +264,7 @@ CollisionVoxelsModel* AttachedBodiesCollisionModel::createVoxelsModel( void AttachedBodiesCollisionModel::generateSpheresModel( const std::string& id, const std::vector& shapes, - const Affine3dVector& transforms, + const Isometry3dVector& transforms, CollisionSpheresModelConfig& spheres_model) { assert(std::all_of(shapes.begin(), shapes.end(), @@ -315,7 +315,7 @@ void AttachedBodiesCollisionModel::generateSpheresModel( void AttachedBodiesCollisionModel::generateVoxelsModel( const std::string& id, const std::vector& shapes, - const Affine3dVector& transforms, + const Isometry3dVector& transforms, CollisionVoxelModelConfig& voxels_model) { ROS_DEBUG_NAMED(ABM_LOGGER, "Generate voxels model configuration"); @@ -324,7 +324,7 @@ void AttachedBodiesCollisionModel::generateVoxelsModel( bool AttachedBodiesCollisionModel::voxelizeAttachedBody( const std::vector& shapes, - const Affine3dVector& transforms, + const Isometry3dVector& transforms, CollisionVoxelsModel& model) const { if (shapes.size() != transforms.size()) { diff --git a/sbpl_collision_checking/src/collision_space.cpp b/sbpl_collision_checking/src/collision_space.cpp index d98bd2f7..2b56c0ea 100644 --- a/sbpl_collision_checking/src/collision_space.cpp +++ b/sbpl_collision_checking/src/collision_space.cpp @@ -200,7 +200,7 @@ bool CollisionSpace::removeShapes(const CollisionObject* object) bool CollisionSpace::attachObject( const std::string& id, const std::vector& shapes, - const Affine3dVector& transforms, + const Isometry3dVector& transforms, const std::string& link_name) { return m_abcm->attachBody(id, shapes, transforms, link_name); diff --git a/sbpl_collision_checking/src/conversions.cpp b/sbpl_collision_checking/src/conversions.cpp index 34a0b0fa..afdd21fb 100644 --- a/sbpl_collision_checking/src/conversions.cpp +++ b/sbpl_collision_checking/src/conversions.cpp @@ -253,7 +253,7 @@ void ConvertCollisionObjectMsgToWorldObject( continue; } - Eigen::Affine3d transform; + Eigen::Isometry3d transform; tf::poseMsgToEigen(pose, transform); o.shapes_.push_back(sp); @@ -270,7 +270,7 @@ void ConvertCollisionObjectMsgToWorldObject( continue; } - Eigen::Affine3d transform; + Eigen::Isometry3d transform; tf::poseMsgToEigen(pose, transform); o.shapes_.push_back(sp); @@ -287,7 +287,7 @@ void ConvertCollisionObjectMsgToWorldObject( continue; } - Eigen::Affine3d transform; + Eigen::Isometry3d transform; tf::poseMsgToEigen(pose, transform); o.shapes_.push_back(sp); @@ -324,7 +324,7 @@ void ConvertCollisionObjectMsgToCollisionObject( continue; } - Eigen::Affine3d transform; + Eigen::Isometry3d transform; tf::poseMsgToEigen(pose, transform); o.shapes.push_back(sp); @@ -341,7 +341,7 @@ void ConvertCollisionObjectMsgToCollisionObject( continue; } - Eigen::Affine3d transform; + Eigen::Isometry3d transform; tf::poseMsgToEigen(pose, transform); o.shapes.push_back(sp); @@ -358,7 +358,7 @@ void ConvertCollisionObjectMsgToCollisionObject( continue; } - Eigen::Affine3d transform; + Eigen::Isometry3d transform; tf::poseMsgToEigen(pose, transform); o.shapes.push_back(sp); diff --git a/sbpl_collision_checking/src/robot_collision_model.cpp b/sbpl_collision_checking/src/robot_collision_model.cpp index fdfb2e30..73276614 100644 --- a/sbpl_collision_checking/src/robot_collision_model.cpp +++ b/sbpl_collision_checking/src/robot_collision_model.cpp @@ -140,11 +140,11 @@ bool RobotCollisionModel::initRobotModel( // breadth-first traversal of all links in the robot - std::stack> links; + std::stack> links; links.push(root_link); while (!links.empty()) { - boost::shared_ptr link; + std::shared_ptr link; link = links.top(); links.pop(); diff --git a/sbpl_collision_checking_test/src/test_collision_model.cpp b/sbpl_collision_checking_test/src/test_collision_model.cpp index 23997377..c0703b33 100644 --- a/sbpl_collision_checking_test/src/test_collision_model.cpp +++ b/sbpl_collision_checking_test/src/test_collision_model.cpp @@ -201,12 +201,12 @@ int main(int argc, char* argv[]) //////////////////////////////////////////////////////// std::vector shapes; - smpl::collision::Affine3dVector transforms; + smpl::collision::Isometry3dVector transforms; // auto ao_shape = boost::make_shared(0.10, 0.20); shapes::ShapeConstPtr ao_shape(new shapes::Cylinder(0.10, 0.20)); shapes.push_back(std::move(ao_shape)); - transforms.push_back(Eigen::Affine3d::Identity()); + transforms.push_back(Eigen::Isometry3d::Identity()); const std::string attach_link = "ee_link"; const std::string attached_body_id = "ao1"; diff --git a/smpl_moveit_interface/package.xml b/smpl_moveit_interface/package_inactive.xml similarity index 100% rename from smpl_moveit_interface/package.xml rename to smpl_moveit_interface/package_inactive.xml diff --git a/smpl_test/CMakeLists.txt b/smpl_test/CMakeLists.txt index 5df234a3..1ef59f8e 100644 --- a/smpl_test/CMakeLists.txt +++ b/smpl_test/CMakeLists.txt @@ -24,7 +24,7 @@ find_package(catkin visualization_msgs) find_package(orocos_kdl REQUIRED) -find_package(OMPL REQUIRED) +find_package(ompl REQUIRED) find_package(smpl REQUIRED) catkin_package() @@ -39,9 +39,9 @@ include_directories(SYSTEM ${orocos_kdl_INCLUDE_DIRS}) add_executable(callPlanner src/call_planner.cpp src/collision_space_scene.cpp) target_link_libraries(callPlanner ${catkin_LIBRARIES} smpl::smpl) -add_executable(call_ompl_planner src/call_ompl_planner.cpp src/collision_space_scene.cpp) -target_include_directories(call_ompl_planner SYSTEM PRIVATE ${OMPL_INCLUDE_DIRS}) -target_link_libraries(call_ompl_planner ${catkin_LIBRARIES} ${OMPL_LIBRARIES} smpl::smpl) +#add_executable(call_ompl_planner src/call_ompl_planner.cpp src/collision_space_scene.cpp) +#target_include_directories(call_ompl_planner SYSTEM PRIVATE ${OMPL_INCLUDE_DIRS}) +#target_link_libraries(call_ompl_planner ${catkin_LIBRARIES} ${OMPL_LIBRARIES} smpl::smpl) add_executable(occupancy_grid_test src/occupancy_grid_test.cpp) target_link_libraries(occupancy_grid_test ${catkin_LIBRARIES} smpl::smpl) @@ -73,8 +73,8 @@ target_link_libraries(xytheta smpl::smpl) add_executable(debug_vis_demo src/debug_vis_demo.cpp) target_link_libraries(debug_vis_demo ${catkin_LIBRARIES} smpl::smpl) -add_executable(distance_map_test src/distance_map_test.cpp) -target_link_libraries(distance_map_test ${catkin_LIBRARIES} ${Boost_LIBRARIES} smpl::smpl) +#add_executable(distance_map_test src/distance_map_test.cpp) +#target_link_libraries(distance_map_test ${catkin_LIBRARIES} ${Boost_LIBRARIES} smpl::smpl) install( TARGETS callPlanner diff --git a/smpl_test/src/collision_space_scene.cpp b/smpl_test/src/collision_space_scene.cpp index 73804aeb..3619e69d 100644 --- a/smpl_test/src/collision_space_scene.cpp +++ b/smpl_test/src/collision_space_scene.cpp @@ -26,7 +26,7 @@ auto ConvertCollisionObjectToObject(const moveit_msgs::CollisionObject& co) return nullptr; } - Eigen::Affine3d transform; + Eigen::Isometry3d transform; tf::poseMsgToEigen(pose, transform); o->shapes_.push_back(sp); @@ -43,7 +43,7 @@ auto ConvertCollisionObjectToObject(const moveit_msgs::CollisionObject& co) return nullptr; } - Eigen::Affine3d transform; + Eigen::Isometry3d transform; tf::poseMsgToEigen(pose, transform); o->shapes_.push_back(sp); @@ -60,7 +60,7 @@ auto ConvertCollisionObjectToObject(const moveit_msgs::CollisionObject& co) return nullptr; } - Eigen::Affine3d transform; + Eigen::Isometry3d transform; tf::poseMsgToEigen(pose, transform); o->shapes_.push_back(sp); @@ -90,7 +90,7 @@ auto ConvertOctomapToObject(const octomap_msgs::OctomapWithPose& octomap) decltype(shapes::OcTree().octree) ot(tree); // snap into a shared_ptr shapes::ShapeConstPtr sp(new shapes::OcTree(ot)); // snap into a shape - Eigen::Affine3d transform; + Eigen::Isometry3d transform; tf::poseMsgToEigen(octomap.origin, transform); // construct the object diff --git a/smpl_urdf_robot_model/include/smpl_urdf_robot_model/robot_model.h b/smpl_urdf_robot_model/include/smpl_urdf_robot_model/robot_model.h index bc87a870..4145ab2e 100644 --- a/smpl_urdf_robot_model/include/smpl_urdf_robot_model/robot_model.h +++ b/smpl_urdf_robot_model/include/smpl_urdf_robot_model/robot_model.h @@ -3,6 +3,7 @@ // standard includes #include +#include // system includes #include From 1fb2afb7bbf5f82ede13b0748429b6001c44a2ce Mon Sep 17 00:00:00 2001 From: Fahad Islam Date: Thu, 21 May 2020 13:23:33 -0400 Subject: [PATCH 3/9] added collision model and launch example for franka --- .../config/collision_model_franka.yaml | 134 ++++++++++++++++++ smpl_test/config/franka.mprim | 12 ++ smpl_test/config/franka_arm.yaml | 17 +++ smpl_test/experiments/franka_goal.yaml | 35 +++++ smpl_test/launch/goal_franka.launch | 48 +++++++ smpl_test/src/call_planner.cpp | 9 ++ .../src/franka_allowed_collision_pairs.h | 19 +++ 7 files changed, 274 insertions(+) create mode 100644 sbpl_collision_checking_test/config/collision_model_franka.yaml create mode 100644 smpl_test/config/franka.mprim create mode 100644 smpl_test/config/franka_arm.yaml create mode 100644 smpl_test/experiments/franka_goal.yaml create mode 100644 smpl_test/launch/goal_franka.launch create mode 100644 smpl_test/src/franka_allowed_collision_pairs.h diff --git a/sbpl_collision_checking_test/config/collision_model_franka.yaml b/sbpl_collision_checking_test/config/collision_model_franka.yaml new file mode 100644 index 00000000..5baad55f --- /dev/null +++ b/sbpl_collision_checking_test/config/collision_model_franka.yaml @@ -0,0 +1,134 @@ +# 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: world_joint + type: planar # 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 } + +# 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 +# allowed_collisions: +# - { sphere1: l70, sphere2: l81 } diff --git a/smpl_test/config/franka.mprim b/smpl_test/config/franka.mprim new file mode 100644 index 00000000..2274d156 --- /dev/null +++ b/smpl_test/config/franka.mprim @@ -0,0 +1,12 @@ +Motion_Primitives(degrees): 11 7 7 + 7 0 0 0 0 0 0 + 0 7 0 0 0 0 0 + 0 0 7 0 0 0 0 + 0 0 0 7 0 0 0 + 4 0 0 0 0 0 0 + 0 4 0 0 0 0 0 + 0 0 4 0 0 0 0 + 0 0 0 4 0 0 0 + 0 0 0 0 4 0 0 + 0 0 0 0 0 4 0 + 0 0 0 0 0 0 4 diff --git a/smpl_test/config/franka_arm.yaml b/smpl_test/config/franka_arm.yaml new file mode 100644 index 00000000..d60d7bf5 --- /dev/null +++ b/smpl_test/config/franka_arm.yaml @@ -0,0 +1,17 @@ +planning: + discretization: + panda_joint1 0.017453292519943295 + panda_joint2 0.017453292519943295 + panda_joint3 0.017453292519943295 + panda_joint4 0.017453292519943295 + panda_joint5 0.017453292519943295 + panda_joint6 0.017453292519943295 + panda_joint7 0.017453292519943295 + use_xyz_snap_mprim: false + use_rpy_snap_mprim: false + use_xyzrpy_snap_mprim: true + use_short_dist_mprims: true + xyz_snap_dist_thresh: 0.2 + rpy_snap_dist_thresh: 0.0 + xyzrpy_snap_dist_thresh: 0.0 + short_dist_mprims_thresh: 0.4 diff --git a/smpl_test/experiments/franka_goal.yaml b/smpl_test/experiments/franka_goal.yaml new file mode 100644 index 00000000..e03210f6 --- /dev/null +++ b/smpl_test/experiments/franka_goal.yaml @@ -0,0 +1,35 @@ +initial_configuration: + joint_state: + - { name: panda_joint1, position: 0.0 } + - { name: panda_joint2, position: 0.0 } + - { name: panda_joint3, position: 0.0 } + - { name: panda_joint4, position: -1.0 } + - { name: panda_joint5, position: 0.0 } #-0.2 } + - { name: panda_joint6, position: 1.5 } + - { name: panda_joint7, position: 0.0 } #-0.2 } + +# multi_dof_joint_state: +# - frame_id: map +# child_frame_id: torso_lift_link +# x: -0.05 +# y: 1.0 +# z: 0.789675 +# roll: 0.0 +# pitch: 0.0 +# yaw: 0.0 +# - frame_id: base_footprint +# child_frame_id: torso_lift_link +# x: -0.05 +# y: 0.0 +# z: 0.789675 +# roll: 0.0 +# pitch: 0.0 +# yaw: 0.0 + +goal: + x: 0.4 + y: -0.2 + z: 0.36 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 diff --git a/smpl_test/launch/goal_franka.launch b/smpl_test/launch/goal_franka.launch new file mode 100644 index 00000000..41be75b3 --- /dev/null +++ b/smpl_test/launch/goal_franka.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + group_name: manipulator + planning_joints: + panda_joint1 + panda_joint2 + panda_joint3 + panda_joint4 + panda_joint5 + panda_joint6 + panda_joint7 + kinematics_frame: + panda_link0 + chain_tip_link: + panda_hand + + + + + + + + + + + + + + + + + + + + + diff --git a/smpl_test/src/call_planner.cpp b/smpl_test/src/call_planner.cpp index dd87c356..7732c88c 100644 --- a/smpl_test/src/call_planner.cpp +++ b/smpl_test/src/call_planner.cpp @@ -55,6 +55,7 @@ #include "collision_space_scene.h" #include "pr2_allowed_collision_pairs.h" +#include "franka_allowed_collision_pairs.h" void FillGoalConstraint( const std::vector& pose, @@ -519,6 +520,14 @@ int main(int argc, char* argv[]) cc.setAllowedCollisionMatrix(acm); } + if (cc.robotCollisionModel()->name() == "panda") { + smpl::collision::AllowedCollisionMatrix acm; + for (auto& pair : FrankaAllowedCollisionPairs) { + acm.setEntry(pair.first, pair.second, true); + } + cc.setAllowedCollisionMatrix(acm); + } + ///////////////// // Setup Scene // ///////////////// diff --git a/smpl_test/src/franka_allowed_collision_pairs.h b/smpl_test/src/franka_allowed_collision_pairs.h new file mode 100644 index 00000000..61dc24cf --- /dev/null +++ b/smpl_test/src/franka_allowed_collision_pairs.h @@ -0,0 +1,19 @@ +#ifndef FRANKA_ALLOWED_COLLISION_PAIRS_H +#define FRANKA_ALLOWED_COLLISION_PAIRS_H + +#include + +// Copied from pr2.srdf +const std::pair FrankaAllowedCollisionPairs[] = +{ + { "panda_link0", "panda_link1" }, + { "panda_link1", "panda_link2" }, + { "panda_link2", "panda_link3" }, + { "panda_link3", "panda_link4" }, + { "panda_link4", "panda_link5" }, + { "panda_link5", "panda_link6" }, + { "panda_link6", "panda_link7" }, + { "panda_link7", "panda_hand" } +}; + +#endif From bf66eeec745cd85a3e8dc6840dad9d3ef09aa933 Mon Sep 17 00:00:00 2001 From: Fahad Islam Date: Tue, 9 Jun 2020 10:25:22 -0400 Subject: [PATCH 4/9] added catkin ignores --- smpl_moveit_interface/CATKIN_IGNORE | 0 smpl_moveit_interface/package_inactive.xml | 38 ---------------------- smpl_ompl_interface/CATKIN_IGNORE | 0 3 files changed, 38 deletions(-) create mode 100644 smpl_moveit_interface/CATKIN_IGNORE delete mode 100644 smpl_moveit_interface/package_inactive.xml create mode 100644 smpl_ompl_interface/CATKIN_IGNORE diff --git a/smpl_moveit_interface/CATKIN_IGNORE b/smpl_moveit_interface/CATKIN_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/smpl_moveit_interface/package_inactive.xml b/smpl_moveit_interface/package_inactive.xml deleted file mode 100644 index 1227bff2..00000000 --- a/smpl_moveit_interface/package_inactive.xml +++ /dev/null @@ -1,38 +0,0 @@ - - smpl_moveit_interface - 0.0.0 - MoveIt interface to SBPL - Andrew Dornbush - Andrew Dornbush - - BSD - - catkin - - actionlib - boost - eigen_conversions - geometric_shapes - interactive_markers - moveit_core - moveit_msgs - moveit_ros_planning - roscpp - rviz - smpl - smpl_ros - sbpl_collision_checking - sensor_msgs - pluginlib - visualization_msgs - - libqt4-dev - - libqt4 - - - - - - - diff --git a/smpl_ompl_interface/CATKIN_IGNORE b/smpl_ompl_interface/CATKIN_IGNORE new file mode 100644 index 00000000..e69de29b From a3b6403a8cd84c00cb4058e68525cfad798054f6 Mon Sep 17 00:00:00 2001 From: Shohin Mukherjee Date: Mon, 7 Sep 2020 07:29:59 -0400 Subject: [PATCH 5/9] Added franka collision models --- .../config/collision_model_franka.yaml | 31 ++++++++++++++++--- smpl_ompl_interface/CATKIN_IGNORE | 0 smpl_test/experiments/franka_goal.yaml | 26 +++++++++++----- .../src/franka_allowed_collision_pairs.h | 5 ++- 4 files changed, 48 insertions(+), 14 deletions(-) delete mode 100644 smpl_ompl_interface/CATKIN_IGNORE diff --git a/sbpl_collision_checking_test/config/collision_model_franka.yaml b/sbpl_collision_checking_test/config/collision_model_franka.yaml index 5baad55f..264383d9 100644 --- a/sbpl_collision_checking_test/config/collision_model_franka.yaml +++ b/sbpl_collision_checking_test/config/collision_model_franka.yaml @@ -11,8 +11,8 @@ world_collision_model: robot_collision_model: world_joint: - name: world_joint - type: planar # fixed, floating + name: map + type: floating # fixed, floating voxels_models: - { link_name: panda_link0, res: 0.01 } - { link_name: panda_link1, res: 0.01 } @@ -23,8 +23,8 @@ robot_collision_model: - { 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 } + - { link_name: panda_leftfinger, res: 0.01 } + - { link_name: panda_rightfinger, res: 0.01 } spheres_models: - link_name: panda_link0 auto: false @@ -112,7 +112,26 @@ robot_collision_model: - { 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: @@ -130,5 +149,7 @@ robot_collision_model: - name: panda_link6 - name: panda_link7 - name: panda_hand + - name: panda_rightfinger + - name: panda_leftfinger # allowed_collisions: # - { sphere1: l70, sphere2: l81 } diff --git a/smpl_ompl_interface/CATKIN_IGNORE b/smpl_ompl_interface/CATKIN_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/smpl_test/experiments/franka_goal.yaml b/smpl_test/experiments/franka_goal.yaml index e03210f6..e934e92d 100644 --- a/smpl_test/experiments/franka_goal.yaml +++ b/smpl_test/experiments/franka_goal.yaml @@ -1,12 +1,22 @@ initial_configuration: joint_state: - { name: panda_joint1, position: 0.0 } - - { name: panda_joint2, position: 0.0 } + - { name: panda_joint2, position: -0.78539816 } - { name: panda_joint3, position: 0.0 } - - { name: panda_joint4, position: -1.0 } + - { name: panda_joint4, position: -2.35619449 } - { name: panda_joint5, position: 0.0 } #-0.2 } - - { name: panda_joint6, position: 1.5 } - - { name: panda_joint7, position: 0.0 } #-0.2 } + - { name: panda_joint6, position: 1.57079633, } + - { name: panda_joint7, position: 0.78539816 } #-0.2 } + - { name: panda_finger_joint1, position: 0.04} + - { name: panda_finger_joint2, position: 0.04} + - { name: map/trans_x, position: 0} + - { name: map/trans_y, position: 0.51} + - { name: map/trans_z, position: 0} + - { name: map/rot_x, position: -0.70710677} + - { name: map/rot_y, position: 0} + - { name: map/rot_z, position: 0} + - { name: map/rot_w, position: 0.70710677} + # multi_dof_joint_state: # - frame_id: map @@ -27,9 +37,9 @@ initial_configuration: # yaw: 0.0 goal: - x: 0.4 - y: -0.2 - z: 0.36 - roll: 0.0 + x: 0.50 + y: 0.63 + z: 0.00 + roll: 1.571 pitch: 0.0 yaw: 0.0 diff --git a/smpl_test/src/franka_allowed_collision_pairs.h b/smpl_test/src/franka_allowed_collision_pairs.h index 61dc24cf..8feb06b2 100644 --- a/smpl_test/src/franka_allowed_collision_pairs.h +++ b/smpl_test/src/franka_allowed_collision_pairs.h @@ -13,7 +13,10 @@ const std::pair FrankaAllowedCollisionPairs[] = { "panda_link4", "panda_link5" }, { "panda_link5", "panda_link6" }, { "panda_link6", "panda_link7" }, - { "panda_link7", "panda_hand" } + { "panda_link7", "panda_hand" }, + { "panda_hand", "panda_leftfinger"}, + { "panda_hand", "panda_rightfinger"}, + {"panda_leftfinger", "panda_rightfinger"} }; #endif From 16dc8de199e2d49e9009c5e7152e5523c5f209dd Mon Sep 17 00:00:00 2001 From: Shohin Mukherjee Date: Tue, 15 Sep 2020 12:03:49 -0400 Subject: [PATCH 6/9] Removed CATKIN_IGNORE of smpl_moveit_interface --- smpl_moveit_interface/CATKIN_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 smpl_moveit_interface/CATKIN_IGNORE diff --git a/smpl_moveit_interface/CATKIN_IGNORE b/smpl_moveit_interface/CATKIN_IGNORE deleted file mode 100644 index e69de29b..00000000 From f17c907f8695ecc2d6f9ba1265f3ba0b626a6a02 Mon Sep 17 00:00:00 2001 From: Shohin Mukherjee Date: Sun, 20 Dec 2020 17:40:09 -0500 Subject: [PATCH 7/9] Fixed behaviour of SelfCollisionModel::updateAllowedCollisionMatrix --- sbpl_collision_checking/src/self_collision_model.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/sbpl_collision_checking/src/self_collision_model.cpp b/sbpl_collision_checking/src/self_collision_model.cpp index 7b0fabff..79b5e336 100644 --- a/sbpl_collision_checking/src/self_collision_model.cpp +++ b/sbpl_collision_checking/src/self_collision_model.cpp @@ -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); } + } } } From 708829717df70131f00b29a908fba0825f650d64 Mon Sep 17 00:00:00 2001 From: Shohin Mukherjee Date: Sun, 20 Dec 2020 17:41:05 -0500 Subject: [PATCH 8/9] Added attached object visualization in visulization of robot collision model --- sbpl_collision_checking/src/collision_space.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/sbpl_collision_checking/src/collision_space.cpp b/sbpl_collision_checking/src/collision_space.cpp index 2b56c0ea..37e46719 100644 --- a/sbpl_collision_checking/src/collision_space.cpp +++ b/sbpl_collision_checking/src/collision_space.cpp @@ -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; } From f7321629c3acbbbc4a83013299f02556bf61dee6 Mon Sep 17 00:00:00 2001 From: Shohin Mukherjee Date: Fri, 19 Feb 2021 09:31:37 -0500 Subject: [PATCH 9/9] Joining the bfs3d search thread so it can be reassigned --- smpl/src/bfs3d/bfs3d.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/smpl/src/bfs3d/bfs3d.cpp b/smpl/src/bfs3d/bfs3d.cpp index 32e15740..76f30f2b 100644 --- a/smpl/src/bfs3d/bfs3d.cpp +++ b/smpl/src/bfs3d/bfs3d.cpp @@ -179,6 +179,10 @@ void BFS_3D::run(int x, int y, int z) m_running = true; + if (m_search_thread.joinable()) { + m_search_thread.join(); + } + // fire off background thread to compute bfs m_search_thread = std::thread([&]() {