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/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; } diff --git a/sbpl_collision_checking/src/collision_space.cpp b/sbpl_collision_checking/src/collision_space.cpp index d98bd2f7..37e46719 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); @@ -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; } 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/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); } + } } } 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..264383d9 --- /dev/null +++ b/sbpl_collision_checking_test/config/collision_model_franka.yaml @@ -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 } 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/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([&]() { diff --git a/smpl_moveit_interface/package.xml b/smpl_moveit_interface/package.xml deleted file mode 100644 index 1227bff2..00000000 --- a/smpl_moveit_interface/package.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_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/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..e934e92d --- /dev/null +++ b/smpl_test/experiments/franka_goal.yaml @@ -0,0 +1,45 @@ +initial_configuration: + joint_state: + - { name: panda_joint1, position: 0.0 } + - { name: panda_joint2, position: -0.78539816 } + - { name: panda_joint3, position: 0.0 } + - { name: panda_joint4, position: -2.35619449 } + - { name: panda_joint5, 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 +# 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.50 + y: 0.63 + z: 0.00 + roll: 1.571 + 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/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_test/src/franka_allowed_collision_pairs.h b/smpl_test/src/franka_allowed_collision_pairs.h new file mode 100644 index 00000000..8feb06b2 --- /dev/null +++ b/smpl_test/src/franka_allowed_collision_pairs.h @@ -0,0 +1,22 @@ +#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" }, + { "panda_hand", "panda_leftfinger"}, + { "panda_hand", "panda_rightfinger"}, + {"panda_leftfinger", "panda_rightfinger"} +}; + +#endif 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