diff --git a/CMakeLists.txt b/CMakeLists.txt index c4a1f6e..01c0a23 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,6 +6,9 @@ # cmake requirements cmake_minimum_required(VERSION 2.8) +if(COMMAND cmake_policy) + cmake_policy(SET CMP0003 NEW) +endif(COMMAND cmake_policy) # Build options have to be before PROJECT(...) SET(CMAKE_CONFIGURATION_TYPES "Debug;Release" CACHE PATH "Configuration types") @@ -35,6 +38,29 @@ else(CMAKE_SIZEOF_VOID_P EQUAL 8) #MESSAGE("Architecture x86 32 bit") endif(CMAKE_SIZEOF_VOID_P EQUAL 8) +############################################################################### +# Core libraries + +option(BUILD_SPAM_CORE "Build Spam Core library" YES) +option(BUILD_SPAM_HBPLAN "Build Spam hypothesis-based planning library" YES) + +############################################################################### +# Applications +option(BUILD_SPAM_APP "Build Spam application library" YES) +option(BUILD_SPAM_POSEPLANNER "Build Spam pose planner application" YES) +option(BUILD_SPAM_R2GPLANNER "Build Spam RAG planner application" YES) + +############################################################################### +# Data Libraries +option(BUILD_SPAM_DATA_BELIEF "Build Spam data belief library" YES) +option(BUILD_SPAM_DATA_R2GTRAJECTORY "Build Spam data R2G trajectory library" YES) + +############################################################################### +# Demos +option(BUILD_SPAM_DEMO_RAG "Build Spam demo reach-to-grasp library" YES) + +############################################################################### + # Traget names SET(CMAKE_RELEASE_POSTFIX "" CACHE PATH "Release postfix") mark_as_advanced(CMAKE_RELEASE_POSTFIX) @@ -99,7 +125,7 @@ if (WIN32) SET(MSVC_VER "110") elseif(MSVC12) SET(MSVC_VER "120") - endif() + endif(MSVC90) # Build options SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /D_CRT_SECURE_NO_DEPRECATE") @@ -214,7 +240,7 @@ elseif (UNIX) FIND_PACKAGE(OpenCV REQUIRED) # PCL FIND_PACKAGE(PCL REQUIRED) -endif() +endif(WIN32) ADD_DEFINITIONS( ${OPENCV_DEFINITIONS} @@ -247,147 +273,408 @@ INCLUDE_DIRECTORIES( ${GRASP_INCLUDE} ) -# Spam library -SET(SPAM_HEADERS - ${PROJECT_ROOT}/include/Spam/Spam/Belief.h - ${PROJECT_ROOT}/include/Spam/Spam/Spam.h - ${PROJECT_ROOT}/include/Spam/Spam/GraphPlanner.h - ${PROJECT_ROOT}/include/Spam/Spam/Heuristic.h -) -SET(SPAM_SOURCES - ${PROJECT_ROOT}/src/Spam/Spam/Belief.cpp - ${PROJECT_ROOT}/src/Spam/Spam/Spam.cpp - ${PROJECT_ROOT}/src/Spam/Spam/GraphPlanner.cpp - ${PROJECT_ROOT}/src/Spam/Spam/Heuristic.cpp -) -if(BUILD_DYNAMIC_LIBS) - ADD_LIBRARY(SpamSpam SHARED ${SPAM_HEADERS} ${SPAM_SOURCES}) - #SET_TARGET_PROPERTIES(SpamSpam PROPERTIES COMPILE_FLAGS ${CMAKE_DLL_EXPORT_FLAGS}) -else(BUILD_DYNAMIC_LIBS) - ADD_LIBRARY(SpamSpam STATIC ${SPAM_HEADERS} ${SPAM_SOURCES}) -endif(BUILD_DYNAMIC_LIBS) -if (WIN32) - TARGET_LINK_LIBRARIES( - SpamSpam - optimized GraspCore${CMAKE_RELEASE_POSTFIX} debug GraspCore${CMAKE_DEBUG_POSTFIX} - optimized GraspContact${CMAKE_RELEASE_POSTFIX} debug GraspContact${CMAKE_DEBUG_POSTFIX} - #optimized GraspGrasp${CMAKE_RELEASE_POSTFIX} debug GraspGrasp${CMAKE_DEBUG_POSTFIX} - optimized GraspApp${CMAKE_RELEASE_POSTFIX} debug GraspApp${CMAKE_DEBUG_POSTFIX} - optimized GraspActiveCtrl${CMAKE_RELEASE_POSTFIX} debug GraspActiveCtrl${CMAKE_DEBUG_POSTFIX} - optimized GraspArmHandForce${CMAKE_RELEASE_POSTFIX} debug GraspArmHandForce${CMAKE_DEBUG_POSTFIX} - #NxCharacter64 PhysXCooking64 PhysXCore64 PhysXLoader64 - expat freeglut Gdiplus - optimized GolemDefs${CMAKE_RELEASE_POSTFIX} debug GolemDefs${CMAKE_DEBUG_POSTFIX} - optimized GolemMath${CMAKE_RELEASE_POSTFIX} debug GolemMath${CMAKE_DEBUG_POSTFIX} - optimized GolemSys${CMAKE_RELEASE_POSTFIX} debug GolemSys${CMAKE_DEBUG_POSTFIX} - optimized GolemTools${CMAKE_RELEASE_POSTFIX} debug GolemTools${CMAKE_DEBUG_POSTFIX} - optimized GolemCtrl${CMAKE_RELEASE_POSTFIX} debug GolemCtrl${CMAKE_DEBUG_POSTFIX} - optimized GolemPlan${CMAKE_RELEASE_POSTFIX} debug GolemPlan${CMAKE_DEBUG_POSTFIX} - optimized GolemUI${CMAKE_RELEASE_POSTFIX} debug GolemUI${CMAKE_DEBUG_POSTFIX} - optimized GolemUICtrl${CMAKE_RELEASE_POSTFIX} debug GolemUICtrl${CMAKE_DEBUG_POSTFIX} - optimized GolemDeviceSM${CMAKE_RELEASE_POSTFIX} debug GolemDeviceSM${CMAKE_DEBUG_POSTFIX} - optimized GolemDeviceSingleCtrl${CMAKE_RELEASE_POSTFIX} debug GolemDeviceSingleCtrl${CMAKE_DEBUG_POSTFIX} - optimized CamcalbCalb${CMAKE_RELEASE_POSTFIX} debug CamcalbCalb${CMAKE_DEBUG_POSTFIX} - optimized CamcalbMatas${CMAKE_RELEASE_POSTFIX} debug CamcalbMatas${CMAKE_DEBUG_POSTFIX} - ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${OpenCV_LIBS} +############################################################################### +# +# CORE library +# +############################################################################### +IF (BUILD_SPAM_CORE) + SET(CORE_HEADERS + ${PROJECT_ROOT}/include/Spam/Core/Collision.h + ${PROJECT_ROOT}/include/Spam/Core/JContact.h ) -elseif (UNIX) - TARGET_LINK_LIBRARIES( - SpamSpam - ${PCL_LIBRARIES} - optimized CamcalbCalb${CMAKE_RELEASE_POSTFIX} debug CamcalbCalb${CMAKE_DEBUG_POSTFIX} - optimized CamcalbMatas${CMAKE_RELEASE_POSTFIX} debug CamcalbMatas${CMAKE_DEBUG_POSTFIX} - ${OpenCV_LIBS} - optimized GraspCore${CMAKE_RELEASE_POSTFIX} debug GraspCore${CMAKE_DEBUG_POSTFIX} - optimized GraspContact${CMAKE_RELEASE_POSTFIX} debug GraspContact${CMAKE_DEBUG_POSTFIX} - #optimized GraspGrasp${CMAKE_RELEASE_POSTFIX} debug GraspGrasp${CMAKE_DEBUG_POSTFIX} - optimized GraspApp${CMAKE_RELEASE_POSTFIX} debug GraspApp${CMAKE_DEBUG_POSTFIX} - optimized GraspActiveCtrl${CMAKE_RELEASE_POSTFIX} debug GraspActiveCtrl${CMAKE_DEBUG_POSTFIX} - optimized GraspArmHandForce${CMAKE_RELEASE_POSTFIX} debug GraspArmHandForce${CMAKE_DEBUG_POSTFIX} - ${Boost_LIBRARIES} - optimized GolemDefs${CMAKE_RELEASE_POSTFIX} debug GolemDefs${CMAKE_DEBUG_POSTFIX} - optimized GolemSys${CMAKE_RELEASE_POSTFIX} debug GolemSys${CMAKE_DEBUG_POSTFIX} - optimized GolemTools${CMAKE_RELEASE_POSTFIX} debug GolemTools${CMAKE_DEBUG_POSTFIX} - optimized GolemCtrl${CMAKE_RELEASE_POSTFIX} debug GolemCtrl${CMAKE_DEBUG_POSTFIX} - optimized GolemPlan${CMAKE_RELEASE_POSTFIX} debug GolemPlan${CMAKE_DEBUG_POSTFIX} - optimized GolemUICtrl${CMAKE_RELEASE_POSTFIX} debug GolemUICtrl${CMAKE_DEBUG_POSTFIX} - optimized GolemDeviceSM${CMAKE_RELEASE_POSTFIX} debug GolemDeviceSM${CMAKE_DEBUG_POSTFIX} - optimized GolemDeviceSingleCtrl${CMAKE_RELEASE_POSTFIX} debug GolemDeviceSingleCtrl${CMAKE_DEBUG_POSTFIX} - ${CMAKE_DL_LIBS} - optimized GolemMath${CMAKE_RELEASE_POSTFIX} debug GolemMath${CMAKE_DEBUG_POSTFIX} - optimized GolemUI${CMAKE_RELEASE_POSTFIX} debug GolemUI${CMAKE_DEBUG_POSTFIX} - GL - GLU - glut - #NxCharacter NxCooking PhysXCore PhysXLoader + SET(CORE_SOURCES + ${PROJECT_ROOT}/src/Spam/Core/Collision.cpp + ${PROJECT_ROOT}/src/Spam/Core/JContact.cpp + ) + if(BUILD_DYNAMIC_LIBS) + ADD_LIBRARY(SpamCore SHARED ${CORE_HEADERS} ${CORE_SOURCES}) + #SET_TARGET_PROPERTIES(SpamSpam PROPERTIES COMPILE_FLAGS ${CMAKE_DLL_EXPORT_FLAGS}) + else(BUILD_DYNAMIC_LIBS) + ADD_LIBRARY(SpamCore STATIC ${CORE_HEADERS} ${CORE_SOURCES}) + endif(BUILD_DYNAMIC_LIBS) + if (WIN32) + TARGET_LINK_LIBRARIES( + SpamCore + optimized GraspCore${CMAKE_RELEASE_POSTFIX} debug GraspCore${CMAKE_DEBUG_POSTFIX} + optimized GraspContact${CMAKE_RELEASE_POSTFIX} debug GraspContact${CMAKE_DEBUG_POSTFIX} + #optimized GraspGrasp${CMAKE_RELEASE_POSTFIX} debug GraspGrasp${CMAKE_DEBUG_POSTFIX} + optimized GraspApp${CMAKE_RELEASE_POSTFIX} debug GraspApp${CMAKE_DEBUG_POSTFIX} + optimized GraspActiveCtrl${CMAKE_RELEASE_POSTFIX} debug GraspActiveCtrl${CMAKE_DEBUG_POSTFIX} + optimized GraspArmHandForce${CMAKE_RELEASE_POSTFIX} debug GraspArmHandForce${CMAKE_DEBUG_POSTFIX} + #NxCharacter64 PhysXCooking64 PhysXCore64 PhysXLoader64 + expat freeglut Gdiplus + optimized GolemDefs${CMAKE_RELEASE_POSTFIX} debug GolemDefs${CMAKE_DEBUG_POSTFIX} + optimized GolemMath${CMAKE_RELEASE_POSTFIX} debug GolemMath${CMAKE_DEBUG_POSTFIX} + optimized GolemSys${CMAKE_RELEASE_POSTFIX} debug GolemSys${CMAKE_DEBUG_POSTFIX} + optimized GolemTools${CMAKE_RELEASE_POSTFIX} debug GolemTools${CMAKE_DEBUG_POSTFIX} + optimized GolemCtrl${CMAKE_RELEASE_POSTFIX} debug GolemCtrl${CMAKE_DEBUG_POSTFIX} + optimized GolemPlan${CMAKE_RELEASE_POSTFIX} debug GolemPlan${CMAKE_DEBUG_POSTFIX} + optimized GolemUI${CMAKE_RELEASE_POSTFIX} debug GolemUI${CMAKE_DEBUG_POSTFIX} + optimized GolemUICtrl${CMAKE_RELEASE_POSTFIX} debug GolemUICtrl${CMAKE_DEBUG_POSTFIX} + optimized GolemDeviceSM${CMAKE_RELEASE_POSTFIX} debug GolemDeviceSM${CMAKE_DEBUG_POSTFIX} + optimized GolemDeviceSingleCtrl${CMAKE_RELEASE_POSTFIX} debug GolemDeviceSingleCtrl${CMAKE_DEBUG_POSTFIX} + optimized CamcalbCalb${CMAKE_RELEASE_POSTFIX} debug CamcalbCalb${CMAKE_DEBUG_POSTFIX} + optimized CamcalbMatas${CMAKE_RELEASE_POSTFIX} debug CamcalbMatas${CMAKE_DEBUG_POSTFIX} + ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${OpenCV_LIBS} + ) + elseif (UNIX) + TARGET_LINK_LIBRARIES( + SpamCore + ${PCL_LIBRARIES} + optimized CamcalbCalb${CMAKE_RELEASE_POSTFIX} debug CamcalbCalb${CMAKE_DEBUG_POSTFIX} + optimized CamcalbMatas${CMAKE_RELEASE_POSTFIX} debug CamcalbMatas${CMAKE_DEBUG_POSTFIX} + ${OpenCV_LIBS} + optimized GraspCore${CMAKE_RELEASE_POSTFIX} debug GraspCore${CMAKE_DEBUG_POSTFIX} + optimized GraspContact${CMAKE_RELEASE_POSTFIX} debug GraspContact${CMAKE_DEBUG_POSTFIX} + #optimized GraspGrasp${CMAKE_RELEASE_POSTFIX} debug GraspGrasp${CMAKE_DEBUG_POSTFIX} + optimized GraspApp${CMAKE_RELEASE_POSTFIX} debug GraspApp${CMAKE_DEBUG_POSTFIX} + optimized GraspActiveCtrl${CMAKE_RELEASE_POSTFIX} debug GraspActiveCtrl${CMAKE_DEBUG_POSTFIX} + optimized GraspArmHandForce${CMAKE_RELEASE_POSTFIX} debug GraspArmHandForce${CMAKE_DEBUG_POSTFIX} + ${Boost_LIBRARIES} + optimized GolemDefs${CMAKE_RELEASE_POSTFIX} debug GolemDefs${CMAKE_DEBUG_POSTFIX} + optimized GolemSys${CMAKE_RELEASE_POSTFIX} debug GolemSys${CMAKE_DEBUG_POSTFIX} + optimized GolemTools${CMAKE_RELEASE_POSTFIX} debug GolemTools${CMAKE_DEBUG_POSTFIX} + optimized GolemCtrl${CMAKE_RELEASE_POSTFIX} debug GolemCtrl${CMAKE_DEBUG_POSTFIX} + optimized GolemPlan${CMAKE_RELEASE_POSTFIX} debug GolemPlan${CMAKE_DEBUG_POSTFIX} + optimized GolemUICtrl${CMAKE_RELEASE_POSTFIX} debug GolemUICtrl${CMAKE_DEBUG_POSTFIX} + optimized GolemDeviceSM${CMAKE_RELEASE_POSTFIX} debug GolemDeviceSM${CMAKE_DEBUG_POSTFIX} + optimized GolemDeviceSingleCtrl${CMAKE_RELEASE_POSTFIX} debug GolemDeviceSingleCtrl${CMAKE_DEBUG_POSTFIX} + ${CMAKE_DL_LIBS} + optimized GolemMath${CMAKE_RELEASE_POSTFIX} debug GolemMath${CMAKE_DEBUG_POSTFIX} + optimized GolemUI${CMAKE_RELEASE_POSTFIX} debug GolemUI${CMAKE_DEBUG_POSTFIX} + GL + GLU + glut + #NxCharacter NxCooking PhysXCore PhysXLoader + ) + endif(WIN32) + SET_PROPERTY(TARGET SpamCore PROPERTY RELEASE_POSTFIX ${CMAKE_RELEASE_POSTFIX}) + SET_PROPERTY(TARGET SpamCore PROPERTY DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) + SET_PROPERTY(TARGET SpamCore PROPERTY PROJECT_LABEL "Core") + SET_PROPERTY(TARGET SpamCore PROPERTY FOLDER "Spam") + if(BUILD_DYNAMIC_LIBS) + if (WIN32) + INSTALL(TARGETS SpamCore RUNTIME DESTINATION bin LIBRARY DESTINATION bin ARCHIVE DESTINATION core_libs) + elseif (UNIX) + INSTALL(TARGETS SpamCore LIBRARY DESTINATION bin LIBRARY DESTINATION bin ARCHIVE DESTINATION core_libs) + endif() + else(BUILD_DYNAMIC_LIBS) + INSTALL(TARGETS SpamCore ARCHIVE DESTINATION lib COMPONENT core_libs) + endif(BUILD_DYNAMIC_LIBS) + INSTALL(FILES ${CORE_HEADERS} DESTINATION include/Spam/Core/ COMPONENT core_headers) + SOURCE_GROUP("Include Files" FILES ${CORE_HEADERS}) +ENDIF (BUILD_SPAM_CORE) + +############################################################################### +# +# HBPLAN library +# +############################################################################### +IF (BUILD_SPAM_HBPLAN) + SET(HBPLAN_HEADERS + ${PROJECT_ROOT}/include/Spam/HBPlan/Belief.h + ${PROJECT_ROOT}/include/Spam/HBPlan/GraphPlanner.h + ${PROJECT_ROOT}/include/Spam/HBPlan/Heuristic.h + ${PROJECT_ROOT}/include/Spam/HBPlan/Hypothesis.h + ) + SET(HBPLAN_SOURCES + ${PROJECT_ROOT}/src/Spam/HBPlan/Belief.cpp + ${PROJECT_ROOT}/src/Spam/HBPlan/GraphPlanner.cpp + ${PROJECT_ROOT}/src/Spam/HBPlan/Heuristic.cpp + ${PROJECT_ROOT}/src/Spam/HBPlan/Hypothesis.cpp + ) + if(BUILD_DYNAMIC_LIBS) + ADD_LIBRARY(SpamHBPlan SHARED ${HBPLAN_HEADERS} ${HBPLAN_SOURCES}) + #SET_TARGET_PROPERTIES(SpamSpam PROPERTIES COMPILE_FLAGS ${CMAKE_DLL_EXPORT_FLAGS}) + else(BUILD_DYNAMIC_LIBS) + ADD_LIBRARY(SpamHBPlan STATIC ${HBPLAN_HEADERS} ${HBPLAN_SOURCES}) + endif(BUILD_DYNAMIC_LIBS) + if (WIN32) + TARGET_LINK_LIBRARIES(SpamHBPlan SpamCore) + elseif (UNIX) + TARGET_LINK_LIBRARIES(SpamHBPlan SpamCore) + endif() + SET_PROPERTY(TARGET SpamHBPlan PROPERTY RELEASE_POSTFIX ${CMAKE_RELEASE_POSTFIX}) + SET_PROPERTY(TARGET SpamHBPlan PROPERTY DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) + SET_PROPERTY(TARGET SpamHBPlan PROPERTY PROJECT_LABEL "HBPlan") + SET_PROPERTY(TARGET SpamHBPlan PROPERTY FOLDER "Spam") + if(BUILD_DYNAMIC_LIBS) + if (WIN32) + INSTALL(TARGETS SpamHBPlan RUNTIME DESTINATION bin LIBRARY DESTINATION bin ARCHIVE DESTINATION core_libs) + elseif (UNIX) + INSTALL(TARGETS SpamHBPlan LIBRARY DESTINATION bin LIBRARY DESTINATION bin ARCHIVE DESTINATION core_libs) + endif() + else(BUILD_DYNAMIC_LIBS) + INSTALL(TARGETS SpamHBPlan ARCHIVE DESTINATION lib COMPONENT core_libs) + endif(BUILD_DYNAMIC_LIBS) + INSTALL(FILES ${HBPLAN_HEADERS} DESTINATION include/Spam/HBPlan/ COMPONENT core_headers) + SOURCE_GROUP("Include Files" FILES ${HBPLAN_HEADERS}) +ENDIF (BUILD_SPAM_HBPLAN) + +############################################################################### +# +# APP library +# +############################################################################### +IF (BUILD_SPAM_APP) + SET(APP_POSEPLANNER_HEADERS + ${PROJECT_ROOT}/include/Spam/App/PosePlanner/Data.h + ${PROJECT_ROOT}/include/Spam/App/PosePlanner/PosePlanner.h ) -endif() -SET_PROPERTY(TARGET SpamSpam PROPERTY RELEASE_POSTFIX ${CMAKE_RELEASE_POSTFIX}) -SET_PROPERTY(TARGET SpamSpam PROPERTY DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) -SET_PROPERTY(TARGET SpamSpam PROPERTY PROJECT_LABEL "Spam") -SET_PROPERTY(TARGET SpamSpam PROPERTY FOLDER "Spam") -INSTALL(TARGETS SpamSpam RUNTIME DESTINATION bin LIBRARY DESTINATION bin ARCHIVE DESTINATION lib) -INSTALL(FILES ${SPAM_HEADERS} DESTINATION include/Spam/Spam/) + SET(APP_R2GPlanner_HEADERS + ${PROJECT_ROOT}/include/Spam/App/R2GPlanner/Data.h + ${PROJECT_ROOT}/include/Spam/App/R2GPlanner/R2GPlanner.h + ) + SET(APP_SOURCES + ${PROJECT_ROOT}/src/Spam/App/PosePlanner/PosePlanner.cpp + ${PROJECT_ROOT}/src/Spam/App/R2GPlanner/R2GPlanner.cpp + ) + SET(APP_FILES + ) + + if(BUILD_DYNAMIC_LIBS) + ADD_LIBRARY(SpamApp SHARED ${APP_POSEPLANNER_HEADERS} ${APP_R2GPlanner_HEADERS} ${APP_SOURCES} ${APP_FILES}) + #SET_TARGET_PROPERTIES(SpamSpam PROPERTIES COMPILE_FLAGS ${CMAKE_DLL_EXPORT_FLAGS}) + else(BUILD_DYNAMIC_LIBS) + ADD_LIBRARY(SpamApp STATIC ${APP_POSEPLANNER_HEADERS} ${APP_R2GPlanner_HEADERS} ${APP_SOURCES}) + endif(BUILD_DYNAMIC_LIBS) + if (WIN32) + TARGET_LINK_LIBRARIES(SpamApp SpamHBPlan) + elseif (UNIX) + TARGET_LINK_LIBRARIES(SpamApp SpamHBPlan) + endif() + + SET_PROPERTY(TARGET SpamApp PROPERTY RELEASE_POSTFIX ${CMAKE_RELEASE_POSTFIX}) + SET_PROPERTY(TARGET SpamApp PROPERTY DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) + SET_PROPERTY(TARGET SpamApp PROPERTY PROJECT_LABEL "App") + SET_PROPERTY(TARGET SpamApp PROPERTY FOLDER "Spam/App") + + if(BUILD_DYNAMIC_LIBS) + if (WIN32) + INSTALL(TARGETS SpamApp RUNTIME DESTINATION bin LIBRARY DESTINATION bin ARCHIVE DESTINATION app_libs) + elseif (UNIX) + INSTALL(TARGETS SpamApp LIBRARY DESTINATION bin LIBRARY DESTINATION bin ARCHIVE DESTINATION app_libs) + endif() + else(BUILD_DYNAMIC_LIBS) + INSTALL(TARGETS SpamApp ARCHIVE DESTINATION lib COMPONENT core_libs) + endif(BUILD_DYNAMIC_LIBS) + INSTALL(FILES ${APP_POSEPLANNER_HEADERS} DESTINATION include/Spam/App/PosePlanner/ COMPONENT apps_headers) + INSTALL(FILES ${APP_R2GPlanner_HEADERS} DESTINATION include/Spam/App/R2GPlanner/ COMPONENT apps_headers) + INSTALL(FILES ${APP_FILES} DESTINATION bin COMPONENT apps_configs) + + SOURCE_GROUP("Include Files" FILES ${APP_POSEPLANNER_HEADERS} ${APP_R2GPlanner_HEADERS}) + SOURCE_GROUP("Resource Files" FILES ${APP_FILES}) +ENDIF (BUILD_SPAM_APP) ############################################################################### # # PosePlanner executables # ############################################################################### +IF (BUILD_SPAM_POSEPLANNER) + SET(POSE_PLANNER_SOURCES + ${PROJECT_ROOT}/src/Spam/App/PosePlanner/Main.cpp + ) + SET(POSE_PLANNER_HEADERS + ) + SET(POSE_PLANNER_FILES + ) -SET(POSE_PLANNER_SOURCES - ${PROJECT_ROOT}/src/Spam/PosePlanner/PosePlanner.cpp -) -SET(POSE_PLANNER_HEADERS - ${PROJECT_ROOT}/include/Spam/PosePlanner/PosePlanner.h -) -SET(POSE_PLANNER_FILES - ${PROJECT_ROOT}/resources/SpamPosePlanner_RobotBoris.xml -) + ADD_EXECUTABLE(SpamPosePlanner ${POSE_PLANNER_SOURCES} ${POSE_PLANNER_HEADERS} ${POSE_PLANNER_FILES}) + SET_TARGET_PROPERTIES(SpamPosePlanner PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} -D_SPAM_POSE_MAIN_") + TARGET_LINK_LIBRARIES(SpamPosePlanner SpamApp) + COPY_FILES(SpamPosePlanner ${RUNTIME_OUTPUT_DIRECTORY} ${POSE_PLANNER_FILES}) + SET_PROPERTY(TARGET SpamPosePlanner PROPERTY RELEASE_POSTFIX ${CMAKE_RELEASE_POSTFIX}) + SET_PROPERTY(TARGET SpamPosePlanner PROPERTY DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) + INSTALL(TARGETS SpamPosePlanner RUNTIME DESTINATION bin COMPONENT apps_execs) + INSTALL(FILES ${POSE_PLANNER_HEADERS} DESTINATION include/Spam/PosePlanner/ COMPONENT apps_headers) + INSTALL(FILES ${POSE_PLANNER_FILES} DESTINATION bin COMPONENT apps_configs) + + SET_PROPERTY(TARGET SpamPosePlanner PROPERTY PROJECT_LABEL "PosePlanner") + SET_PROPERTY(TARGET SpamPosePlanner PROPERTY FOLDER "Spam/App") + SOURCE_GROUP("Include Files" FILES ${POSE_PLANNER_HEADERS}) + SOURCE_GROUP("Resource Files" FILES ${POSE_PLANNER_FILES}) +ENDIF (BUILD_SPAM_POSEPLANNER) + +############################################################################### +# +# R2GPlanner executables +# +############################################################################### +IF (BUILD_SPAM_R2GPLANNER) + SET(R2G_PLANNER_SOURCES + ${PROJECT_ROOT}/src/Spam/App/R2GPlanner/Main.cpp + ) + SET(R2G_PLANNER_HEADERS + ) + SET(R2G_PLANNER_FILES + ${PROJECT_ROOT}/resources/Spam/R2GPlanner/SpamR2GPlanner_RobotBoris.xml + ) -ADD_EXECUTABLE(SpamPosePlanner ${POSE_PLANNER_SOURCES} ${POSE_PLANNER_HEADERS} ${POSE_PLANNER_FILES}) -SET_TARGET_PROPERTIES(SpamPosePlanner PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} -D_SPAM_POSE_MAIN_") -TARGET_LINK_LIBRARIES(SpamPosePlanner SpamSpam) -COPY_FILES(SpamPosePlanner ${RUNTIME_OUTPUT_DIRECTORY} ${POSE_PLANNER_FILES}) -SET_PROPERTY(TARGET SpamPosePlanner PROPERTY RELEASE_POSTFIX ${CMAKE_RELEASE_POSTFIX}) -SET_PROPERTY(TARGET SpamPosePlanner PROPERTY DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) -INSTALL(TARGETS SpamPosePlanner RUNTIME DESTINATION bin) -INSTALL(FILES ${POSE_PLANNER_HEADERS} DESTINATION include/Spam/PosePlanner/) -INSTALL(FILES ${POSE_PLANNER_FILES} DESTINATION bin) - -SET_PROPERTY(TARGET SpamPosePlanner PROPERTY PROJECT_LABEL "PosePlanner") -SET_PROPERTY(TARGET SpamPosePlanner PROPERTY FOLDER "Spam") -SOURCE_GROUP("Include Files" FILES ${POSE_PLANNER_HEADERS}) -SOURCE_GROUP("Resource Files" FILES ${POSE_PLANNER_FILES}) + ADD_EXECUTABLE(SpamR2GPlanner ${R2G_PLANNER_SOURCES} ${R2G_PLANNER_HEADERS} ${R2G_PLANNER_FILES}) + SET_TARGET_PROPERTIES(SpamR2GPlanner PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} -D_SPAM_RAG_MAIN_") + TARGET_LINK_LIBRARIES(SpamR2GPlanner SpamApp) + COPY_FILES(SpamR2GPlanner ${RUNTIME_OUTPUT_DIRECTORY} ${R2G_PLANNER_FILES}) + SET_PROPERTY(TARGET SpamR2GPlanner PROPERTY RELEASE_POSTFIX ${CMAKE_RELEASE_POSTFIX}) + SET_PROPERTY(TARGET SpamR2GPlanner PROPERTY DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) + INSTALL(TARGETS SpamR2GPlanner RUNTIME DESTINATION bin COMPONENT apps_execs) + INSTALL(FILES ${R2G_PLANNER_HEADERS} DESTINATION include/Spam/R2GPlanner/ COMPONENT apps_header) + INSTALL(FILES ${R2G_PLANNER_FILES} DESTINATION bin COMPONENT apps_configs) + + SET_PROPERTY(TARGET SpamR2GPlanner PROPERTY PROJECT_LABEL "R2GPlanner") + SET_PROPERTY(TARGET SpamR2GPlanner PROPERTY FOLDER "Spam/App") + SOURCE_GROUP("Include Files" FILES ${R2G_PLANNER_HEADERS}) + SOURCE_GROUP("Resource Files" FILES ${R2G_PLANNER_FILES}) +ENDIF (BUILD_SPAM_R2GPLANNER) ############################################################################### # -# RagPlanner executables +# Data Belief library # ############################################################################### +IF (BUILD_SPAM_DATA_BELIEF) + SET(DATA_BELIEF_HEADERS + ${PROJECT_ROOT}/include/Spam/Data/Belief/Belief.h + ) + SET(DATA_BELIEF_SOURCES + ${PROJECT_ROOT}/src/Spam/Data/Belief/Belief.cpp + ) + SET(DATA_BELIEF_FILES + ${PROJECT_ROOT}/resources/Spam/Data/SpamDataBelief.xml + ) + + if(BUILD_DYNAMIC_LIBS) + ADD_LIBRARY(SpamDataBelief SHARED ${DATA_BELIEF_HEADERS} ${DATA_BELIEF_SOURCES} ${DATA_BELIEF_FILES}) + #SET_TARGET_PROPERTIES(SpamSpam PROPERTIES COMPILE_FLAGS ${CMAKE_DLL_EXPORT_FLAGS}) + else(BUILD_DYNAMIC_LIBS) + ADD_LIBRARY(SpamDataBelief STATIC ${DATA_BELIEF_HEADERS} ${DATA_BELIEF_SOURCES} ${DATA_BELIEF_FILES}) + endif(BUILD_DYNAMIC_LIBS) + if (WIN32) + TARGET_LINK_LIBRARIES(SpamDataBelief SpamHBPlan) + elseif (UNIX) + TARGET_LINK_LIBRARIES(SpamDataBelief SpamHBPlan) + endif() + + SET_PROPERTY(TARGET SpamDataBelief PROPERTY RELEASE_POSTFIX ${CMAKE_RELEASE_POSTFIX}) + SET_PROPERTY(TARGET SpamDataBelief PROPERTY DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) + SET_PROPERTY(TARGET SpamDataBelief PROPERTY PROJECT_LABEL "Belief") + SET_PROPERTY(TARGET SpamDataBelief PROPERTY FOLDER "Spam/Data") + + COPY_FILES(SpamDataBelief ${RUNTIME_OUTPUT_DIRECTORY} ${DATA_BELIEF_FILES}) + if(BUILD_DYNAMIC_LIBS) + if (WIN32) + INSTALL(TARGETS SpamDataBelief RUNTIME DESTINATION bin LIBRARY DESTINATION bin ARCHIVE DESTINATION app_libs) + elseif (UNIX) + INSTALL(TARGETS SpamDataBelief LIBRARY DESTINATION bin LIBRARY DESTINATION bin ARCHIVE DESTINATION app_libs) + endif() + else(BUILD_DYNAMIC_LIBS) + INSTALL(TARGETS SpamDataBelief ARCHIVE DESTINATION lib COMPONENT data_libs) + endif(BUILD_DYNAMIC_LIBS) + INSTALL(FILES ${DATA_BELIEF_HEADERS} DESTINATION include/Spam/Data/Belief/ COMPONENT data_headers) + INSTALL(FILES ${DATA_BELIEF_FILES} DESTINATION bin COMPONENT data_configs) + + SOURCE_GROUP("Include Files" FILES ${DATA_BELIEF_HEADERS}) + SOURCE_GROUP("Resource Files" FILES ${DATA_BELIEF_FILES}) +ENDIF (BUILD_SPAM_DATA_BELIEF) -SET(RAG_PLANNER_SOURCES - ${PROJECT_ROOT}/src/Spam/PosePlanner/PosePlanner.cpp - ${PROJECT_ROOT}/src/Spam/RagPlanner/RagPlanner.cpp -) -SET(RAG_PLANNER_HEADERS - ${PROJECT_ROOT}/include/Spam/RagPlanner/RagPlanner.h -) -SET(RAG_PLANNER_FILES - ${PROJECT_ROOT}/resources/SpamRagPlanner_RobotBoris.xml -) +############################################################################### +# +# Data Belief library +# +############################################################################### +IF (BUILD_SPAM_DATA_R2GTRAJECTORY) + SET(DATA_R2GTRAJECTORY_HEADERS + ${PROJECT_ROOT}/include/Spam/Data/R2GTrajectory/R2GTrajectory.h + ) + SET(DATA_R2GTRAJECTORY_SOURCES + ${PROJECT_ROOT}/src/Spam/Data/R2GTrajectory/R2GTrajectory.cpp + ) + SET(DATA_R2GTRAJECTORY_FILES + ${PROJECT_ROOT}/resources/Spam/Data/SpamDataR2GTrajectory.xml + ) + + if(BUILD_DYNAMIC_LIBS) + ADD_LIBRARY(SpamDataR2GTrajectory SHARED ${DATA_R2GTRAJECTORY_HEADERS} ${DATA_R2GTRAJECTORY_SOURCES} ${DATA_R2GTRAJECTORY_FILES}) + #SET_TARGET_PROPERTIES(SpamSpam PROPERTIES COMPILE_FLAGS ${CMAKE_DLL_EXPORT_FLAGS}) + else(BUILD_DYNAMIC_LIBS) + ADD_LIBRARY(SpamDataR2GTrajectory STATIC ${DATA_R2GTRAJECTORY_HEADERS} ${DATA_R2GTRAJECTORY_SOURCES} ${DATA_R2GTRAJECTORY_FILES}) + endif(BUILD_DYNAMIC_LIBS) + if (WIN32) + TARGET_LINK_LIBRARIES(SpamDataR2GTrajectory SpamHBPlan) + elseif (UNIX) + TARGET_LINK_LIBRARIES(SpamDataR2GTrajectory SpamHBPlan) + endif() + + SET_PROPERTY(TARGET SpamDataR2GTrajectory PROPERTY RELEASE_POSTFIX ${CMAKE_RELEASE_POSTFIX}) + SET_PROPERTY(TARGET SpamDataR2GTrajectory PROPERTY DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) + SET_PROPERTY(TARGET SpamDataR2GTrajectory PROPERTY PROJECT_LABEL "R2GTrajectory") + SET_PROPERTY(TARGET SpamDataR2GTrajectory PROPERTY FOLDER "Spam/Data") + + COPY_FILES(SpamDataR2GTrajectory ${RUNTIME_OUTPUT_DIRECTORY} ${DATA_R2GTRAJECTORY_FILES}) + if(BUILD_DYNAMIC_LIBS) + if (WIN32) + INSTALL(TARGETS SpamDataR2GTrajectory RUNTIME DESTINATION bin LIBRARY DESTINATION bin ARCHIVE DESTINATION app_libs) + elseif (UNIX) + INSTALL(TARGETS SpamDataR2GTrajectory LIBRARY DESTINATION bin LIBRARY DESTINATION bin ARCHIVE DESTINATION app_libs) + endif() + else(BUILD_DYNAMIC_LIBS) + INSTALL(TARGETS SpamDataR2GTrajectory ARCHIVE DESTINATION lib COMPONENT data_libs) + endif(BUILD_DYNAMIC_LIBS) + INSTALL(FILES ${DATA_R2GTRAJECTORY_HEADERS} DESTINATION include/Spam/Data/R2GTrajectory/ COMPONENT data_headers) + INSTALL(FILES ${DATA_R2GTRAJECTORY_FILES} DESTINATION bin COMPONENT data_configs) + + SOURCE_GROUP("Include Files" FILES ${DATA_R2GTRAJECTORY_HEADERS}) + SOURCE_GROUP("Resource Files" FILES ${DATA_R2GTRAJECTORY_FILES}) +ENDIF (BUILD_SPAM_DATA_R2GTRAJECTORY) -ADD_EXECUTABLE(SpamRagPlanner ${RAG_PLANNER_SOURCES} ${RAG_PLANNER_HEADERS} ${RAG_PLANNER_FILES}) -SET_TARGET_PROPERTIES(SpamRagPlanner PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} -D_SPAM_RAG_MAIN_") -TARGET_LINK_LIBRARIES(SpamRagPlanner SpamSpam) -COPY_FILES(SpamRagPlanner ${RUNTIME_OUTPUT_DIRECTORY} ${RAG_PLANNER_FILES}) -SET_PROPERTY(TARGET SpamRagPlanner PROPERTY RELEASE_POSTFIX ${CMAKE_RELEASE_POSTFIX}) -SET_PROPERTY(TARGET SpamRagPlanner PROPERTY DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) -INSTALL(TARGETS SpamRagPlanner RUNTIME DESTINATION bin) -INSTALL(FILES ${RAG_PLANNER_HEADERS} DESTINATION include/Spam/RagPlanner/) -INSTALL(FILES ${RAG_PLANNER_FILES} DESTINATION bin) - -SET_PROPERTY(TARGET SpamRagPlanner PROPERTY PROJECT_LABEL "RagPlanner") -SET_PROPERTY(TARGET SpamRagPlanner PROPERTY FOLDER "Spam") -SOURCE_GROUP("Include Files" FILES ${RAG_PLANNER_HEADERS}) -SOURCE_GROUP("Resource Files" FILES ${RAG_PLANNER_FILES}) +############################################################################### +# +# Spam demo +# +############################################################################### +IF(BUILD_SPAM_DEMO_RAG) + SET(DEMO_RAG_SOURCES + ${PROJECT_ROOT}/src/Spam/Demo/R2GPlanner/R2GDemo.cpp + ) + SET(DEMO_RAG_HEADERS + ${PROJECT_ROOT}/include/Spam/Demo/R2GPlanner/R2GDemo.h + ) + SET(DEMO_RAG_FILES + ${PROJECT_ROOT}/resources/Spam/Demo/SpamDemoR2G_RobotBoris.xml + ${PROJECT_ROOT}/resources/Spam/Demo/GraspCameraDepthSimDemoRag.xml + ${PROJECT_ROOT}/resources/Spam/Demo/GraspCameraKinectDemoRag.xml + ${PROJECT_ROOT}/resources/Spam/Demo/GraspCameraOpenCVDemoRag.xml + ${PROJECT_ROOT}/resources/Spam/Demo/GraspCameraOpenGLDemoRag.xml + ${PROJECT_ROOT}/resources/Spam/Demo/GraspCameraOpenNIDemoRag.xml + ${PROJECT_ROOT}/resources/Spam/Demo/GraspCameraPointGreyDemoRag.xml + ${PROJECT_ROOT}/resources/Spam/Demo/GraspCameraRobotDepthDemoRag.xml + ${PROJECT_ROOT}/resources/Spam/Demo/GraspDataContactModelDemoRag.xml + ${PROJECT_ROOT}/resources/Spam/Demo/GraspDataContactQueryDemoRag.xml + ${PROJECT_ROOT}/resources/Spam/Demo/GraspDataImageDemoRag.xml + ${PROJECT_ROOT}/resources/Spam/Demo/GraspDataPointsCurvDemoRag.xml + ${PROJECT_ROOT}/resources/Spam/Demo/GraspDataTrajectoryDemoRag.xml + ${PROJECT_ROOT}/resources/Spam/Demo/GraspDataVideoDemoRag.xml + ) + + ADD_EXECUTABLE(SpamDemoRag ${DEMO_RAG_SOURCES} ${DEMO_RAG_HEADERS} ${DEMO_RAG_FILES}) + if (WIN32) + SET_TARGET_PROPERTIES(SpamDemoRag PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS}") + elseif (UNIX) + SET_TARGET_PROPERTIES(SpamDemoRag PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} -Wno-invalid-offsetof") + endif() + TARGET_LINK_LIBRARIES(SpamDemoRag SpamApp) + COPY_FILES(SpamDemoRag ${RUNTIME_OUTPUT_DIRECTORY} ${DEMO_RAG_FILES}) + SET_PROPERTY(TARGET SpamDemoRag PROPERTY RELEASE_POSTFIX ${CMAKE_RELEASE_POSTFIX}) + SET_PROPERTY(TARGET SpamDemoRag PROPERTY DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) + + INSTALL(TARGETS SpamDemoRag RUNTIME DESTINATION bin COMPONENT demo_execs) + INSTALL(FILES ${DEMO_RAG_HEADERS} DESTINATION include/Spam/Demo/Rag/ COMPONENT demo_headers) + INSTALL(FILES ${DEMO_RAG_SOURCES} DESTINATION src/Spam/Demo/Rag/ COMPONENT demo_sources) + INSTALL(FILES ${DEMO_RAG_FILES} DESTINATION bin COMPONENT demo_configs) + + SET_PROPERTY(TARGET SpamDemoRag PROPERTY PROJECT_LABEL "Spam") + SET_PROPERTY(TARGET SpamDemoRag PROPERTY FOLDER "Spam/Demo") + SOURCE_GROUP("Include Files" FILES ${DEMO_RAG_HEADERS}) + SOURCE_GROUP("Resource Files" FILES ${DEMO_RAG_FILES}) +ENDIF(BUILD_SPAM_DEMO_RAG) diff --git a/README.md b/README.md index 7961b41..84f30d8 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,2 @@ -Spam -==== - -Spam +# Spam-2.0 +Simultaneous Perception and Manipulation version 2.0 diff --git a/include/Spam/App/PosePlanner/Data.h b/include/Spam/App/PosePlanner/Data.h new file mode 100644 index 0000000..8715d29 --- /dev/null +++ b/include/Spam/App/PosePlanner/Data.h @@ -0,0 +1,91 @@ +//------------------------------------------------------------------------------ +// Simultaneous Perception And Manipulation (SPAM) +//------------------------------------------------------------------------------ +// +// Copyright (c) 2010 University of Birmingham. +// All rights reserved. +// +// Intelligence Robot Lab. +// School of Computer Science +// University of Birmingham +// Edgbaston, Brimingham. UK. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// with the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimers. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimers in +// the documentation and/or other materials provided with the +// distribution. +// * Neither the names of the Simultaneous Perception And Manipulation +// (SPAM), University of Birmingham, nor the names of its contributors +// may be used to endorse or promote products derived from this Software +// without specific prior written permission. +// +// The software is provided "as is", without warranty of any kind, express or +// implied, including but not limited to the warranties of merchantability, +// fitness for a particular purpose and noninfringement. In no event shall the +// contributors or copyright holders be liable for any claim, damages or other +// liability, whether in an action of contract, tort or otherwise, arising +// from, out of or in connection with the software or the use of other dealings +// with the software. +// +//------------------------------------------------------------------------------ +//! @Author: Claudio Zito +//! @Date: 25/03/2014 +//------------------------------------------------------------------------------ +#pragma once +#ifndef _SPAM_POSEPLANNER_DATA_H_ +#define _SPAM_POSEPLANNER_DATA_H_ + +//------------------------------------------------------------------------------ + +#include +#include +#include + +//------------------------------------------------------------------------------ + +namespace grasp { +namespace data { + +//------------------------------------------------------------------------------ + +/** Initialises handler. +* (Optionally) Implemented by Handler. +*/ +class HandlerBelief { +public: + /** Sets planner and controllers. */ + virtual void set() = 0; +}; + +/** Trajectory collection and tools. +* (Optionally) Implemented by Item. +*/ +class Belief { +public: + /** Returns command trajectory with velocity profile. */ + virtual void createBelief() = 0; +}; + +//------------------------------------------------------------------------------ + +}; // namespace +}; // namespace + +//namespace golem { +// +//template <> void Stream::read(spam::Data &data) const; +//template <> void Stream::write(const spam::Data &data); +// +//}; // namespace + + +#endif /*_GRASP_POSEPLANNER_POSEPLANNER_H_*/ diff --git a/include/Spam/PosePlanner/PosePlanner.h b/include/Spam/App/PosePlanner/PosePlanner.h similarity index 99% rename from include/Spam/PosePlanner/PosePlanner.h rename to include/Spam/App/PosePlanner/PosePlanner.h index c23648b..2a27e96 100644 --- a/include/Spam/PosePlanner/PosePlanner.h +++ b/include/Spam/App/PosePlanner/PosePlanner.h @@ -47,15 +47,10 @@ //------------------------------------------------------------------------------ #include - #include #include #include -#include - -#include -#include -//#include +#include //------------------------------------------------------------------------------ diff --git a/include/Spam/App/R2GPlanner/Data.h b/include/Spam/App/R2GPlanner/Data.h new file mode 100644 index 0000000..6645133 --- /dev/null +++ b/include/Spam/App/R2GPlanner/Data.h @@ -0,0 +1,65 @@ +//------------------------------------------------------------------------------ +// Simultaneous Perception And Manipulation (SPAM) +//------------------------------------------------------------------------------ +// +// Copyright (c) 2010 University of Birmingham. +// All rights reserved. +// +// Intelligence Robot Lab. +// School of Computer Science +// University of Birmingham +// Edgbaston, Brimingham. UK. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// with the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimers. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimers in +// the documentation and/or other materials provided with the +// distribution. +// * Neither the names of the Simultaneous Perception And Manipulation +// (SPAM), University of Birmingham, nor the names of its contributors +// may be used to endorse or promote products derived from this Software +// without specific prior written permission. +// +// The software is provided "as is", without warranty of any kind, express or +// implied, including but not limited to the warranties of merchantability, +// fitness for a particular purpose and noninfringement. In no event shall the +// contributors or copyright holders be liable for any claim, damages or other +// liability, whether in an action of contract, tort or otherwise, arising +// from, out of or in connection with the software or the use of other dealings +// with the software. +// +//------------------------------------------------------------------------------ +//! @Author: Claudio Zito +//! @Date: 27/10/2012 +//------------------------------------------------------------------------------ +#pragma once +#ifndef _SPAM_RAGPLANNER_DATA_H_ +#define _SPAM_RAGPLANNER_DATA_H_ + +//------------------------------------------------------------------------------ + +#include +#include +#include + +//------------------------------------------------------------------------------ + +namespace grasp { +namespace data { +//------------------------------------------------------------------------------ + + +//------------------------------------------------------------------------------ + +}; // namespace +}; // namespace + +#endif /*_SPAM_RAGPLANNER_DATA_H_*/ diff --git a/include/Spam/RagPlanner/RagPlanner.h b/include/Spam/App/R2GPlanner/R2GPlanner.h similarity index 97% rename from include/Spam/RagPlanner/RagPlanner.h rename to include/Spam/App/R2GPlanner/R2GPlanner.h index 2d51496..b76bcdd 100644 --- a/include/Spam/RagPlanner/RagPlanner.h +++ b/include/Spam/App/R2GPlanner/R2GPlanner.h @@ -46,7 +46,8 @@ //------------------------------------------------------------------------------ -#include +#include +#include #include //------------------------------------------------------------------------------ @@ -65,7 +66,7 @@ namespace spam { Based on Platt R. et al. "A hypothesis-based algorithm for planning and control in non-Gaussian belief spaces", 2011. */ -class RagPlanner : public PosePlanner, protected golem::Profile::CallbackDist { +class R2GPlanner : public PosePlanner, protected golem::Profile::CallbackDist { public: /** Force reader. Overwrite the ActiveCtrl reader. this retrieves the triggered joints */ typedef std::function&)> GuardsReader; @@ -73,7 +74,7 @@ class RagPlanner : public PosePlanner, protected golem::Profile::CallbackDist { /** Data */ class Data : public PosePlanner::Data { public: - friend class RagPlanner; + friend class R2GPlanner; /** Data bundle description */ class Desc : public PosePlanner::Data::Desc { @@ -95,7 +96,7 @@ class RagPlanner : public PosePlanner, protected golem::Profile::CallbackDist { protected: /** Demo */ - RagPlanner* owner; + R2GPlanner* owner; /** Load from xml context */ virtual void load(const std::string& prefix, const golem::XMLContext* xmlcontext, const grasp::data::Handler::Map& handlerMap); @@ -110,7 +111,7 @@ class RagPlanner : public PosePlanner, protected golem::Profile::CallbackDist { class Desc : public PosePlanner::Desc { protected: - GRASP_CREATE_FROM_OBJECT_DESC1(RagPlanner, golem::Object::Ptr, golem::Scene&) + GRASP_CREATE_FROM_OBJECT_DESC1(R2GPlanner, golem::Object::Ptr, golem::Scene&) public: /** Smart pointer */ @@ -212,9 +213,9 @@ class RagPlanner : public PosePlanner, protected golem::Profile::CallbackDist { // gaussian filter mask grasp::RealSeq mask; // computes gaussian - template inline _Real N(const _Real mean, const _Real stdev) { + template inline _Real N(const _Real x, const _Real stdev) { const _Real norm = golem::numeric_const<_Real>::ONE / (stdev*Math::sqrt(2 * golem::numeric_const<_Real>::PI)); - return norm*golem::Math::exp(-.5*Math::sqr(_Real(mean) / _Real(stdev))); // gaussian + return norm*golem::Math::exp(-.5*Math::sqr(_Real(x) / _Real(stdev))); // gaussian } // computes guassian on a vector template inline std::vector<_Real> N(_Ptr begin, _Ptr end, const size_t dim, const _Real stdev) { @@ -393,8 +394,8 @@ class RagPlanner : public PosePlanner, protected golem::Profile::CallbackDist { virtual void render() const; void renderHand(const golem::Controller::State &state, const golem::Bounds::Seq &bounds, bool clear = true); - RagPlanner(golem::Scene &scene); - ~RagPlanner(); + R2GPlanner(golem::Scene &scene); + ~R2GPlanner(); bool create(const Desc& desc); }; diff --git a/include/Spam/Spam/Spam.h b/include/Spam/Core/Collision.h similarity index 72% rename from include/Spam/Spam/Spam.h rename to include/Spam/Core/Collision.h index e1bae5c..1fe67eb 100644 --- a/include/Spam/Spam/Spam.h +++ b/include/Spam/Core/Collision.h @@ -41,18 +41,15 @@ //! @Date: 27/10/2012 //------------------------------------------------------------------------------ #pragma once -#ifndef _SPAM_SPAM_H_ -#define _SPAM_SPAM_H_ +#ifndef _SPAM_COLLISION_H_ +#define _SPAM_COLLISION_H_ //------------------------------------------------------------------------------ -#include #include -#include +#include #include -#include -#include -#include +#include //------------------------------------------------------------------------------ @@ -66,14 +63,6 @@ namespace pcl { struct PolygonMesh; }; -namespace grasp { - class Manipulator; -}; - -namespace spam { - class FTDrivenHeuristic; -}; - //------------------------------------------------------------------------------ namespace spam { @@ -86,88 +75,10 @@ namespace spam { //------------------------------------------------------------------------------ /** Forward declaration */ -class Robot; -class Belief; class Hypothesis; //------------------------------------------------------------------------------ -/** Type of guards for Justin */ -enum FTGuardTypes { - /** Absolute values */ - FTGUARD_ABS = 0, - /** Less than */ - FTGUARD_LESSTHAN, - /**Greater than */ - FTGUARD_GREATERTHAN, -}; -/** HandChains */ -enum HandChains { - UNKNOWN = 0, - THUMB, - INDEX, - MIDDLE, - RING, - PINKY, -}; -/** Force/torque guards for Justin and BHAM robots */ -class FTGuard { -public: - typedef std::vector Seq; - - /** Name of the guard {right,left}_{tcp,thumb,tip,middle,ring} */ - std::string name; - /** Type of guard {|>,<,>} */ - FTGuardTypes type; - /** Force/torque threshold for the guard */ - golem::Real threshold; - /** Force/torque measured */ - golem::Real force; - - /** Last arm's joint index */ - golem::Configspace::Index armIdx; - /** Joint index */ - golem::Configspace::Index jointIdx; - /** Number of chains in the hand */ - golem::U32 handChains; - /** Number of joints per fingers */ - golem::U32 fingerJoints; - - /** C'ctor */ - FTGuard(const grasp::Manipulator &manipulator); - - /** D'ctor */ - virtual ~FTGuard() {}; - - /** Prints the guard in the format for Justin. - Example: {right,left}_{tcp,thumb,tip,middle,ring} {0,1,2,[3,4]} {|>,<,>} value **/ - std::string str() const; - - /** Returns the chain index to the finger [1, 5] */ - inline golem::U32 getHandChain() { - return ((golem::U32)(jointIdx - armIdx) / fingerJoints) + 1; - } - /** Returns the chain index to the finger [1, 5] */ - inline const golem::U32 getHandChain() const { - return ((golem::U32)(jointIdx - armIdx) / fingerJoints) + 1; - } - /** Returns the index of the joint per finger [0,3] */ - inline golem::U32 getHandJoint() { - return (golem::U32)(jointIdx - armIdx) % fingerJoints; - } - /** Returns the index of the joint per finger [0,3] */ - inline const golem::U32 getHandJoint() const { - return (golem::U32)(jointIdx - armIdx) % fingerJoints; - } - - /** Sets the chain and joint iterators */ - void create(golem::Configspace::Index& joint); -}; -/** Reads/writes guards from/to a given XML context */ -void XMLData(FTGuard &val, golem::XMLContext* xmlcontext, bool create = false); - -//------------------------------------------------------------------------------ - /** Grasp collision model */ class Collision { public: @@ -751,229 +662,6 @@ void XMLData(Collision::Desc& val, golem::XMLContext* xmlcontext, bool create = //------------------------------------------------------------------------------ -/** Hypothesis over object poses */ -class Hypothesis { -public: - friend class FTDrivenHeuristic; - friend class Belief; - typedef golem::shared_ptr Ptr; - typedef std::map Map; - typedef std::vector Seq; - - /** Bounds Appearance */ - class BoundsAppearance { - public: - /** Show bounds solid */ - bool showSolid; - /** Show bounds wire frames */ - bool showWire; - /** Bounds solid colour */ - golem::RGBA solidColour; - /** Bounds wire colour */ - golem::RGBA wireColour; - /** Bounds wireframe thickness */ - golem::Real wireWidth; - - /** Constructs from description object */ - BoundsAppearance() { - setToDefault(); - } - /** Sets the parameters to the default values */ - void setToDefault() { - showSolid = false; - showWire = true; - solidColour = golem::RGBA(golem::U8(255), golem::U8(255), golem::U8(0), golem::U8(127)); - wireColour = golem::RGBA(golem::U8(255), golem::U8(255), golem::U8(0), golem::U8(127)); - wireWidth = golem::Real(1.0); - } - /** Checks if the description is valid. */ - bool isValid() const { - if (wireWidth <= golem::REAL_ZERO) - return false; - return true; - } - - /** Draw bounds */ - void draw(const golem::Bounds::Seq& bounds, golem::DebugRenderer& renderer) const; - }; - - /** Appearance */ - class Appearance { - public: - /** Show frame */ - bool showFrames; - /** Show point cloud */ - bool showPoints; - /** Frame size of the sample */ - golem::Vec3 frameSize; - /** clolour of the point cloud */ - golem::RGBA colour; - - /** Bounds colour */ - BoundsAppearance bounds; - - /** Constructs from description object */ - Appearance() { - setToDefault(); - } - /** Sets the parameters to the default values */ - void setToDefault() { - showFrames = true; - showPoints = true; - frameSize.set(golem::Real(0.02)); - colour = golem::RGBA::MAGENTA; - bounds.setToDefault(); - } - /** Checks if the description is valid. */ - bool isValid() const { - if (!bounds.isValid()) - return false; - if (!frameSize.isPositive()) - return false; - return true; - } - }; - - class Desc { - public: - typedef golem::shared_ptr Ptr; - - Appearance appearance; - Collision::Desc::Ptr collisionDescPtr; - - /** Constructs description object */ - Desc() { - Desc::setToDefault(); - } - /** Nothing to do here */ - virtual ~Desc() {} - /** Creates the object from the description. */ - virtual Hypothesis::Ptr create(const grasp::Manipulator& manipulator) const { - return Hypothesis::Ptr(new Hypothesis(manipulator, *this)); - } - /** Sets description to default values */ - void setToDefault() { - appearance.setToDefault(); - collisionDescPtr.reset(new Collision::Desc()); - } - /** Checks if the description is valid. */ - virtual bool isValid() const { - if (!appearance.isValid()) - return false; - if (collisionDescPtr != nullptr && !collisionDescPtr->isValid()) - return false; - return true; - } - - }; - - /** Create */ - virtual void create(const golem::U32 idx, const golem::Mat34 &trn, const grasp::RBPose::Sample &s, golem::Rand& rand, const grasp::Cloud::PointSeq& points); - - /** Complete constructor */ - //Hypothesis(const golem::U32 idx, const golem::Mat34 &trn, const grasp::RBPose::Sample &s, grasp::Cloud::PointSeq &p) { - // index = idx; - // modelFrame = trn; - // sample = s; - // for (grasp::Cloud::PointSeq::const_iterator i = p.begin(); i != p.end(); ++i) - // points.push_back(*i); - // appearance.setToDefault(); - // boundsDesc.setToDefault(); - // build(); - // //buildMesh(); - //} - ///** Destrutor */ - //~Hypothesis() { - // pTree.release(); - // pTriangles.release(); - //} - - - /** Returns this sample in model frame **/ - inline grasp::RBPose::Sample toRBPoseSample() const { return sample; }; - /** Returns this sample in global frame (default: robot frame) **/ - inline grasp::RBPose::Sample toRBPoseSampleGF() const { return grasp::RBPose::Sample(sample.toMat34() * modelFrame, sample.weight, sample.cdf); }; - /** Returns the point cloud in global frame */ - inline grasp::Cloud::PointSeq getCloud() const { return points; }; - - /** Collision detection at a given waypoint */ - inline bool check(const Collision::Waypoint& waypoint, const grasp::Manipulator::Config& config, bool debug = false) const { - return collisionPtr->check(waypoint, config, debug); - }; - /** Collision detection at a given waypoint */ - inline bool check(const Collision::FlannDesc& desc, const golem::Rand& rand, const grasp::Manipulator::Config& config, bool debug = false) const { - return collisionPtr->check(desc, rand, config, debug); - } - - /** Collision detection at a given waypoint */ - inline virtual golem::Real estimate(const Collision::FlannDesc& desc, const grasp::Manipulator::Config& config, golem::Real maxDist = golem::REAL_MAX, bool debug = false) const { - return collisionPtr->estimate(desc, config, maxDist, debug); - } - - /** Collision likelihood estimation at a given waypoint */ - inline golem::Real evaluate(const Collision::Waypoint& waypoint, const grasp::Manipulator::Config& config, bool debug = false) const { - return collisionPtr->evaluate(waypoint, config, debug); - } - /** Collision likelihood estimation at a given waypoint */ - inline golem::Real evaluate(const Collision::FlannDesc& desc, const grasp::Manipulator::Config& config, bool debug = false) const { - return collisionPtr->evaluate(desc, config, debug); - } - - /** Return seq of bounds */ - golem::Bounds::Seq bounds(); - - /** Prints global pose of the hypothesis */ - std::string str() const; - - /** Draw hypotheses */ - void draw(golem::DebugRenderer &renderer) const; - - /** Draw collisions */ - void draw(const Collision::Waypoint &waypoint, const grasp::Manipulator::Config& config, golem::DebugRenderer& renderer) const; - /** Draw collision using kdtree */ - void draw(golem::DebugRenderer& renderer, const golem::Rand& rand, const grasp::Manipulator::Config& config) const; - /** Draw estimate */ - void draw(golem::DebugRenderer& renderer, const grasp::Manipulator::Config& config, const Collision::FlannDesc& desc) const { - collisionPtr->draw(renderer, config, desc); - } - /** Draw simulate */ - void draw(golem::DebugRenderer& renderer, const grasp::Manipulator::Config& config, std::vector &joints, grasp::RealSeq &forces, const Collision::FlannDesc& desc) const { - collisionPtr->draw(renderer, config, joints, forces, desc); - } - Appearance appearance; - -protected: - /** Identifier */ - golem::U32 index; - /** Model frame **/ - golem::Mat34 modelFrame; - /** Hypothesis. NOTE: contains the query (or sample) frame w.r.t model frame **/ - grasp::RBPose::Sample sample; - /** Point cloud */ - grasp::Cloud::PointSeq points; - - /** Bounding box desc for the object */ - golem::BoundingBox::Desc boundsDesc; - - - /** Manipulator */ - const grasp::Manipulator& manipulator; - /** Description */ - const Desc desc; - - /** Collision detection pointer */ - Collision::Ptr collisionPtr; - - /** Create */ - Hypothesis(const grasp::Manipulator& manipulator, const Desc& desc); -}; - -//------------------------------------------------------------------------------ - -void XMLData(Hypothesis::Desc& val, golem::XMLContext* xmlcontext, bool create = false); - -//------------------------------------------------------------------------------ - }; /** namespace */ -#endif /** _SPAM_SPAM_H_ */ \ No newline at end of file +#endif /** _SPAM_COLLISION_H_ */ \ No newline at end of file diff --git a/include/Spam/Core/JContact.h b/include/Spam/Core/JContact.h new file mode 100644 index 0000000..02b36e9 --- /dev/null +++ b/include/Spam/Core/JContact.h @@ -0,0 +1,137 @@ +//------------------------------------------------------------------------------ +// Simultaneous Perception And Manipulation (SPAM) +//------------------------------------------------------------------------------ +// +// Copyright (c) 2010 University of Birmingham. +// All rights reserved. +// +// Intelligence Robot Lab. +// School of Computer Science +// University of Birmingham +// Edgbaston, Brimingham. UK. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// with the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimers. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimers in +// the documentation and/or other materials provided with the +// distribution. +// * Neither the names of the Simultaneous Perception And Manipulation +// (SPAM), University of Birmingham, nor the names of its contributors +// may be used to endorse or promote products derived from this Software +// without specific prior written permission. +// +// The software is provided "as is", without warranty of any kind, express or +// implied, including but not limited to the warranties of merchantability, +// fitness for a particular purpose and noninfringement. In no event shall the +// contributors or copyright holders be liable for any claim, damages or other +// liability, whether in an action of contract, tort or otherwise, arising +// from, out of or in connection with the software or the use of other dealings +// with the software. +// +//------------------------------------------------------------------------------ +//! @Author: Claudio Zito +//! @Date: 27/10/2012 +//------------------------------------------------------------------------------ +#pragma once +#ifndef _SPAM_JCONTACT_H_ +#define _SPAM_JCONTACT_H_ + +//------------------------------------------------------------------------------ + +#include +#include + +//------------------------------------------------------------------------------ + +namespace spam { + +//------------------------------------------------------------------------------ + +/** Type of guards for Justin */ +enum FTGuardTypes { + /** Absolute values */ + FTGUARD_ABS = 0, + /** Less than */ + FTGUARD_LESSTHAN, + /**Greater than */ + FTGUARD_GREATERTHAN, +}; +/** HandChains */ +enum HandChains { + UNKNOWN = 0, + THUMB, + INDEX, + MIDDLE, + RING, + PINKY, +}; + +/** Force/torque guards for Justin and BHAM robots */ +class FTGuard { +public: + typedef std::vector Seq; + + /** Name of the guard {right,left}_{tcp,thumb,tip,middle,ring} */ + std::string name; + /** Type of guard {|>,<,>} */ + FTGuardTypes type; + /** Force/torque threshold for the guard */ + golem::Real threshold; + /** Force/torque measured */ + golem::Real force; + + /** Last arm's joint index */ + golem::Configspace::Index armIdx; + /** Joint index */ + golem::Configspace::Index jointIdx; + /** Number of chains in the hand */ + golem::U32 handChains; + /** Number of joints per fingers */ + golem::U32 fingerJoints; + + /** C'ctor */ + FTGuard(const grasp::Manipulator &manipulator); + + /** D'ctor */ + virtual ~FTGuard() {}; + + /** Prints the guard in the format for Justin. + Example: {right,left}_{tcp,thumb,tip,middle,ring} {0,1,2,[3,4]} {|>,<,>} value **/ + std::string str() const; + + /** Returns the chain index to the finger [1, 5] */ + inline golem::U32 getHandChain() { + return ((golem::U32)(jointIdx - armIdx) / fingerJoints) + 1; + } + /** Returns the chain index to the finger [1, 5] */ + inline const golem::U32 getHandChain() const { + return ((golem::U32)(jointIdx - armIdx) / fingerJoints) + 1; + } + /** Returns the index of the joint per finger [0,3] */ + inline golem::U32 getHandJoint() { + return (golem::U32)(jointIdx - armIdx) % fingerJoints; + } + /** Returns the index of the joint per finger [0,3] */ + inline const golem::U32 getHandJoint() const { + return (golem::U32)(jointIdx - armIdx) % fingerJoints; + } + + /** Sets the chain and joint iterators */ + void create(golem::Configspace::Index& joint); +}; +/** Reads/writes guards from/to a given XML context */ +void XMLData(FTGuard &val, golem::XMLContext* xmlcontext, bool create = false); + +//------------------------------------------------------------------------------ + +}; /** namespace */ + +#endif /** _SPAM_JCONTACT_H_ */ \ No newline at end of file diff --git a/include/Spam/Data/Belief/Belief.h b/include/Spam/Data/Belief/Belief.h new file mode 100644 index 0000000..229b119 --- /dev/null +++ b/include/Spam/Data/Belief/Belief.h @@ -0,0 +1,61 @@ +//------------------------------------------------------------------------------ +// Simultaneous Perception And Manipulation (SPAM) +//------------------------------------------------------------------------------ +// +// Copyright (c) 2010 University of Birmingham. +// All rights reserved. +// +// Intelligence Robot Lab. +// School of Computer Science +// University of Birmingham +// Edgbaston, Brimingham. UK. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// with the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimers. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimers in +// the documentation and/or other materials provided with the +// distribution. +// * Neither the names of the Simultaneous Perception And Manipulation +// (SPAM), University of Birmingham, nor the names of its contributors +// may be used to endorse or promote products derived from this Software +// without specific prior written permission. +// +// The software is provided "as is", without warranty of any kind, express or +// implied, including but not limited to the warranties of merchantability, +// fitness for a particular purpose and noninfringement. In no event shall the +// contributors or copyright holders be liable for any claim, damages or other +// liability, whether in an action of contract, tort or otherwise, arising +// from, out of or in connection with the software or the use of other dealings +// with the software. +// +//------------------------------------------------------------------------------ +//! @Author: Claudio Zito +//! @Date: 25/03/2014 +//------------------------------------------------------------------------------ +#pragma once +#ifndef _SPAM_DATA_BELIEF_H_ +#define _SPAM_DATA_BELIEF_H_ + +//------------------------------------------------------------------------------ + +//#include + +//------------------------------------------------------------------------------ + +namespace spam { + +//------------------------------------------------------------------------------ + +//------------------------------------------------------------------------------ + +}; // namespace + +#endif /*_SPAM_DATA_BELIEF_H_*/ diff --git a/include/Spam/Data/R2GTrajectory/R2GTrajectory.h b/include/Spam/Data/R2GTrajectory/R2GTrajectory.h new file mode 100644 index 0000000..4c81ba7 --- /dev/null +++ b/include/Spam/Data/R2GTrajectory/R2GTrajectory.h @@ -0,0 +1,62 @@ +//------------------------------------------------------------------------------ +// Simultaneous Perception And Manipulation (SPAM) +//------------------------------------------------------------------------------ +// +// Copyright (c) 2010 University of Birmingham. +// All rights reserved. +// +// Intelligence Robot Lab. +// School of Computer Science +// University of Birmingham +// Edgbaston, Brimingham. UK. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// with the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimers. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimers in +// the documentation and/or other materials provided with the +// distribution. +// * Neither the names of the Simultaneous Perception And Manipulation +// (SPAM), University of Birmingham, nor the names of its contributors +// may be used to endorse or promote products derived from this Software +// without specific prior written permission. +// +// The software is provided "as is", without warranty of any kind, express or +// implied, including but not limited to the warranties of merchantability, +// fitness for a particular purpose and noninfringement. In no event shall the +// contributors or copyright holders be liable for any claim, damages or other +// liability, whether in an action of contract, tort or otherwise, arising +// from, out of or in connection with the software or the use of other dealings +// with the software. +// +//------------------------------------------------------------------------------ +//! @Author: Claudio Zito +//! @Date: 25/03/2014 +//------------------------------------------------------------------------------ +#pragma once +#ifndef _SPAM_DATA_R2GPLANNER_H_ +#define _SPAM_DATA_R2GPLANNER_H_ + +//------------------------------------------------------------------------------ + +//#include + +//------------------------------------------------------------------------------ + +namespace spam { + +//------------------------------------------------------------------------------ + + +//------------------------------------------------------------------------------ + +}; // namespace + +#endif /*_GRASP_POSEPLANNER_POSEPLANNER_H_*/ diff --git a/include/Spam/Demo/R2GPlanner/R2GDemo.h b/include/Spam/Demo/R2GPlanner/R2GDemo.h new file mode 100644 index 0000000..f54b67b --- /dev/null +++ b/include/Spam/Demo/R2GPlanner/R2GDemo.h @@ -0,0 +1,99 @@ +//------------------------------------------------------------------------------ +// Simultaneous Perception And Manipulation (SPAM) +//------------------------------------------------------------------------------ +// +// Copyright (c) 2010 University of Birmingham. +// All rights reserved. +// +// Intelligence Robot Lab. +// School of Computer Science +// University of Birmingham +// Edgbaston, Brimingham. UK. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// with the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimers. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimers in +// the documentation and/or other materials provided with the +// distribution. +// * Neither the names of the Simultaneous Perception And Manipulation +// (SPAM), University of Birmingham, nor the names of its contributors +// may be used to endorse or promote products derived from this Software +// without specific prior written permission. +// +// The software is provided "as is", without warranty of any kind, express or +// implied, including but not limited to the warranties of merchantability, +// fitness for a particular purpose and noninfringement. In no event shall the +// contributors or copyright holders be liable for any claim, damages or other +// liability, whether in an action of contract, tort or otherwise, arising +// from, out of or in connection with the software or the use of other dealings +// with the software. +// +//------------------------------------------------------------------------------ +//! @Author: Claudio Zito +//! @Date: 25/03/2014 +//------------------------------------------------------------------------------ +#pragma once +#ifndef _SPAM_DEMO_R2G_H_ +#define _SPAM_DEMO_R2G_H_ + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ + +namespace spam { + +//------------------------------------------------------------------------------ + +/** Grasp demo. */ +class R2GDemo : public R2GPlanner { +public: + /** R2G demo description */ + class Desc : public R2GPlanner::Desc { + public: + typedef golem::shared_ptr Ptr; + + /** Constructs from description object */ + Desc() { + Desc::setToDefault(); + } + + /** Sets the parameters to the default values */ + virtual void setToDefault() { + } + + /** Assert that the description is valid. */ + virtual void assertValid(const grasp::Assert::Context& ac) const { + } + /** Load descritpion from xml context. */ + virtual void load(golem::Context& context, const golem::XMLContext* xmlcontext); + + protected: + GRASP_CREATE_FROM_OBJECT_DESC1(R2GDemo, golem::Object::Ptr, golem::Scene&) + }; + +protected: + /** golem::UIRenderer interface */ + virtual void render() const; + + void create(const Desc& desc); + R2GDemo(golem::Scene& scene); + ~R2GDemo(); +}; + +//------------------------------------------------------------------------------ + +}; + +//------------------------------------------------------------------------------ + +#endif /*_SPAM_DEMO_R2G_H_*/ diff --git a/include/Spam/Spam/Belief.h b/include/Spam/HBPlan/Belief.h similarity index 96% rename from include/Spam/Spam/Belief.h rename to include/Spam/HBPlan/Belief.h index a82b73a..d84da58 100644 --- a/include/Spam/Spam/Belief.h +++ b/include/Spam/HBPlan/Belief.h @@ -46,30 +46,7 @@ //------------------------------------------------------------------------------ -//#include -//#include -//#include -#include - -////------------------------------------------------------------------------------ -// -//namespace flann { -// template struct L2_Simple; -//}; -// -//namespace pcl { -// struct PointXYZ; -// template class KdTreeFLANN; -// struct PolygonMesh; -//}; -// -//namespace grasp { -// class Manipulator; -//}; -// -//namespace spam { -// class FTDrivenHeuristic; -//}; +#include //------------------------------------------------------------------------------ diff --git a/include/Spam/Spam/GraphPlanner.h b/include/Spam/HBPlan/GraphPlanner.h similarity index 98% rename from include/Spam/Spam/GraphPlanner.h rename to include/Spam/HBPlan/GraphPlanner.h index 60622bf..c28c4a3 100644 --- a/include/Spam/Spam/GraphPlanner.h +++ b/include/Spam/HBPlan/GraphPlanner.h @@ -46,7 +46,7 @@ //------------------------------------------------------------------------------ -#include +#include //------------------------------------------------------------------------------ @@ -59,12 +59,6 @@ namespace spam { //------------------------------------------------------------------------------ -// Forward declaration -class FTDrivenHeuristic; -//class RagGraphPlanner; - -//------------------------------------------------------------------------------ - /** Abstract class for Arm movement planinng using Probabilistic Road Map approach. */ //class RagPathFinder : public golem::PathFinder { //public: @@ -138,7 +132,6 @@ class RagGraphPlanner : public golem::GraphPlanner { // RagPathFinder::Desc::Ptr pGlobalPathFinderDesc; // /** Local path finder description */ // RagPathFinder::Desc::Ptr pLocalPathFinderDesc; - // /** Constructs the description object. */ // RagPathFinderDesc() { // setToDefault(); @@ -159,11 +152,9 @@ class RagGraphPlanner : public golem::GraphPlanner { // return false; // if (pLocalPathFinderDesc != NULL && !pLocalPathFinderDesc->isValid()) // return false; - // return true; // } //}; - /////** Path optimiser description */ ////class PathOptimisationDesc { ////public: @@ -203,17 +194,14 @@ class RagGraphPlanner : public golem::GraphPlanner { //// return false; //// if (distPathThr <= REAL_ZERO || distPathThr >= REAL_ONE) //// return false; - //// return true; //// } ////}; - /////** Local finder description */ ////class LocalFinderDesc { ////public: //// /** Search range */ //// std::vector range; - //// /** Constructs the description object. */ //// LocalFinderDesc() { //// setToDefault(); @@ -279,6 +267,7 @@ class RagGraphPlanner : public golem::GraphPlanner { ///** Overwrites the path finder desc */ //RagPathFinderDesc ragPathFinderDesc; //RagPathFinder::Ptr pGlobalPathFinder, pLocalPathFinder; + /** Controller state info */ golem::Controller::State::Info armInfo; /** Controller state info */ diff --git a/include/Spam/Spam/Heuristic.h b/include/Spam/HBPlan/Heuristic.h similarity index 78% rename from include/Spam/Spam/Heuristic.h rename to include/Spam/HBPlan/Heuristic.h index a9e3bd4..c48359e 100644 --- a/include/Spam/Spam/Heuristic.h +++ b/include/Spam/HBPlan/Heuristic.h @@ -46,22 +46,8 @@ //------------------------------------------------------------------------------ -//#include #include -#include -//#include - -//------------------------------------------------------------------------------ - -//namespace flann { -// template struct L2_Simple; -//}; -// -//namespace pcl { -// struct PointXYZ; -// template class KdTreeFLANN; -// struct PolygonMesh; -//}; +#include //------------------------------------------------------------------------------ @@ -86,47 +72,6 @@ class FTDrivenHeuristic : public golem::Heuristic { friend class Desc; friend class FTModelDesc; - /** Hypothesis over object poses */ - //class HypSample { - //public: - // friend class FTDrivenHeuristic; - // typedef golem::shared_ptr Ptr; - // typedef std::map Map; - - // /** Default construtor */ - // HypSample() {} - // /** Complete constructor */ - // HypSample(const golem::U32 idx, const grasp::RBPose::Sample &s, grasp::Cloud::PointSeq &p) { - // index = idx; - // sample = s; - // for (grasp::Cloud::PointSeq::const_iterator i = p.begin(); i != p.end(); ++i) - // points.push_back(*i); - // build(); - // //buildMesh(); - // } - // /** Destrutor */ - // ~HypSample() { - // pTree.release(); - // pTriangles.release(); - // } - // - //protected: - // /** Builds a pcl::PointCloud and its kd tree */ - // bool build(); - // /** Builds a pcl::PointCloud and its mesh */ - // bool buildMesh(); - - // /** Identifier */ - // golem::U32 index; - // /** Hypothesis */ - // grasp::RBPose::Sample sample; - // /** Point cloud */ - // grasp::Cloud::PointSeq points; - // /** Kd tree */ - // golem::shared_ptr>> pTree; - // /** Polygon mesh */ - // golem::shared_ptr pTriangles; - //}; /** Observational model for force/torque model of arm/hand robot Describes a cone over the normal of the end effector in which the likelihood of sensing a contact is greater than zero. @@ -178,6 +123,7 @@ class FTDrivenHeuristic : public golem::Heuristic { return true; } }; + /** Description file */ class Desc : public golem::Heuristic::Desc { protected: @@ -290,17 +236,6 @@ class FTDrivenHeuristic : public golem::Heuristic { /** Acquires manipulator */ inline void setManipulator(grasp::Manipulator *ptr) { manipulator.reset(ptr); pBelief->setManipulator(ptr); /*collision.reset(new Collision(context, *manipulator));*/ }; - /** Sets model cloud points */ -// void setModel(grasp::Cloud::PointSeq::const_iterator begin, grasp::Cloud::PointSeq::const_iterator end, const golem::Mat34 &transform); - /** Sets the current belief state */ -// void setBeliefState(grasp::RBPose::Sample::Seq &samples, const golem::Mat34 &transform); - /** Evaluate the likelihood of reading a contact between robot's pose and the sample */ -// golem::Real evaluate(const grasp::Manipulator *manipulator, const golem::Waypoint &w, const grasp::RBPose::Sample &sample, const std::vector &triggeredGuards, const grasp::RealSeq &force, const golem::Mat34 &trn) const; - /** Evaluate the likelihood of reading a contact between robot's pose and the sample */ -// golem::Real evaluate(const grasp::Manipulator *manipulator, const golem::Waypoint &w, const grasp::RBPose::Sample &sample, const std::vector &triggeredGuards, const grasp::RealSeq &force, const golem::Mat34 &trn) const; - /** Evaluate the likelihood of reading a contact between robot's pose and the sample */ -// golem::Real evaluate(const golem::Bounds::Seq &bounds, const grasp::RBCoord &pose, const grasp::RBPose::Sample &sample, const golem::Real &force, const golem::Mat34 &trn, bool &interect) const; - /** Collision detection test function for the single waypoint. * @param w waypoint * @return true if a collision has been detected; false otherwise @@ -335,7 +270,6 @@ class FTDrivenHeuristic : public golem::Heuristic { //if (collision.get()) { // golem::U32 jobs = 1; - // const golem::Parallels* parallels = context.getParallels(); // if (parallels != NULL) { // jobs = parallels->getNumOfThreads(); @@ -385,14 +319,6 @@ class FTDrivenHeuristic : public golem::Heuristic { inline bool expectedCollisions(const golem::Controller::State& state) const { const grasp::Manipulator::Config config = manipulator->getConfig(state); return hypothesisBoundsSeq.empty() ? false : intersect(manipulator->getBounds(config.config, config.frame.toMat34()), hypothesisBoundsSeq, false); - //if (intersect(manipulator->getBounds(manipulator->getConfig(state), manipulator->getPose(state).toMat34()), pBelief->uncertaintyRegionBounds(), false) && !hypothesisBoundsSeq.empty()) { - // return intersect(manipulator->getBounds(manipulator->getConfig(state), manipulator->getPose(state).toMat34()), hypothesisBoundsSeq, false); - //for (auto i = pBelief->getHypotheses().begin(); i != pBelief->getHypotheses().end(); ++i) - // if (intersect(manipulator->getBounds(manipulator->getConfig(state), manipulator->getPose(state).toMat34()), (*i)->bounds(), false)) - // return true; - //} - //return false; - //return intersect(manipulator->getBounds(manipulator->getConfig(state), manipulator->getPose(state).toMat34()), pBelief->uncertaintyRegionBounds(), false); } void renderHypothesisCollisionBounds(golem::DebugRenderer& renderer) { @@ -477,17 +403,6 @@ class FTDrivenHeuristic : public golem::Heuristic { /** Pair observational function over a trajectory */ void h(const golem::Waypoint &wi, const golem::Waypoint &wj, std::vector &y) const; - /** Distance to nearest k points on the object's surface */ -// golem::Real dist2NearestKPoints(const grasp::RBCoord &pose, Belief::Hypothesis::Seq::const_iterator p, const bool normal = true) const; - /** Distance to nearest point on the object's surface */ -// golem::Real dist2NearestPoint(const grasp::RBCoord &pose, Belief::Hypothesis::Seq::const_iterator p, const bool normal = true) const; - /** Distance to nearest k points using trimesh */ -// golem::Real dist2NearestTriangle(const grasp::RBCoord &pose, Belief::Hypothesis::Seq::const_iterator p, const bool normal = true) const; - /** Kernel function */ -// golem::Real kernel(golem::Real x, golem::Real lambda = golem::REAL_ONE) const; - /** Probability density value=p(x|p) for x given the sampled particle p */ -// golem::Real density(const grasp::RBCoord &x, const grasp::RBCoord &p) const; - /** Evaluate contact with hyptothesis */ golem::Real evaluate(const Hypothesis::Seq::const_iterator &hypothesis, const golem::Waypoint &w) const; diff --git a/include/Spam/HBPlan/Hypothesis.h b/include/Spam/HBPlan/Hypothesis.h new file mode 100644 index 0000000..a56effb --- /dev/null +++ b/include/Spam/HBPlan/Hypothesis.h @@ -0,0 +1,304 @@ +//------------------------------------------------------------------------------ +// Simultaneous Perception And Manipulation (SPAM) +//------------------------------------------------------------------------------ +// +// Copyright (c) 2010 University of Birmingham. +// All rights reserved. +// +// Intelligence Robot Lab. +// School of Computer Science +// University of Birmingham +// Edgbaston, Brimingham. UK. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// with the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimers. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimers in +// the documentation and/or other materials provided with the +// distribution. +// * Neither the names of the Simultaneous Perception And Manipulation +// (SPAM), University of Birmingham, nor the names of its contributors +// may be used to endorse or promote products derived from this Software +// without specific prior written permission. +// +// The software is provided "as is", without warranty of any kind, express or +// implied, including but not limited to the warranties of merchantability, +// fitness for a particular purpose and noninfringement. In no event shall the +// contributors or copyright holders be liable for any claim, damages or other +// liability, whether in an action of contract, tort or otherwise, arising +// from, out of or in connection with the software or the use of other dealings +// with the software. +// +//------------------------------------------------------------------------------ +//! @Author: Claudio Zito +//! @Date: 27/10/2012 +//------------------------------------------------------------------------------ +#pragma once +#ifndef _SPAM_HYPOTHESIS_H_ +#define _SPAM_HYPOTHESIS_H_ + +//------------------------------------------------------------------------------ + +#include +#include + +//------------------------------------------------------------------------------ + +namespace flann { + template struct L2_Simple; +}; + +namespace pcl { + struct PointXYZ; + template class KdTreeFLANN; + struct PolygonMesh; +}; + +namespace grasp { + class Manipulator; +}; + +namespace spam { + class FTDrivenHeuristic; + class Belief; +}; + +//------------------------------------------------------------------------------ + +namespace spam { + +//------------------------------------------------------------------------------ + +/** Hypothesis over object poses */ +class Hypothesis { +public: + friend class FTDrivenHeuristic; + friend class Belief; + typedef golem::shared_ptr Ptr; + typedef std::map Map; + typedef std::vector Seq; + + /** Bounds Appearance */ + class BoundsAppearance { + public: + /** Show bounds solid */ + bool showSolid; + /** Show bounds wire frames */ + bool showWire; + /** Bounds solid colour */ + golem::RGBA solidColour; + /** Bounds wire colour */ + golem::RGBA wireColour; + /** Bounds wireframe thickness */ + golem::Real wireWidth; + + /** Constructs from description object */ + BoundsAppearance() { + setToDefault(); + } + /** Sets the parameters to the default values */ + void setToDefault() { + showSolid = false; + showWire = true; + solidColour = golem::RGBA(golem::U8(255), golem::U8(255), golem::U8(0), golem::U8(127)); + wireColour = golem::RGBA(golem::U8(255), golem::U8(255), golem::U8(0), golem::U8(127)); + wireWidth = golem::Real(1.0); + } + /** Checks if the description is valid. */ + bool isValid() const { + if (wireWidth <= golem::REAL_ZERO) + return false; + return true; + } + + /** Draw bounds */ + void draw(const golem::Bounds::Seq& bounds, golem::DebugRenderer& renderer) const; + }; + + /** Appearance */ + class Appearance { + public: + /** Show frame */ + bool showFrames; + /** Show point cloud */ + bool showPoints; + /** Frame size of the sample */ + golem::Vec3 frameSize; + /** clolour of the point cloud */ + golem::RGBA colour; + + /** Bounds colour */ + BoundsAppearance bounds; + + /** Constructs from description object */ + Appearance() { + setToDefault(); + } + /** Sets the parameters to the default values */ + void setToDefault() { + showFrames = true; + showPoints = true; + frameSize.set(golem::Real(0.02)); + colour = golem::RGBA::MAGENTA; + bounds.setToDefault(); + } + /** Checks if the description is valid. */ + bool isValid() const { + if (!bounds.isValid()) + return false; + if (!frameSize.isPositive()) + return false; + return true; + } + }; + + class Desc { + public: + typedef golem::shared_ptr Ptr; + + Appearance appearance; + Collision::Desc::Ptr collisionDescPtr; + + /** Constructs description object */ + Desc() { + Desc::setToDefault(); + } + /** Nothing to do here */ + virtual ~Desc() {} + /** Creates the object from the description. */ + virtual Hypothesis::Ptr create(const grasp::Manipulator& manipulator) const { + return Hypothesis::Ptr(new Hypothesis(manipulator, *this)); + } + /** Sets description to default values */ + void setToDefault() { + appearance.setToDefault(); + collisionDescPtr.reset(new Collision::Desc()); + } + /** Checks if the description is valid. */ + virtual bool isValid() const { + if (!appearance.isValid()) + return false; + if (collisionDescPtr != nullptr && !collisionDescPtr->isValid()) + return false; + return true; + } + + }; + + /** Create */ + virtual void create(const golem::U32 idx, const golem::Mat34 &trn, const grasp::RBPose::Sample &s, golem::Rand& rand, const grasp::Cloud::PointSeq& points); + + /** Complete constructor */ + //Hypothesis(const golem::U32 idx, const golem::Mat34 &trn, const grasp::RBPose::Sample &s, grasp::Cloud::PointSeq &p) { + // index = idx; + // modelFrame = trn; + // sample = s; + // for (grasp::Cloud::PointSeq::const_iterator i = p.begin(); i != p.end(); ++i) + // points.push_back(*i); + // appearance.setToDefault(); + // boundsDesc.setToDefault(); + // build(); + // //buildMesh(); + //} + ///** Destrutor */ + //~Hypothesis() { + // pTree.release(); + // pTriangles.release(); + //} + + + /** Returns this sample in model frame **/ + inline grasp::RBPose::Sample toRBPoseSample() const { return sample; }; + /** Returns this sample in global frame (default: robot frame) **/ + inline grasp::RBPose::Sample toRBPoseSampleGF() const { return grasp::RBPose::Sample(sample.toMat34() * modelFrame, sample.weight, sample.cdf); }; + /** Returns the point cloud in global frame */ + inline grasp::Cloud::PointSeq getCloud() const { return points; }; + + /** Collision detection at a given waypoint */ + inline bool check(const Collision::Waypoint& waypoint, const grasp::Manipulator::Config& config, bool debug = false) const { + return collisionPtr->check(waypoint, config, debug); + }; + /** Collision detection at a given waypoint */ + inline bool check(const Collision::FlannDesc& desc, const golem::Rand& rand, const grasp::Manipulator::Config& config, bool debug = false) const { + return collisionPtr->check(desc, rand, config, debug); + } + + /** Collision detection at a given waypoint */ + inline virtual golem::Real estimate(const Collision::FlannDesc& desc, const grasp::Manipulator::Config& config, golem::Real maxDist = golem::REAL_MAX, bool debug = false) const { + return collisionPtr->estimate(desc, config, maxDist, debug); + } + + /** Collision likelihood estimation at a given waypoint */ + inline golem::Real evaluate(const Collision::Waypoint& waypoint, const grasp::Manipulator::Config& config, bool debug = false) const { + return collisionPtr->evaluate(waypoint, config, debug); + } + /** Collision likelihood estimation at a given waypoint */ + inline golem::Real evaluate(const Collision::FlannDesc& desc, const grasp::Manipulator::Config& config, bool debug = false) const { + return collisionPtr->evaluate(desc, config, debug); + } + + /** Return seq of bounds */ + golem::Bounds::Seq bounds(); + + /** Prints global pose of the hypothesis */ + std::string str() const; + + /** Draw hypotheses */ + void draw(golem::DebugRenderer &renderer) const; + + /** Draw collisions */ + void draw(const Collision::Waypoint &waypoint, const grasp::Manipulator::Config& config, golem::DebugRenderer& renderer) const; + /** Draw collision using kdtree */ + void draw(golem::DebugRenderer& renderer, const golem::Rand& rand, const grasp::Manipulator::Config& config) const; + /** Draw estimate */ + void draw(golem::DebugRenderer& renderer, const grasp::Manipulator::Config& config, const Collision::FlannDesc& desc) const { + collisionPtr->draw(renderer, config, desc); + } + /** Draw simulate */ + void draw(golem::DebugRenderer& renderer, const grasp::Manipulator::Config& config, std::vector &joints, grasp::RealSeq &forces, const Collision::FlannDesc& desc) const { + collisionPtr->draw(renderer, config, joints, forces, desc); + } + Appearance appearance; + +protected: + /** Identifier */ + golem::U32 index; + /** Model frame **/ + golem::Mat34 modelFrame; + /** Hypothesis. NOTE: contains the query (or sample) frame w.r.t model frame **/ + grasp::RBPose::Sample sample; + /** Point cloud */ + grasp::Cloud::PointSeq points; + + /** Bounding box desc for the object */ + golem::BoundingBox::Desc boundsDesc; + + + /** Manipulator */ + const grasp::Manipulator& manipulator; + /** Description */ + const Desc desc; + + /** Collision detection pointer */ + Collision::Ptr collisionPtr; + + /** Create */ + Hypothesis(const grasp::Manipulator& manipulator, const Desc& desc); +}; + +//------------------------------------------------------------------------------ + +void XMLData(Hypothesis::Desc& val, golem::XMLContext* xmlcontext, bool create = false); + +//------------------------------------------------------------------------------ + +}; /** namespace */ + +#endif /** _SPAM_HYPOTHESIS_H_ */ \ No newline at end of file diff --git a/include/Spam/ShapePlanner/ShapePlanner.h b/include/Spam/ShapePlanner/ShapePlanner.h deleted file mode 100644 index 9b9561a..0000000 --- a/include/Spam/ShapePlanner/ShapePlanner.h +++ /dev/null @@ -1,432 +0,0 @@ -//------------------------------------------------------------------------------ -// Simultaneous Perception And Manipulation (SPAM) -//------------------------------------------------------------------------------ -// -// Copyright (c) 2010 University of Birmingham. -// All rights reserved. -// -// Intelligence Robot Lab. -// School of Computer Science -// University of Birmingham -// Edgbaston, Brimingham. UK. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// with the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimers. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimers in -// the documentation and/or other materials provided with the -// distribution. -// * Neither the names of the Simultaneous Perception And Manipulation -// (SPAM), University of Birmingham, nor the names of its contributors -// may be used to endorse or promote products derived from this Software -// without specific prior written permission. -// -// The software is provided "as is", without warranty of any kind, express or -// implied, including but not limited to the warranties of merchantability, -// fitness for a particular purpose and noninfringement. In no event shall the -// contributors or copyright holders be liable for any claim, damages or other -// liability, whether in an action of contract, tort or otherwise, arising -// from, out of or in connection with the software or the use of other dealings -// with the software. -// -//------------------------------------------------------------------------------ -//! @Author: Claudio Zito -//! @Date: 08/05/2014 -//------------------------------------------------------------------------------ -#pragma once -#ifndef _SPAM_SHAPEPLANNER_SHAPEPLANNER_H_ -#define _SPAM_SHAPEPLANNER_SHAPEPLANNER_H_ - -//------------------------------------------------------------------------------ - -#include -#include -#include -#include -#include -#include -#include - -//------------------------------------------------------------------------------ - -namespace spam { - -//------------------------------------------------------------------------------ - -/** ShapePlanner. */ -class ShapePlanner : public PosePlanner { -public: - /** Grasp mode */ - enum Mode { - MODE_DISABLED, - MODE_DATA, - MODE_CLUSTER, - MODE_CONFIG, - MODE_CONFIG_MODEL, - MODE_CONTACT_MODEL, - MODE_SIZE = MODE_CONTACT_MODEL, - }; - /** Mode default name */ - static const char* ModeName[MODE_SIZE + 1]; - - /** Data */ - class Data : public PosePlanner::Data { - public: - /** Grasp configs */ - grasp::Grasp::Config::Seq graspConfigs; - /** Grasp config clusters */ - grasp::Cluster::Seq graspClusters; - - /** Grasp configs file name extension */ - std::string extGraspConfig; - /** Grasp config clusters file name extension */ - std::string extGraspCluster; - - /** Grasp mode */ - golem::U32 graspMode; - /** Grasp data pointer */ - mutable golem::U32 graspDataPtr; - /** Grasp config cluster pointer */ - mutable golem::U32 graspClusterPtr, graspConfigPtr; - - /** Data appearance */ - mutable grasp::Grasp::Data::Appearance appearanceData; - /** Config appearance */ - mutable grasp::Grasp::Config::Appearance appearanceConfig; - - /** Reset data during construction */ - Data() { - setToDefault(); - } - - /** Sets the parameters to the default values */ - virtual void setToDefault() { - PosePlanner::Data::setToDefault(); - - graspConfigs.clear(); - graspClusters.clear(); - - extGraspConfig = ".graspconfig"; - extGraspCluster = ".graspcluster"; - - resetDataPointers(); - } - /** Assert that the description is valid. */ - virtual void assertValid(const grasp::Assert::Context& ac) const { - Player::Data::assertValid(ac); - - grasp::Assert::valid(extGraspConfig.length() > 0, ac, "extGraspConfig: length 0"); - grasp::Assert::valid(extGraspCluster.length() > 0, ac, "extGraspCluster: length 0"); - } - - /** Reads/writes object from/to a given XML context */ - virtual void xmlData(golem::XMLContext* context, bool create = false) const; - - /** Reset data pointers */ - void resetDataPointers() { - graspMode = MODE_DISABLED; - graspDataPtr = 0; - graspClusterPtr = graspConfigPtr = 0; - } - - /** Retrieve grasp data from pointer */ - template bool getGraspData(_GraspMap& map, _GraspMapIter& grasp, _GraspDataMapIter& data) const { - golem::U32 n = -1; - - // search for index - for (_GraspMapIter i = map.begin(); i != map.end(); ++i) - for (_GraspDataMapIter j = i->second->getDataMap().begin(); j != i->second->getDataMap().end(); ++j) { - grasp = i; - data = j; - if (++n == graspDataPtr) - return true; - } - - // there is some data - if (n != -1) { - graspDataPtr = n; - return true; - } - - return false; - } - /** Set grasp data pointer */ - void setGraspData(const grasp::Grasp::Map& map, grasp::Grasp::Data::Map::const_iterator data); - - /** Grasp configs */ - inline void assertGraspConfigs() const { - graspClusterPtr = graspClusters.empty() ? 0 : graspClusterPtr >= (golem::U32)graspClusters.size() ? (golem::U32)graspClusters.size() - 1 : graspClusterPtr; - graspConfigPtr = graspClusters.empty() || graspConfigs.empty() || graspClusters[graspClusterPtr].end <= 0 ? 0 : graspConfigPtr >= graspClusters[graspClusterPtr].end ? graspClusters[graspClusterPtr].end - 1 : graspConfigPtr; - } - /** Grasp configs */ - inline bool hasGraspConfigs() const { - assertGraspConfigs(); - return !graspConfigs.empty() && (golem::U32)graspClusters.size() > graspClusterPtr && (golem::U32)graspConfigs.size() > graspClusters[graspClusterPtr].begin + graspConfigPtr; - } - /** Grasp config begin */ - inline grasp::Grasp::Config::Seq::const_iterator getGraspConfigBegin() const { - return hasGraspConfigs() ? graspConfigs.begin() + graspClusters[graspClusterPtr].begin : graspConfigs.end(); - } - /** Grasp config end */ - inline grasp::Grasp::Config::Seq::const_iterator getGraspConfigEnd() const { - return hasGraspConfigs() ? graspConfigs.begin() + graspClusters[graspClusterPtr].end : graspConfigs.end(); - } - /** Grasp config size */ - inline golem::U32 getGraspConfigSize() const { - return hasGraspConfigs() ? graspClusters[graspClusterPtr].size() : 0; - } - /** Current grasp pose */ - inline grasp::Grasp::Config::Seq::const_iterator getGraspConfig() const { - return hasGraspConfigs() ? graspConfigs.begin() + graspClusters[graspClusterPtr].begin + graspConfigPtr : graspConfigs.end(); - } - - /** Grasp info */ - void printGraspInfo(const grasp::Manipulator& manipulator) const; - }; - - /** Grasp demo */ - class Demo { - public: - typedef std::map Map; - - /** Object detection */ - class ObjectDetection { - public: - typedef std::vector Seq; - - /** Scan pose */ - Robot::Pose scanPose; - /** Camera name */ - std::string cameraName; - /** Minimum size */ - golem::U32 minSize; - /** Object detection delta change */ - golem::U32 deltaSize; - /** Object detection delta depth */ - golem::Real deltaDepth; - - /** Constructs from description */ - ObjectDetection() { - setToDefault(); - } - /** Sets the parameters to the default values */ - void setToDefault() { - scanPose.setToDefault(); - cameraName.clear(); - minSize = 10000; - deltaSize = 100; - deltaDepth = golem::Real(0.001); - } - /** Checks if the description is valid. */ - bool isValid() const { - scanPose.assertValid(grasp::Assert::Context("grasp::ShapePlanner::Desc::isValid(): scanPose.")); - if (cameraName.length() <= 0 || minSize < 1 || deltaDepth < golem::REAL_EPS) - return false; - return true; - } - }; - - /** Pose estimation */ - bool poseEstimation; - - /** Object detection */ - ObjectDetection::Seq objectDetectionSeq; - - /** Classifier name */ - std::string classifierName; - - /** Object model path */ - std::string modelObject; - /** Trajectory model path */ - grasp::StringSeq modelTrajectories; - - /** Poses */ - Robot::Pose::Seq poses; - - /** Constructs from description */ - Demo() { - setToDefault(); - } - /** Sets the parameters to the default values */ - void setToDefault() { - poseEstimation = true; - - objectDetectionSeq.clear(); - - classifierName.clear(); - modelObject.clear(); - modelTrajectories.clear(); - - poses.clear(); - } - /** Checks if the description is valid. */ - bool isValid() const { - if (objectDetectionSeq.empty()) - return false; - for (ObjectDetection::Seq::const_iterator i = objectDetectionSeq.begin(); i != objectDetectionSeq.end(); ++i) - if (!i->isValid()) - return false; - - if (poseEstimation ? modelObject.length() <= 0 || modelTrajectories.empty() : classifierName.length() <= 0) - return false; - for (grasp::StringSeq::const_iterator i = modelTrajectories.begin(); i != modelTrajectories.end(); ++i) - if (i->length() <= 0) - return false; - - for (grasp::RobotPose::Seq::const_iterator i = poses.begin(); i != poses.end(); ++i) i->assertValid(grasp::Assert::Context("grasp::ShapePlanner::Demo::isValid(): poses[].")); - - return true; - } - }; - - class Desc : public PosePlanner::Desc { - protected: - CREATE_FROM_OBJECT_DESC1(ShapePlanner, golem::Object::Ptr, golem::Scene&) - - public: - typedef golem::shared_ptr Ptr; - - /** Grasp classifiers */ - grasp::Manipulator::Desc::Ptr manipulatorDesc; - - /** Grasp classifiers */ - grasp::Classifier::Desc::Map classifierDescMap; - - /** Clustering algorithm */ - grasp::Cluster::Desc clusterDesc; - - /** Demo descriptions */ - Demo::Map demoMap; - - /** Grasp classifier file name extension */ - std::string extGraspClass; - - /** Data appearance */ - grasp::Grasp::Data::Appearance appearanceData; - /** Config appearance */ - grasp::Grasp::Config::Appearance appearanceConfig; - - /** Constructs from description object */ - Desc() { - Desc::setToDefault(); - } - /** Sets the parameters to the default values */ - virtual void setToDefault() { - PosePlanner::Desc::setToDefault(); - - data.reset(new Data); - - manipulatorDesc.reset(new grasp::Manipulator::Desc); - classifierDescMap.clear(); - clusterDesc.setToDefault(); - demoMap.clear(); - extGraspClass = ".graspclass"; - - appearanceData.setToDefault(); - appearanceConfig.setToDefault(); - } - /** Checks if the description is valid. */ - virtual bool isValid() const { - if (!PosePlanner::Desc::isValid()) - return false; - - if (manipulatorDesc == nullptr || !manipulatorDesc->isValid()) - return false; - - if (classifierDescMap.empty()) - return false; - for (grasp::Classifier::Desc::Map::const_iterator i = classifierDescMap.begin(); i != classifierDescMap.end(); ++i) - if (i->first.length() <= 0 || !i->second->isValid()) - return false; - - if (!clusterDesc.isValid()) - return false; - - for (ShapePlanner::Demo::Map::const_iterator i = demoMap.begin(); i != demoMap.end(); ++i) - if (!i->second.isValid()) - return false; - - if (extGraspClass.length() <= 0) - return false; - - if (!appearanceData.isValid() || !appearanceConfig.isValid()) - return false; - - return true; - } - }; - -protected: - /** Manipulator proxy */ - grasp::Manipulator::Ptr manipulator; - - /** Grasp classifier descriptions */ - grasp::Classifier::Desc::Map classifierDescMap; - /** Current grasp classifier */ - grasp::Classifier::Ptr classifier; - /** Clustering algorithm */ - grasp::Cluster::Desc clusterDesc; - - /** Grasp model renderers */ - golem::DebugRenderer graspRenderer; - - /** Demo descriptions */ - Demo::Map demoMap; - - /** Grasp classifier file name extension */ - std::string extGraspClass; - - /** Creates new data */ - virtual Data::Ptr createData() const; - - /** Load classifier */ - grasp::Classifier::Ptr load(const grasp::Classifier::Desc& desc); - - /** Render data */ - virtual void renderData(Data::Map::const_iterator dataPtr); - /** Reset grasp data pointers */ - void resetDataPointers(); - - /** Shape planner demo */ - void run(const Demo::Map::value_type& demo); - - /** User interface: menu function */ - virtual void function(Data::Map::iterator& dataPtr, int key); - - virtual void render(); - virtual void mouseHandler(int button, int state, int x, int y); - - ShapePlanner(golem::Scene &scene); - bool create(const Desc& desc); -}; - -/** Reads/writes object from/to a given XML context */ -void XMLData(ShapePlanner::Demo::ObjectDetection &val, golem::XMLContext* xmlcontext, bool create = false); -/** Reads/writes object from/to a given XML context */ -void XMLData(ShapePlanner::Demo &val, golem::XMLContext* xmlcontext, bool create = false); -/** Reads/writes object from/to a given XML context */ -void XMLData(ShapePlanner::Demo::Map::value_type &val, golem::XMLContext* xmlcontext, bool create = false); -/** Reads/writes object from/to a given XML context */ -void XMLData(ShapePlanner::Desc &val, golem::Context* context, golem::XMLContext* xmlcontext, bool create = false); - -//------------------------------------------------------------------------------ - -/** ShapePlanner application */ -class ShapePlannerApp : public golem::Application { -protected: - /** Runs Application */ - virtual void run(int argc, char *argv[]); -}; - -//------------------------------------------------------------------------------ - -}; // namespace - -#endif /*_GRASP_SHAPEPLANNER_SHAPEPLANNER_H_*/ diff --git a/include/Spam/Spam/Data.h b/include/Spam/Spam/Data.h deleted file mode 100644 index 6030d9c..0000000 --- a/include/Spam/Spam/Data.h +++ /dev/null @@ -1,334 +0,0 @@ -//------------------------------------------------------------------------------ -// Simultaneous Perception And Manipulation (SPAM) -//------------------------------------------------------------------------------ -// -// Copyright (c) 2010 University of Birmingham. -// All rights reserved. -// -// Intelligence Robot Lab. -// School of Computer Science -// University of Birmingham -// Edgbaston, Brimingham. UK. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// with the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimers. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimers in -// the documentation and/or other materials provided with the -// distribution. -// * Neither the names of the Simultaneous Perception And Manipulation -// (SPAM), University of Birmingham, nor the names of its contributors -// may be used to endorse or promote products derived from this Software -// without specific prior written permission. -// -// The software is provided "as is", without warranty of any kind, express or -// implied, including but not limited to the warranties of merchantability, -// fitness for a particular purpose and noninfringement. In no event shall the -// contributors or copyright holders be liable for any claim, damages or other -// liability, whether in an action of contract, tort or otherwise, arising -// from, out of or in connection with the software or the use of other dealings -// with the software. -// -//------------------------------------------------------------------------------ -//! @Author: Claudio Zito -//! @Date: 27/10/2012 -//------------------------------------------------------------------------------ -#pragma once -#ifndef _SPAM_DATA_H_ -#define _SPAM_DATA_H_ - -//------------------------------------------------------------------------------ - -//#include -//#include -//#include -#include -#include -#include -#include - -//------------------------------------------------------------------------------ - -namespace spam { - -////------------------------------------------------------------------------------ -// -//void XMLData(FTDrivenHeuristic::FTModelDesc& val, golem::XMLContext* context, bool create = false); -// -//void XMLData(FTDrivenHeuristic::Desc& val, golem::XMLContext* context, bool create = false); - -///** Reads/writes object from/to a given XML context */ -//void XMLData(spam::Robot::Desc &val, golem::Context* context, golem::XMLContext* xmlcontext, bool create = false); - -//void XMLData(RagGraphPlanner::Desc &val, golem::XMLContext* context, bool create = false); - -//------------------------------------------------------------------------------ - -/** Trial data for a single demonstration and/or test trial in a binary format (stored on disk) */ -class TrialData { -public: - typedef std::map Map; - friend class golem::Stream; - - /** Header name */ - static const char headerName []; - /** Header version */ - static const golem::U32 headerVersion; - - /** Cache (local): OpenGL settings */ - golem::Scene::OpenGL openGL; - /** Cache (local): action */ - golem::Controller::State::Seq action; - - /** Ieration index */ - std::size_t iteration; - - /** Object label */ - std::string objectLabel; - /** Grasps */ - golem::Controller::State::Seq grasps; - - ///** Approach action waypoints */ - //grasp::RobotState::List approachAction; - ///** Manipulation action waypoints */ - //grasp::RobotState::List manipAction; - ///** Withdraw action waypoints */ - //grasp::RobotState::List approachWithdraw; - ///** Combined action waypoints */ - golem::Controller::State::Seq executedTrajectory; - - /** High dim representation of pdf */ - grasp::RBPose::Sample::Seq density; - /** High dim rep covariance */ - grasp::RBCoord densityCov; - /** Low dim representation of pdf */ - grasp::RBPose::Sample::Seq hypotheses; - /** High dim rep covariance */ - grasp::RBCoord hypothesesCov; - /** Normalise factor */ - golem::Real normFac; - - /** Specified if the trial has noise */ - bool gtNoiseEnable; - /** Trial noise: linear component */ - golem::Vec3 noiseLin; - /** Trial noise: angular component */ - golem::Quat noiseAng; - - /** Specifies if guard have been triggered during the perform of the action */ - int triggered; - /** Contains the index of the triggered guards */ -// std::vector triggeredGuards; - std::vector triggeredGuards; - /** State of the robot at the time a contact occurred */ - golem::Controller::State::Seq triggeredStates; - - /** Safety configurations of the robot */ - grasp::RobotState::Seq homeStates; - - /** Specifies if the replanning should be triggered */ - bool replanning; - - /** Constructor */ - TrialData(const golem::Controller& controller) : controller(controller) { - } - -protected: - const golem::Controller& controller; -}; - -//------------------------------------------------------------------------------ - -//class Collision { -//public: -// typedef std::shared_ptr Ptr; -// /** Bounds */ -// class Bounds { -// public: -// typedef std::vector Seq; -// -// /** Surface */ -// struct Surface { -// typedef std::vector Seq; -// typedef std::vector SeqSeq; -// golem::Vec3 point; -// golem::Vec3 normal; -// }; -// /** Triangle */ -// struct Triangle : public Surface { -// typedef std::vector Seq; -// typedef std::vector SeqSeq; -// golem::Real distance; -// }; -// -// /** Create bounds from convex meshes */ -// void create(const golem::Bounds::Seq& bounds); -// -// /** Pose */ -// static inline void setPose(const golem::Mat34& pose, const Surface& surface, Triangle& triangle) { -// pose.multiply(triangle.point, surface.point); -// pose.R.multiply(triangle.normal, surface.normal); -// triangle.distance = triangle.normal.dot(triangle.point); -// } -// /** Pose */ -// static inline void setPose(const golem::Mat34& pose, const Surface::Seq& surfaces, Triangle::Seq& triangles) { -// triangles.resize(surfaces.size()); -// for (size_t i = 0; i < triangles.size(); ++i) -// setPose(pose, surfaces[i], triangles[i]); -// } -// /** Pose */ -// inline void setPose(const golem::Mat34& pose) { -// for (size_t i = 0; i < triangles.size(); ++i) -// setPose(pose, surfaces[i], triangles[i]); -// } -// -// /** Penetration depth of a given point */ -// static inline golem::Real getDepth(const Triangle::Seq& triangles, const golem::Vec3& point) { -// golem::Real depth = golem::REAL_MAX; -// for (Triangle::Seq::const_iterator i = triangles.begin(); i != triangles.end(); ++i) { -// const golem::Real d = i->distance - i->normal.dot(point); -// if (d < golem::REAL_ZERO) // no collision -// return d; -// if (depth > d) // search for minimum -// depth = d; -// } -// return depth; -// } -// /** Penetration depth of a given point, zero if none */ -// inline golem::Real getDepth(const golem::Vec3& point) const { -// golem::Real depth = golem::REAL_ZERO; // if no bounds or collisions, no effect -// for (Triangle::SeqSeq::const_iterator i = triangles.begin(); i != triangles.end(); ++i) { -// const golem::Real d = getDepth(*i, point); -// if (depth < d) // search for maximum depth (can collide only with one mesh within bounds) -// depth = d; -// } -// return depth; -// } -// -// /** Empty */ -// inline bool empty() const { -// return surfaces.empty(); -// } -// -// /** Triangles */ -// const Triangle::SeqSeq& getTriangles() const { -// return triangles; -// } -// /** Surfaces */ -// const Surface::SeqSeq& getSurfaces() const { -// return surfaces; -// } -// -// private: -// /** Triangles */ -// Triangle::SeqSeq triangles; -// /** Surfaces */ -// Surface::SeqSeq surfaces; -// }; -// -// /** Collision waypoint */ -// class Waypoint { -// public: -// typedef std::vector Seq; -// -// /** Path distance */ -// golem::Real pathDist; -// /** Number of points */ -// golem::U32 points; -// /** Distance standard deviation */ -// golem::Real depthStdDev; -// /** Likelihood multiplier */ -// golem::Real likelihood; -// -// /** Constructs description object */ -// Waypoint() { -// Waypoint::setToDefault(); -// } -// /** Sets the parameters to the default values */ -// void setToDefault() { -// pathDist = golem::Real(0.0); -// points = 1000; -// depthStdDev = golem::Real(1000.0); -// likelihood = golem::Real(1000.0); -// } -// /** Checks if the parameters are valid. */ -// bool isValid() const { -// if (!golem::Math::isFinite(pathDist) || depthStdDev < golem::REAL_EPS || likelihood < golem::REAL_ZERO) -// return false; -// return true; -// } -// }; -// -// /** Collision description */ -// class Desc { -// public: -// /** Enable collisions during last optimisation step */ -// bool enabledLast; -// /** Enable collisions during all optimisation steps */ -// bool enabledAll; -// /** Collision waypoints */ -// Waypoint::Seq waypoints; -// -// /** Constructs description object */ -// Desc() { -// Desc::setToDefault(); -// } -// /** Sets the parameters to the default values */ -// void setToDefault() { -// enabledLast = true; -// enabledAll = false; -// waypoints.clear(); -// waypoints.push_back(Waypoint()); -// } -// /** Checks if the description is valid. */ -// bool isValid() const { -// if (waypoints.empty()) -// return false; -// for (Waypoint::Seq::const_iterator i = waypoints.begin(); i != waypoints.end(); ++i) -// if (!i->isValid()) -// return false; -// return true; -// } -// }; -// -// /** Create */ -// Collision(const grasp::Manipulator& manipulator); -// /** Collision likelihood estimation at a given waypoint */ -// golem::Real evaluate(const Robot *robot, const Waypoint& waypoint, const grasp::Cloud::PointSeq& points, golem::Rand& rand, const grasp::Manipulator::Pose& pose, std::vector &joints, bool debug = false); -// /** Collision likelihood estimation at a given waypoint */ -// golem::Real evaluate(const Waypoint& waypoint, const grasp::Cloud::PointSeq& points, golem::Rand& rand, const grasp::Manipulator::Pose& pose, bool debug = false); -// /** Bounds */ -// const Bounds& getBounds(size_t index) const { -// return bounds[index]; -// } -// -//protected: -// /** Manipulator */ -// const grasp::Manipulator& manipulator; -// /** Joints + base */ -// Bounds bounds[grasp::Manipulator::JOINTS + 1]; -//}; - -//------------------------------------------------------------------------------ - -}; /** namespace */ - -//------------------------------------------------------------------------------ - -//namespace golem { -// -//template <> void Stream::read(spam::TrialData& trialData) const; -//template <> void Stream::write(const spam::TrialData& trialData); -// -//}; // namespace - -//------------------------------------------------------------------------------ - -#endif /** _SPAM_DATA_H_ */ \ No newline at end of file diff --git a/include/Spam/Spam/Robot.h b/include/Spam/Spam/Robot.h deleted file mode 100644 index 6bf952f..0000000 --- a/include/Spam/Spam/Robot.h +++ /dev/null @@ -1,232 +0,0 @@ -//------------------------------------------------------------------------------ -// Simultaneous Perception And Manipulation (SPAM) -//------------------------------------------------------------------------------ -// -// Copyright (c) 2010 University of Birmingham. -// All rights reserved. -// -// Intelligence Robot Lab. -// School of Computer Science -// University of Birmingham -// Edgbaston, Brimingham. UK. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// with the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimers. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimers in -// the documentation and/or other materials provided with the -// distribution. -// * Neither the names of the Simultaneous Perception And Manipulation -// (SPAM), University of Birmingham, nor the names of its contributors -// may be used to endorse or promote products derived from this Software -// without specific prior written permission. -// -// The software is provided "as is", without warranty of any kind, express or -// implied, including but not limited to the warranties of merchantability, -// fitness for a particular purpose and noninfringement. In no event shall the -// contributors or copyright holders be liable for any claim, damages or other -// liability, whether in an action of contract, tort or otherwise, arising -// from, out of or in connection with the software or the use of other dealings -// with the software. -// -//------------------------------------------------------------------------------ -//! @Author: Claudio Zito -//! @Date: 27/10/2012 -//------------------------------------------------------------------------------ -#pragma once -#ifndef _SPAM_SPAM_ROBOT_H_ -#define _SPAM_SPAM_ROBOT_H_ - -//------------------------------------------------------------------------------ - -#include -#include -#include -#include - -//------------------------------------------------------------------------------ - -namespace spam { - -//------------------------------------------------------------------------------ - -/** General active ctroller debug info */ -std::string controllerDebug(grasp::ActiveCtrl &ctrl); - -//------------------------------------------------------------------------------ - -/** Robot is the interafce to a robot (right arm + hand), sensing devices and objects. */ -class Robot : public grasp::Robot { -public: - typedef golem::shared_ptr Ptr; - - /** Force reader. Overwrite the ActiveCtrl reader. this retrieves the triggered joints */ - typedef std::function&)> GuardsReader; - - /** Robot factory */ - class Desc : public grasp::Robot::Desc { - protected: - CREATE_FROM_OBJECT_DESC1(Robot, golem::Object::Ptr, golem::Scene&) - - public: - typedef golem::shared_ptr Ptr; - - /** Constructs from description object */ - Desc() { - Desc::setToDefault(); - } - /** Sets the parameters to the default values */ - virtual void setToDefault() { - grasp::Robot::Desc::setToDefault(); -// physPlannerDesc->pPlannerDesc->pHeuristicDesc.reset(new FTDrivenHeuristic::Desc); - } - /** Checks if the description is valid. */ - virtual bool isValid() const { - if (!grasp::Robot::Desc::isValid()) - return false; - return true; - } - /** virtual destructor is required */ - virtual ~Desc() { - } - }; - - /** (Global search) trajectory of the entire robot from the configuration space and/or workspace target */ - virtual void createTrajectory(const golem::Controller::State& begin, const golem::Controller::State* pcend, const golem::Mat34* pwend, golem::SecTmReal t, const golem::Controller::State::Seq& waypoints, golem::Controller::State::Seq& trajectory); - /** (Local search) trajectory of the arm only from a sequence of configuration space targets in a new reference frame */ - grasp::RBDist trnTrajectory(const golem::Mat34& actionFrame, const golem::Mat34& modelFrame, const golem::Mat34& trn, golem::Controller::State::Seq::const_iterator begin, golem::Controller::State::Seq::const_iterator end, golem::Controller::State::Seq& trajectory); - /** (Local search) trajectory of the arm only from a sequence of configuration space targets in a new reference frame */ - grasp::RBDist findTrnTrajectory(const golem::Mat34& trn, const golem::Controller::State& startPose, golem::Controller::State::Seq::const_iterator begin, golem::Controller::State::Seq::const_iterator end, golem::Controller::State::Seq& trajectory); - - /** Returns a seq of triggered guards if any */ -// FTGuard::Seq getTriggeredGuard() { return triggeredGuards; }; - - ///** Checks if triggered and return in case a vector of indeces */ - //virtual int getTriggeredGuards(FTGuard::Seq &triggeredGuards, golem::Controller::State &state); - - ///** Checks guards on justin and bham robot */ - //int checkGuards(std::vector &triggeredGuards, golem::Controller::State &state); - - ///** Reads torque/force values from the state */ - void readFT(const golem::Controller::State &state, grasp::RealSeq &force) const; - - /** Activates collision detection with group of bounds */ - inline void setCollisionBoundsGroup(golem::U32 collisionGroup) { - physPlanner->setCollisionBoundsGroup(collisionGroup); - } - /** Actives/deactivates collision detection during planning */ - inline void setCollisionDetection(bool collisionDetection) { - pFTDrivenHeuristic->setCollisionDetection(collisionDetection); - } - - ///** Checks if the object is in the hand */ - //size_t isGrasping(FTGuard::Seq &triggeredJoints, golem::Controller::State &state); - - /** Finds a target in configuration space in a new reference frame */ - void findTarget(const golem::Mat34 &trn, const golem::Controller::State &target, golem::Controller::State &cend, const bool lifting = false); - - /** Initialises the cycle */ - inline void initControlCycle() { - arm->initControlCycle(); - hand->initControlCycle(); - } - - /** Stop active controller */ - void stopActiveController(); - /** Activate active controller */ - void startActiveController(const golem::I32 steps = grasp::ActiveCtrl::STEP_DEFAULT); - /** Emergency mode for the controller */ - void emergencyActiveController(); - /** Checks for reions with high likelihood of contacts */ - inline bool expectedCollisions(const golem::Controller::State& state) const { - return pFTDrivenHeuristic->expectedCollisions(state); - } - - inline golem::Real getFilterForce(const golem::I32 i) { - return handFilteredForce[i]; - } - inline grasp::RealSeq getFilterForce() { - return handFilteredForce; - } - - /** Force reader */ - GuardsReader guardsReader; - /** Collect history of FT readings for statistics */ - grasp::ActiveCtrl::ForceReader collectFTInp; - /** FT 2-oreder high-pass filter */ - grasp::ActiveCtrl::ForceReader ftFilter; - - //golem::shared_ptr objectPointCloudPtr; - //Collision::Ptr collision; - //Collision::Waypoint w; - ///** Manipulator pointer */ - //grasp::Manipulator::Ptr manipulator; - ///** Acquires manipulator */ - //inline void setManipulator(grasp::Manipulator *ptr) { manipulator.reset(ptr); collision.reset(new Collision(context, *manipulator)); }; - //golem::Rand rand; -protected: - /** Force/torque driven heuristic for robot controller */ - FTDrivenHeuristic* pFTDrivenHeuristic; - - /** Number of averaging steps before stopping collecting readings */ - golem::U32 windowSize; - golem::I32 steps; - // gaussian filter mask - grasp::RealSeq mask; - // computes gaussian - template inline _Real N(const _Real mean, const _Real stdev) { - const _Real norm = golem::numeric_const<_Real>::ONE / (stdev*Math::sqrt(2 * golem::numeric_const<_Real>::PI)); - return norm*golem::Math::exp(-.5*Math::sqr(_Real(mean) / _Real(stdev))); // gaussian - } - // computes guassian on a vector - template inline std::vector<_Real> N(_Ptr begin, _Ptr end, const size_t dim, const _Real stdev) { - std::vector<_Real> output; - output.assign(dim, golem::numeric_const<_Real>::ZERO); - size_t idx = 0; - for (Ptr i = begin; i != end; ++i) { - output[idx++] = N(*i, stdev); - } - return output; - } - - - /** Input force at sensor, sequence */ - std::vector forceInpSensorSeq; - /** Filtered forces for the hand */ - grasp::RealSeq handFilteredForce; - /** force reader access cs */ - golem::CriticalSection csHandForce; - - - inline golem::I32 dimensions() { return (golem::I32)handInfo.getJoints().size(); } - - /** Trigguered F/T guards */ - FTGuard::Seq triggeredGuards; - - /** Robot descriptor */ - Desc desc; - // golem::Object interface - virtual void render(); - - // construction - Robot(golem::Scene &scene); - bool create(const Desc& desc); -}; - -//------------------------------------------------------------------------------ - -/** Reads/writes object from/to a given XML context */ -void XMLData(spam::Robot::Desc &val, golem::Context* context, golem::XMLContext* xmlcontext, bool create = false); - -//------------------------------------------------------------------------------ - -}; // namespace - -#endif /*_SPAM_SPAM_ROBOT_H_*/ diff --git a/resources/GraspDataImageSPAMObject.xml b/resources/GraspDataImageSPAMObject.xml deleted file mode 100644 index 8030b40..0000000 --- a/resources/GraspDataImageSPAMObject.xml +++ /dev/null @@ -1,73 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/Spam/Data/SpamDataBelief.xml b/resources/Spam/Data/SpamDataBelief.xml new file mode 100644 index 0000000..1689ecd --- /dev/null +++ b/resources/Spam/Data/SpamDataBelief.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/Spam/Data/SpamDataR2GTrajectory.xml b/resources/Spam/Data/SpamDataR2GTrajectory.xml new file mode 100644 index 0000000..1689ecd --- /dev/null +++ b/resources/Spam/Data/SpamDataR2GTrajectory.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/GraspCameraDepthSimSPAM.xml b/resources/Spam/Demo/GraspCameraDepthSimDemoRag.xml similarity index 100% rename from resources/GraspCameraDepthSimSPAM.xml rename to resources/Spam/Demo/GraspCameraDepthSimDemoRag.xml diff --git a/resources/GraspCameraKinectSPAM.xml b/resources/Spam/Demo/GraspCameraKinectDemoRag.xml similarity index 100% rename from resources/GraspCameraKinectSPAM.xml rename to resources/Spam/Demo/GraspCameraKinectDemoRag.xml diff --git a/resources/GraspCameraOpenCVSPAM.xml b/resources/Spam/Demo/GraspCameraOpenCVDemoRag.xml similarity index 100% rename from resources/GraspCameraOpenCVSPAM.xml rename to resources/Spam/Demo/GraspCameraOpenCVDemoRag.xml diff --git a/resources/GraspCameraOpenGLSPAM.xml b/resources/Spam/Demo/GraspCameraOpenGLDemoRag.xml similarity index 100% rename from resources/GraspCameraOpenGLSPAM.xml rename to resources/Spam/Demo/GraspCameraOpenGLDemoRag.xml diff --git a/resources/GraspCameraOpenNISPAM.cal b/resources/Spam/Demo/GraspCameraOpenNIDemoRag.cal similarity index 100% rename from resources/GraspCameraOpenNISPAM.cal rename to resources/Spam/Demo/GraspCameraOpenNIDemoRag.cal diff --git a/resources/GraspCameraOpenNISPAM.xml b/resources/Spam/Demo/GraspCameraOpenNIDemoRag.xml similarity index 100% rename from resources/GraspCameraOpenNISPAM.xml rename to resources/Spam/Demo/GraspCameraOpenNIDemoRag.xml diff --git a/resources/GraspCameraPointGreySPAM.xml b/resources/Spam/Demo/GraspCameraPointGreyDemoRag.xml similarity index 100% rename from resources/GraspCameraPointGreySPAM.xml rename to resources/Spam/Demo/GraspCameraPointGreyDemoRag.xml diff --git a/resources/GraspCameraRobotDepthSPAM.xml b/resources/Spam/Demo/GraspCameraRobotDepthDemoRag.xml similarity index 100% rename from resources/GraspCameraRobotDepthSPAM.xml rename to resources/Spam/Demo/GraspCameraRobotDepthDemoRag.xml diff --git a/resources/GraspDataContactModelSPAM.xml b/resources/Spam/Demo/GraspDataContactModelDemoRag.xml similarity index 100% rename from resources/GraspDataContactModelSPAM.xml rename to resources/Spam/Demo/GraspDataContactModelDemoRag.xml diff --git a/resources/GraspDataContactQuerySPAM.xml b/resources/Spam/Demo/GraspDataContactQueryDemoRag.xml similarity index 100% rename from resources/GraspDataContactQuerySPAM.xml rename to resources/Spam/Demo/GraspDataContactQueryDemoRag.xml diff --git a/resources/GraspDataImageSPAM.xml b/resources/Spam/Demo/GraspDataImageDemoRag.xml similarity index 100% rename from resources/GraspDataImageSPAM.xml rename to resources/Spam/Demo/GraspDataImageDemoRag.xml diff --git a/resources/GraspDataPointsCurvSPAM.xml b/resources/Spam/Demo/GraspDataPointsCurvDemoRag.xml similarity index 100% rename from resources/GraspDataPointsCurvSPAM.xml rename to resources/Spam/Demo/GraspDataPointsCurvDemoRag.xml diff --git a/resources/GraspDataTrajectorySPAM.xml b/resources/Spam/Demo/GraspDataTrajectoryDemoRag.xml similarity index 100% rename from resources/GraspDataTrajectorySPAM.xml rename to resources/Spam/Demo/GraspDataTrajectoryDemoRag.xml diff --git a/resources/GraspDataVideoSPAM.xml b/resources/Spam/Demo/GraspDataVideoDemoRag.xml similarity index 100% rename from resources/GraspDataVideoSPAM.xml rename to resources/Spam/Demo/GraspDataVideoDemoRag.xml diff --git a/resources/SpamRagPlanner_RobotBoris.xml b/resources/Spam/Demo/SpamDemoR2G_RobotBoris.xml similarity index 100% rename from resources/SpamRagPlanner_RobotBoris.xml rename to resources/Spam/Demo/SpamDemoR2G_RobotBoris.xml diff --git a/resources/SpamPosePlanner_RobotBham.xml b/resources/Spam/PosePlanner/SpamPosePlanner_RobotBham.xml similarity index 100% rename from resources/SpamPosePlanner_RobotBham.xml rename to resources/Spam/PosePlanner/SpamPosePlanner_RobotBham.xml diff --git a/resources/SpamPosePlanner_RobotBoris.xml b/resources/Spam/PosePlanner/SpamPosePlanner_RobotBoris.xml similarity index 100% rename from resources/SpamPosePlanner_RobotBoris.xml rename to resources/Spam/PosePlanner/SpamPosePlanner_RobotBoris.xml diff --git a/resources/SpamPosePlanner_RobotJustin.xml b/resources/Spam/PosePlanner/SpamPosePlanner_RobotJustin.xml similarity index 100% rename from resources/SpamPosePlanner_RobotJustin.xml rename to resources/Spam/PosePlanner/SpamPosePlanner_RobotJustin.xml diff --git a/resources/SpamRagPlanner_RobotBham.xml b/resources/Spam/R2GPlanner/SpamR2GPlanner_RobotBham.xml similarity index 100% rename from resources/SpamRagPlanner_RobotBham.xml rename to resources/Spam/R2GPlanner/SpamR2GPlanner_RobotBham.xml diff --git a/resources/Spam/R2GPlanner/SpamR2GPlanner_RobotBoris.xml b/resources/Spam/R2GPlanner/SpamR2GPlanner_RobotBoris.xml new file mode 100644 index 0000000..f5aebc2 --- /dev/null +++ b/resources/Spam/R2GPlanner/SpamR2GPlanner_RobotBoris.xml @@ -0,0 +1,541 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + --> + + + + + + + + + + + + + + diff --git a/resources/SpamRagPlanner_RobotJustin.xml b/resources/Spam/R2GPlanner/SpamR2GPlanner_RobotJustin.xml similarity index 100% rename from resources/SpamRagPlanner_RobotJustin.xml rename to resources/Spam/R2GPlanner/SpamR2GPlanner_RobotJustin.xml diff --git a/resources/SpamDeviceRobotBhamSim.xml b/resources/SpamDeviceRobotBhamSim.xml deleted file mode 100644 index ec07c83..0000000 --- a/resources/SpamDeviceRobotBhamSim.xml +++ /dev/null @@ -1,165 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/SpamDeviceRobotJustin.xml b/resources/SpamDeviceRobotJustin.xml deleted file mode 100644 index 32ea779..0000000 --- a/resources/SpamDeviceRobotJustin.xml +++ /dev/null @@ -1,192 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/SpamShapePlanner_RobotBham.xml b/resources/SpamShapePlanner_RobotBham.xml deleted file mode 100644 index 61abb9f..0000000 --- a/resources/SpamShapePlanner_RobotBham.xml +++ /dev/null @@ -1,703 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/SpamShapePlanner_RobotJustin.xml b/resources/SpamShapePlanner_RobotJustin.xml deleted file mode 100644 index 7252735..0000000 --- a/resources/SpamShapePlanner_RobotJustin.xml +++ /dev/null @@ -1,428 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - /> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/SpamShapePlanner_RobotUIBK.xml b/resources/SpamShapePlanner_RobotUIBK.xml deleted file mode 100644 index de42fd5..0000000 --- a/resources/SpamShapePlanner_RobotUIBK.xml +++ /dev/null @@ -1,437 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - /> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/Spam/App/PosePlanner/Main.cpp b/src/Spam/App/PosePlanner/Main.cpp new file mode 100644 index 0000000..7a46166 --- /dev/null +++ b/src/Spam/App/PosePlanner/Main.cpp @@ -0,0 +1,15 @@ +/** @file PosePlanner.cpp + * + * @author Claudio Zito + * + * @version 1.0 + * + */ + +#include + +//------------------------------------------------------------------------------ + +int main(int argc, char *argv[]) { + return spam::PosePlanner::Desc().main(argc, argv); +} diff --git a/src/Spam/PosePlanner/PosePlanner.cpp b/src/Spam/App/PosePlanner/PosePlanner.cpp similarity index 98% rename from src/Spam/PosePlanner/PosePlanner.cpp rename to src/Spam/App/PosePlanner/PosePlanner.cpp index 0d352a5..3f9f488 100644 --- a/src/Spam/PosePlanner/PosePlanner.cpp +++ b/src/Spam/App/PosePlanner/PosePlanner.cpp @@ -9,14 +9,13 @@ #include #include #include -#include +#include #include #include #include #include #include -#include #include #include @@ -889,7 +888,7 @@ grasp::data::Item::Map::iterator spam::PosePlanner::estimatePose(Data::Mode mode throw Message(Message::LEVEL_ERROR, "PosePlanner::estimatePose(): Does not find %s.", itemName.c_str()); // retrive point cloud with curvature - data::ItemPointsCurv *pointsCurv = is(ptr->second.get()); + grasp::data::ItemPointsCurv *pointsCurv = is(ptr->second.get()); if (!pointsCurv) throw Message(Message::LEVEL_ERROR, "PosePlanner::estimatePose(): Does not support PointdCurv interface."); grasp::Cloud::PointCurvSeq curvPoints = *pointsCurv->cloud; @@ -980,7 +979,7 @@ grasp::data::Item::Map::iterator spam::PosePlanner::objectCapture(const Data::Mo grasp::Camera* camera = mode != Data::MODE_MODEL ? queryCamera : modelCamera; grasp::ConfigMat34::Seq scanPoseSeq = mode != Data::MODE_MODEL ? queryScanPoseSeq : modelScanPoseSeq; - data::Capture* capture = is(handlerScan); + grasp::data::Capture* capture = is(handlerScan); if (!capture) throw Message(Message::LEVEL_ERROR, "Handler %s does not support Capture interface", objectHandlerScan->getID().c_str()); @@ -1004,7 +1003,7 @@ grasp::data::Item::Map::iterator spam::PosePlanner::objectCapture(const Data::Mo this->gotoPose(*pose++); return ++index < size; }; - typedef std::vector ItemPtrSeq; + typedef std::vector ItemPtrSeq; ItemPtrSeq itemPtrSeq; itemPtrSeq.resize(size); U32 ii = 0; for (bool stop = false; !stop;) { @@ -1012,35 +1011,35 @@ grasp::data::Item::Map::iterator spam::PosePlanner::objectCapture(const Data::Mo RenderBlock renderBlock(*this); { golem::CriticalSectionWrapper cswData(scene.getCS()); - itemPtrSeq[ii] = to(dataCurrentPtr)->itemMap.insert(to(dataCurrentPtr)->itemMap.end(), data::Item::Map::value_type(itemNameRaw, capture->capture(*to(sensorCurrentPtr), [&](const grasp::TimeStamp*) -> bool { return true; }))); + itemPtrSeq[ii] = to(dataCurrentPtr)->itemMap.insert(to(dataCurrentPtr)->itemMap.end(), grasp::data::Item::Map::value_type(itemNameRaw, capture->capture(*to(sensorCurrentPtr), [&](const grasp::TimeStamp*) -> bool { return true; }))); Data::View::setItem(to(dataCurrentPtr)->itemMap, itemPtrSeq[ii++], to(dataCurrentPtr)->getView()); } } // Transform to point curv // generate features - data::Transform* transform = is(handler); + grasp::data::Transform* transform = is(handler); if (!transform) throw Message(Message::LEVEL_ERROR, "Handler %s does not support Transform interface", handler->getID().c_str()); context.write("Transform items to %s\n", handler->getID().c_str()); - data::Item::List list; + grasp::data::Item::List list; for (auto ptr = itemPtrSeq.begin(); ptr != itemPtrSeq.end(); ptr++) list.insert(list.end(), *ptr); - data::Item::Ptr item = transform->transform(list); + grasp::data::Item::Ptr item = transform->transform(list); // item name // readString("Save point Curv as: ", itemName); // insert processed object, remove old one - data::Item::Map::iterator pointCurvPtr; + grasp::data::Item::Map::iterator pointCurvPtr; RenderBlock renderBlock(*this); { golem::CriticalSectionWrapper cswData(getCS()); // remove the raw point clouds to(dataCurrentPtr)->itemMap.erase(itemNameRaw); to(dataCurrentPtr)->itemMap.erase(itemName); - pointCurvPtr = to(dataCurrentPtr)->itemMap.insert(to(dataCurrentPtr)->itemMap.end(), data::Item::Map::value_type(itemName, item)); + pointCurvPtr = to(dataCurrentPtr)->itemMap.insert(to(dataCurrentPtr)->itemMap.end(), grasp::data::Item::Map::value_type(itemName, item)); Data::View::setItem(to(dataCurrentPtr)->itemMap, pointCurvPtr, to(dataCurrentPtr)->getView()); } @@ -1052,19 +1051,19 @@ grasp::data::Item::Map::iterator spam::PosePlanner::objectProcess(const Data::Mo const std::string itemName = mode == Data::MODE_DEFAULT ? objectItem : mode != Data::MODE_MODEL ? queryItem : modelItem; grasp::data::Handler* handler = mode == Data::MODE_DEFAULT ? objectHandler : mode != Data::MODE_MODEL ? queryHandler : modelHandler; // generate features - data::Transform* transform = is(handler); + grasp::data::Transform* transform = is(handler); if (!transform) throw Message(Message::LEVEL_ERROR, "Handler %s does not support Transform interface", handler->getID().c_str()); - data::Item::List list; + grasp::data::Item::List list; list.insert(list.end(), ptr); - data::Item::Ptr item = transform->transform(list); + grasp::data::Item::Ptr item = transform->transform(list); // insert processed object, remove old one RenderBlock renderBlock(*this); golem::CriticalSectionWrapper cswData(getCS()); to(dataCurrentPtr)->itemMap.erase(itemName); - ptr = to(dataCurrentPtr)->itemMap.insert(to(dataCurrentPtr)->itemMap.end(), data::Item::Map::value_type(itemName, item)); + ptr = to(dataCurrentPtr)->itemMap.insert(to(dataCurrentPtr)->itemMap.end(), grasp::data::Item::Map::value_type(itemName, item)); Data::View::setItem(to(dataCurrentPtr)->itemMap, ptr, to(dataCurrentPtr)->getView()); return ptr; } @@ -1389,12 +1388,3 @@ template <> void golem::Stream::write(const spam::PosePlanner::Data &trialData) } //------------------------------------------------------------------------------ - -#ifdef _SPAM_POSE_MAIN_ -int main(int argc, char *argv[]) { - return spam::PosePlanner::Desc().main(argc, argv); -} -//int main(int argc, char *argv[]) { -// return spam::PosePlannerApp().main(argc, argv); -//} -#endif // _SPAM_POSEPLANNER_MAIN_ \ No newline at end of file diff --git a/src/Spam/App/R2GPlanner/Main.cpp b/src/Spam/App/R2GPlanner/Main.cpp new file mode 100644 index 0000000..c8b54e3 --- /dev/null +++ b/src/Spam/App/R2GPlanner/Main.cpp @@ -0,0 +1,15 @@ +/** @file RAGPlanner.cpp + * + * @author Claudio Zito (The University Of Birmingham) + * + * @version 1.0 + * + */ + +#include + +//------------------------------------------------------------------------------ + +int main(int argc, char *argv[]) { + return spam::R2GPlanner::Desc().main(argc, argv); +} \ No newline at end of file diff --git a/src/Spam/RagPlanner/RagPlanner.cpp b/src/Spam/App/R2GPlanner/R2GPlanner.cpp similarity index 97% rename from src/Spam/RagPlanner/RagPlanner.cpp rename to src/Spam/App/R2GPlanner/R2GPlanner.cpp index 1e9b99d..d869038 100644 --- a/src/Spam/RagPlanner/RagPlanner.cpp +++ b/src/Spam/App/R2GPlanner/R2GPlanner.cpp @@ -10,7 +10,7 @@ #include #include //#include -#include +#include #include #ifdef WIN32 @@ -34,16 +34,16 @@ using namespace spam; //------------------------------------------------------------------------------ -grasp::data::Data::Ptr spam::RagPlanner::Data::Desc::create(golem::Context &context) const { - grasp::data::Data::Ptr data(new RagPlanner::Data(context)); - static_cast(data.get())->create(*this); +grasp::data::Data::Ptr spam::R2GPlanner::Data::Desc::create(golem::Context &context) const { + grasp::data::Data::Ptr data(new R2GPlanner::Data(context)); + static_cast(data.get())->create(*this); return data; } -spam::RagPlanner::Data::Data(golem::Context &context) : PosePlanner::Data(context), owner(nullptr) { +spam::R2GPlanner::Data::Data(golem::Context &context) : PosePlanner::Data(context), owner(nullptr) { } -void spam::RagPlanner::Data::create(const Desc& desc) { +void spam::R2GPlanner::Data::create(const Desc& desc) { PosePlanner::Data::create(desc); triggered = 0; @@ -51,21 +51,21 @@ void spam::RagPlanner::Data::create(const Desc& desc) { release = false; } -void spam::RagPlanner::Data::createRender() { +void spam::R2GPlanner::Data::createRender() { PosePlanner::Data::createRender(); } -void spam::RagPlanner::Data::load(const std::string& prefix, const golem::XMLContext* xmlcontext, const grasp::data::Handler::Map& handlerMap) { +void spam::R2GPlanner::Data::load(const std::string& prefix, const golem::XMLContext* xmlcontext, const grasp::data::Handler::Map& handlerMap) { PosePlanner::Data::load(prefix, xmlcontext, handlerMap); } -void spam::RagPlanner::Data::save(const std::string& prefix, golem::XMLContext* xmlcontext) const { +void spam::R2GPlanner::Data::save(const std::string& prefix, golem::XMLContext* xmlcontext) const { PosePlanner::Data::save(prefix, xmlcontext); } //------------------------------------------------------------------------------ -void RagPlanner::Desc::load(golem::Context& context, const golem::XMLContext* xmlcontext) { +void R2GPlanner::Desc::load(golem::Context& context, const golem::XMLContext* xmlcontext) { PosePlanner::Desc::load(context, xmlcontext); xmlcontext = xmlcontext->getContextFirst("rag_planner"); @@ -90,15 +90,15 @@ void RagPlanner::Desc::load(golem::Context& context, const golem::XMLContext* xm //------------------------------------------------------------------------------ -RagPlanner::RagPlanner(Scene &scene) : PosePlanner(scene) { +R2GPlanner::R2GPlanner(Scene &scene) : PosePlanner(scene) { } -RagPlanner::~RagPlanner() { +R2GPlanner::~R2GPlanner() { if (dataFTRaw.is_open()) dataFTRaw.close(); if (dataFTRaw.is_open()) dataFTFiltered.close(); } -bool RagPlanner::create(const Desc& desc) { +bool R2GPlanner::create(const Desc& desc) { PosePlanner::create(desc); // throws if (desc.distance.size() < (size_t)info.getJoints().size()) @@ -236,7 +236,7 @@ bool RagPlanner::create(const Desc& desc) { /*ArmHandForce **/armHandForce = dynamic_cast(&*activectrlMap.find("ArmHandForce+ArmHandForce")->second); if (!armHandForce) - throw Message(Message::LEVEL_ERROR, "RagPlanner::create(): armHandForce is invalid"); + throw Message(Message::LEVEL_ERROR, "R2GPlanner::create(): armHandForce is invalid"); armMode = armHandForce->getArmCtrl()->getMode(); handMode = armHandForce->getHandCtrl()->getMode(); context.write("Active control mode [arm/hand]: %s/%s\n", ActiveCtrlForce::ModeName[armMode], ActiveCtrlForce::ModeName[handMode]); @@ -573,8 +573,8 @@ bool RagPlanner::create(const Desc& desc) { objectPointCloudPtr.reset(new grasp::Cloud::PointSeq(grasp::to(dataCurrentPtr)->simulateObjectPose)); } - data::Item::Map::const_iterator item = to(dataCurrentPtr)->getItem(true); - data::Trajectory* trajectory = is(item->second.get()); + grasp::data::Item::Map::const_iterator item = to(dataCurrentPtr)->getItem(true); + grasp::data::Trajectory* trajectory = is(item->second.get()); if (!trajectory) throw Cancel("Error: no trajectory selected."); // play @@ -716,7 +716,7 @@ bool RagPlanner::create(const Desc& desc) { grasp::to(dataCurrentPtr)->actionType = action::NONE_ACTION; grasp::to(dataCurrentPtr)->replanning = false; grasp::data::Item::Map::iterator modelContactPtr, modelPtr, queryGraspPtr, queryGraspTrjPtr; - data::Item::Ptr queryGraspTrj; + grasp::data::Item::Ptr queryGraspTrj; U32 modelViews = modelScanPoseSeq.size(), queryViews = queryScanPoseSeq.size(), trials = 5; const U32 maxFailures = 2, maxIterations = 5; // collects results as string @@ -827,27 +827,27 @@ bool RagPlanner::create(const Desc& desc) { // create a grasp // retrieve model contact if (modelGraspItemSeq.empty()) - throw Message(Message::LEVEL_ERROR, "RagPlanner::demo(): No model grasp have been created."); + throw Message(Message::LEVEL_ERROR, "R2GPlanner::demo(): No model grasp have been created."); // generate features - data::Transform* transform = is(queryGraspHandler); + grasp::data::Transform* transform = is(queryGraspHandler); if (!transform) throw Message(Message::LEVEL_ERROR, "Handler %s does not support Transform interface", queryGraspHandler->getID().c_str()); - data::Item::List list; + grasp::data::Item::List list; for (auto i = modelGraspItemSeq.begin(); i != modelGraspItemSeq.end(); ++i) { - data::Item::Map::iterator ptr = to(dataCurrentPtr)->itemMap.find(*i); + grasp::data::Item::Map::iterator ptr = to(dataCurrentPtr)->itemMap.find(*i); if (ptr == to(dataCurrentPtr)->itemMap.end()) - throw Message(Message::LEVEL_ERROR, "RagPlanner::estimatePose(): Does not find %s.", (*i).c_str()); + throw Message(Message::LEVEL_ERROR, "R2GPlanner::estimatePose(): Does not find %s.", (*i).c_str()); list.insert(list.end(), ptr); } // retrieve model point cloud modelPtr = to(dataCurrentPtr)->itemMap.find(modelItem); if (modelPtr == to(dataCurrentPtr)->itemMap.end()) - throw Message(Message::LEVEL_ERROR, "RagPlanner::estimatePose(): Does not find %s.", modelItem.c_str()); + throw Message(Message::LEVEL_ERROR, "R2GPlanner::estimatePose(): Does not find %s.", modelItem.c_str()); list.insert(list.end(), modelPtr); // grasp on model - data::Item::Ptr queryGraspItemPtr = transform->transform(list); + grasp::data::Item::Ptr queryGraspItemPtr = transform->transform(list); // insert processed object, remove old one //grasp::data::Item::Map::iterator queryGraspPtr; @@ -866,7 +866,7 @@ bool RagPlanner::create(const Desc& desc) { // CREATE A GRASP TRAJECTORY //--------------------------------------------------------------------// context.write("Convert [%s]: handler %s, input %s...\n", queryItemTrj.c_str(), queryHandlerTrj->getID().c_str(), queryGraspPtr->first.c_str()); - data::Convert* convert = is(queryGraspPtr); + grasp::data::Convert* convert = is(queryGraspPtr); if (!convert) throw Message(Message::LEVEL_ERROR, "Handler %s does not support Convert interface", queryHandlerTrj->getID().c_str()); @@ -878,7 +878,7 @@ bool RagPlanner::create(const Desc& desc) { { golem::CriticalSectionWrapper cswData(getCS()); to(dataCurrentPtr)->itemMap.erase(queryItemTrj); - queryGraspTrjPtr = to(dataCurrentPtr)->itemMap.insert(to(dataCurrentPtr)->itemMap.end(), data::Item::Map::value_type(queryItemTrj, queryGraspTrj)); + queryGraspTrjPtr = to(dataCurrentPtr)->itemMap.insert(to(dataCurrentPtr)->itemMap.end(), grasp::data::Item::Map::value_type(queryItemTrj, queryGraspTrj)); Data::View::setItem(to(dataCurrentPtr)->itemMap, queryGraspTrjPtr, to(dataCurrentPtr)->getView()); } // to(dataCurrentPtr)->createRender(); @@ -1316,7 +1316,7 @@ bool RagPlanner::create(const Desc& desc) { //------------------------------------------------------------------------------ -void RagPlanner::render() const { +void R2GPlanner::render() const { PosePlanner::render(); // handRenderer.reset(); { @@ -1325,7 +1325,7 @@ void RagPlanner::render() const { } } -void RagPlanner::renderHand(const golem::Controller::State &state, const Bounds::Seq &bounds, bool clear) { +void R2GPlanner::renderHand(const golem::Controller::State &state, const Bounds::Seq &bounds, bool clear) { return; { golem::CriticalSectionWrapper csw(getCS()); @@ -1342,7 +1342,7 @@ void RagPlanner::renderHand(const golem::Controller::State &state, const Bounds: //------------------------------------------------------------------------------ -void RagPlanner::findTarget(const golem::Mat34 &trn, const golem::Controller::State &target, golem::Controller::State &cend, const bool lifting) { +void R2GPlanner::findTarget(const golem::Mat34 &trn, const golem::Controller::State &target, golem::Controller::State &cend, const bool lifting) { // arm chain and joints pointers const golem::Chainspace::Index armChain = armInfo.getChains().begin(); const golem::Configspace::Range armJoints = armInfo.getJoints(); @@ -1375,7 +1375,7 @@ void RagPlanner::findTarget(const golem::Mat34 &trn, const golem::Controller::St context.write("Robot::findTarget(): Pose error: lin=%.9f, ang=%.9f\n", err.lin, err.ang); } -void RagPlanner::findTarget(const golem::Mat34& queryTrn, const golem::Mat34& modelFrame, const golem::Controller::State& target, golem::Controller::State& cend, const bool lifting) { +void R2GPlanner::findTarget(const golem::Mat34& queryTrn, const golem::Mat34& modelFrame, const golem::Controller::State& target, golem::Controller::State& cend, const bool lifting) { // arm chain and joints pointers const golem::Chainspace::Index armChain = armInfo.getChains().begin(); const golem::Configspace::Range armJoints = armInfo.getJoints(); @@ -1416,7 +1416,7 @@ void RagPlanner::findTarget(const golem::Mat34& queryTrn, const golem::Mat34& mo //------------------------------------------------------------------------------ -void RagPlanner::createTrajectory(const golem::Controller::State& begin, const golem::Controller::State* pcend, const golem::Mat34* pwend, golem::SecTmReal t, const golem::Controller::State::Seq& waypoints, golem::Controller::State::Seq& trajectory) { +void R2GPlanner::createTrajectory(const golem::Controller::State& begin, const golem::Controller::State* pcend, const golem::Mat34* pwend, golem::SecTmReal t, const golem::Controller::State::Seq& waypoints, golem::Controller::State::Seq& trajectory) { if (!pcend && !pwend) throw Message(Message::LEVEL_ERROR, "Robot::findTrajectory(): no target specified"); @@ -1462,7 +1462,7 @@ void RagPlanner::createTrajectory(const golem::Controller::State& begin, const g throw Message(Message::LEVEL_ERROR, "Robot::findTrajectory(): unable to find trajectory"); } -grasp::RBDist RagPlanner::trnTrajectory(const golem::Mat34& actionFrame, const golem::Mat34& modelFrame, const golem::Mat34& trn, golem::Controller::State::Seq::const_iterator begin, golem::Controller::State::Seq::const_iterator end, golem::Controller::State::Seq& trajectory) { +grasp::RBDist R2GPlanner::trnTrajectory(const golem::Mat34& actionFrame, const golem::Mat34& modelFrame, const golem::Mat34& trn, golem::Controller::State::Seq::const_iterator begin, golem::Controller::State::Seq::const_iterator end, golem::Controller::State::Seq& trajectory) { if (begin == end) throw Message(Message::LEVEL_ERROR, "Robot::createTrajectory(2): Empty input trajectory"); // arm chain and joints pointers @@ -1529,7 +1529,7 @@ grasp::RBDist RagPlanner::trnTrajectory(const golem::Mat34& actionFrame, const g return err; } -grasp::RBDist RagPlanner::findTrnTrajectory(const golem::Mat34& trn, const golem::Controller::State& startPose, golem::Controller::State::Seq::const_iterator begin, golem::Controller::State::Seq::const_iterator end, golem::Controller::State::Seq& trajectory) { +grasp::RBDist R2GPlanner::findTrnTrajectory(const golem::Mat34& trn, const golem::Controller::State& startPose, golem::Controller::State::Seq::const_iterator begin, golem::Controller::State::Seq::const_iterator end, golem::Controller::State::Seq& trajectory) { if (begin == end) throw Message(Message::LEVEL_ERROR, "Robot::transformTrajectory(): Empty input trajectory"); // arm chain and joints pointers @@ -1591,24 +1591,24 @@ grasp::RBDist RagPlanner::findTrnTrajectory(const golem::Mat34& trn, const golem //------------------------------------------------------------------------------ -Real RagPlanner::distConfigspaceCoord(const ConfigspaceCoord& prev, const ConfigspaceCoord& next) const { +Real R2GPlanner::distConfigspaceCoord(const ConfigspaceCoord& prev, const ConfigspaceCoord& next) const { Real dist = REAL_ZERO; for (Configspace::Index i = info.getJoints().begin(); i < info.getJoints().end(); ++i) dist += distance[i] * Math::sqr(prev[i] - next[i]); return Math::sqrt(dist); } -Real RagPlanner::distCoord(Real prev, Real next) const { +Real R2GPlanner::distCoord(Real prev, Real next) const { return Math::abs(prev - next); } -bool RagPlanner::distCoordEnabled(const Configspace::Index& index) const { +bool R2GPlanner::distCoordEnabled(const Configspace::Index& index) const { return true; } //------------------------------------------------------------------------------ -void RagPlanner::profile(golem::SecTmReal duration, const golem::Controller::State::Seq& inp, golem::Controller::State::Seq& out, const bool silent) const { +void R2GPlanner::profile(golem::SecTmReal duration, const golem::Controller::State::Seq& inp, golem::Controller::State::Seq& out, const bool silent) const { if (inp.size() < 2) throw Message(Message::LEVEL_ERROR, "Player::profile(): At least two waypoints required"); @@ -1629,7 +1629,7 @@ void RagPlanner::profile(golem::SecTmReal duration, const golem::Controller::Sta pProfile->profile(out, ptr, tmpend); } -void RagPlanner::perform(const std::string& data, const std::string& item, const golem::Controller::State::Seq& trajectory, bool testTrajectory) { +void R2GPlanner::perform(const std::string& data, const std::string& item, const golem::Controller::State::Seq& trajectory, bool testTrajectory) { if (trajectory.size() < 2) throw Message(Message::LEVEL_ERROR, "Player::perform(): At least two waypoints required"); @@ -1846,7 +1846,7 @@ void RagPlanner::perform(const std::string& data, const std::string& item, const //------------------------------------------------------------------------------ -bool RagPlanner::execute(data::Data::Map::iterator dataPtr, grasp::Waypoint::Seq& trajectory) { +bool R2GPlanner::execute(data::Data::Map::iterator dataPtr, grasp::Waypoint::Seq& trajectory) { const golem::Chainspace::Index armChain = armInfo.getChains().begin(); bool silent = to(dataPtr)->actionType != action::NONE_ACTION; // context.debug("execute(): silen=%s, actionType=%s\n", silent ? "TRUE" : "FALSE", actionToString(grasp::to(dataPtr)->actionType)); @@ -2287,7 +2287,7 @@ bool RagPlanner::execute(data::Data::Map::iterator dataPtr, grasp::Waypoint::Seq //------------------------------------------------------------------------------ -//grasp::Cloud::PointSeqMap::iterator RagPlanner::getTrnPoints(Data::Map::iterator dataPtr, const Mat34 &trn) { +//grasp::Cloud::PointSeqMap::iterator R2GPlanner::getTrnPoints(Data::Map::iterator dataPtr, const Mat34 &trn) { // grasp::Cloud::PointSeqMap::iterator points; // if (!grasp::to(dataPtr)->ptrPoints || (points = grasp::to(dataPtr)->getPoints(grasp::to(dataPtr)->points)) == grasp::to(dataPtr)->points.end()) // throw Message(Message::LEVEL_NOTICE, "Director::getPoints(): No points are selected!"); @@ -2300,7 +2300,7 @@ bool RagPlanner::execute(data::Data::Map::iterator dataPtr, grasp::Waypoint::Seq // return points; //} // -//grasp::Cloud::RawPointSeqMultiMap::iterator RagPlanner::getTrnRawPoints(Data::Map::iterator dataPtr, const Mat34 &trn) { +//grasp::Cloud::RawPointSeqMultiMap::iterator R2GPlanner::getTrnRawPoints(Data::Map::iterator dataPtr, const Mat34 &trn) { // grasp::Cloud::RawPointSeqMultiMap::iterator points; // if ((points = grasp::to(dataPtr)->getPoints(grasp::to(dataPtr)->pointsRaw)) == grasp::to(dataPtr)->pointsRaw.end()) // throw Message(Message::LEVEL_NOTICE, "Director::getPoints(): No points are selected!"); @@ -2316,7 +2316,7 @@ bool RagPlanner::execute(data::Data::Map::iterator dataPtr, grasp::Waypoint::Seq //------------------------------------------------------------------------------ -Real RagPlanner::simContacts(const golem::Bounds::Seq::const_iterator &begin, const golem::Bounds::Seq::const_iterator &end, const golem::Mat34 pose) { +Real R2GPlanner::simContacts(const golem::Bounds::Seq::const_iterator &begin, const golem::Bounds::Seq::const_iterator &end, const golem::Mat34 pose) { // if the point cloud is empty simContacts return the default behavior of force reader. //if (objectPointCloudPtr->empty()) // return REAL_ZERO; @@ -2345,7 +2345,7 @@ Real RagPlanner::simContacts(const golem::Bounds::Seq::const_iterator &begin, co //------------------------------------------------------------------------------ -//void RagPlanner::renderData(Data::Map::const_iterator dataPtr) { +//void R2GPlanner::renderData(Data::Map::const_iterator dataPtr) { // ShapePlanner::renderData(dataPtr); // { // golem::CriticalSectionWrapper csw(csDataRenderer); @@ -2358,7 +2358,7 @@ Real RagPlanner::simContacts(const golem::Bounds::Seq::const_iterator &begin, co // } //} // -//void RagPlanner::renderContacts() { +//void R2GPlanner::renderContacts() { // context.write("render contacts...\n"); // GenWorkspaceChainState gwcs; // robot->getController()->chainForwardTransform(robot->recvState().config.cpos, gwcs.wpos); @@ -2382,19 +2382,19 @@ Real RagPlanner::simContacts(const golem::Bounds::Seq::const_iterator &begin, co // context.write("Done.\n"); //} // -//void RagPlanner::renderPose(const Mat34 &pose) { +//void R2GPlanner::renderPose(const Mat34 &pose) { //// testPose.reset(); // testPose.addAxes(pose, featureFrameSize*10); //} // -//void RagPlanner::renderUpdate(const golem::Mat34 &pose, const grasp::RBPose::Sample::Seq &samples) { +//void R2GPlanner::renderUpdate(const golem::Mat34 &pose, const grasp::RBPose::Sample::Seq &samples) { // testUpdate.reset(); // renderPose(pose); // for (grasp::RBPose::Sample::Seq::const_iterator i = samples.begin(); i != samples.end(); ++i) // testUpdate.addAxes(Mat34(i->q, i->p), featureFrameSize*i->weight*10); //} // -//void RagPlanner::renderHand(const golem::Controller::State &state, const Bounds::Seq &bounds, bool clear) { +//void R2GPlanner::renderHand(const golem::Controller::State &state, const Bounds::Seq &bounds, bool clear) { // { // golem::CriticalSectionWrapper csw(csDataRenderer); // debugRenderer.reset(); @@ -2449,7 +2449,7 @@ Real RagPlanner::simContacts(const golem::Bounds::Seq::const_iterator &begin, co //// } //} // -//void RagPlanner::renderWaypoints(const Bounds::Seq &bounds, bool clear) { +//void R2GPlanner::renderWaypoints(const Bounds::Seq &bounds, bool clear) { // { // golem::CriticalSectionWrapper csw(csDataRenderer); // debugRenderer.reset(); @@ -2469,10 +2469,10 @@ Real RagPlanner::simContacts(const golem::Bounds::Seq::const_iterator &begin, co //------------------------------------------------------------------------------ -void RagPlanner::updateAndResample(Data::Map::iterator dataPtr) { +void R2GPlanner::updateAndResample(Data::Map::iterator dataPtr) { if (!pBelief.get() || grasp::to(dataPtr)->queryPoints.empty()) return; - context.debug("RagPlanner::updateAndResample(): %d triggered guards:\n", /*grasp::to(dataPtr)->*/triggeredGuards.size()); + context.debug("R2GPlanner::updateAndResample(): %d triggered guards:\n", /*grasp::to(dataPtr)->*/triggeredGuards.size()); // update samples' weights golem::Real norm = golem::REAL_ZERO, c = golem::REAL_ZERO; @@ -2544,10 +2544,4 @@ void RagPlanner::updateAndResample(Data::Map::iterator dataPtr) { // std::cout << "spam:update&resample 15\n"; } -//------------------------------------------------------------------------------ - -#ifdef _SPAM_RAG_MAIN_ -int main(int argc, char *argv[]) { - return spam::RagPlanner::Desc().main(argc, argv); -} -#endif // _SPAM_RAG_MAIN_ \ No newline at end of file +//------------------------------------------------------------------------------ \ No newline at end of file diff --git a/src/Spam/Spam/Spam.cpp b/src/Spam/Core/Collision.cpp similarity index 86% rename from src/Spam/Spam/Spam.cpp rename to src/Spam/Core/Collision.cpp index 6aa073f..d8a3f9e 100644 --- a/src/Spam/Spam/Spam.cpp +++ b/src/Spam/Core/Collision.cpp @@ -5,23 +5,7 @@ * @version 1.0 * */ -#include -//#include -#include -#include - -//#ifdef WIN32 -// #pragma warning (push) -// #pragma warning (disable : 4244) -// #pragma warning (disable : 4996) -//#endif -//#include -//#include -//#ifdef WIN32 -// #pragma warning (pop) -//#endif -// -//#include +#include #ifdef WIN32 #pragma warning (push) @@ -39,7 +23,6 @@ #pragma warning (pop) #endif - //------------------------------------------------------------------------------ using namespace golem; @@ -92,62 +75,6 @@ static Vec3 getRelativeFrame(const golem::Mat34 &reference, const golem::Vec3 &p return v;// .normalise(); } -static std::string getGuardName(const U32 finger) { - if (finger == HandChains::UNKNOWN) return "UNKNOWN"; - if (finger == HandChains::THUMB) return "THUMB"; - if (finger == HandChains::INDEX) return "INDEX"; - if (finger == HandChains::MIDDLE) return "MIDDLE"; - if (finger == HandChains::RING) return "RING"; - if (finger == HandChains::PINKY) return "PINKY"; - return ""; -} - -static HandChains bound2Chain(const U32 finger) { - if (7 < finger && finger < 10) - return HandChains::THUMB; - if (11 < finger && finger < 14) - return HandChains::INDEX; - if (15 < finger && finger < 18) - return HandChains::MIDDLE; - if (19 < finger && finger < 22) - return HandChains::RING; - if (22 < finger && finger < 25) - return HandChains::PINKY; - return HandChains::UNKNOWN; - -} -//------------------------------------------------------------------------------ - -FTGuard::FTGuard(const grasp::Manipulator& manipulator) { - // arm joint indices [0, 6] - armIdx = manipulator.getArmInfo().getJoints().end()-1; - handChains = manipulator.getHandInfo().getChains().size(); - fingerJoints = U32(manipulator.getHandInfo().getJoints().size() / handChains); -// manipulator.getContext().write("FTGuard armJoints=%u, handChains=%u\n", armIdx, handChains); -} - -std::string FTGuard::str() const { - std::string ss = getGuardName(getHandChain()) + " joint=" + boost::lexical_cast(getHandJoint()) + " measured_force=" + boost::lexical_cast(force) + (type == FTGUARD_ABS ? " |> " : type == FTGUARD_LESSTHAN ? " < " : " > ") + boost::lexical_cast(threshold); - printf("%s\n", ss.c_str()); - return ss; -} - -void FTGuard::create(golem::Configspace::Index& i) { - jointIdx = i; - const size_t handIdx = i - armIdx; - threshold = 0.2; - force = REAL_ZERO; - type = FTGUARD_ABS; -// printf("FTGuard(armJoints=%u, handChains=%u, fingerJoints=%u) chain=%u, chain joint=%u, joint=%u\n", armIdx, handChains, fingerJoints, (handIdx / fingerJoints) + 1, handIdx % fingerJoints, i); -} - -void XMLData(FTGuard &val, XMLContext* xmlcontext, bool create) { - golem::XMLData("name", val.name, xmlcontext, create); -// golem::XMLData("joint", val.joint, xmlcontext, create); -// golem::XMLData("type", val.type, xmlcontext, create); -// golem::XMLData("value", val.value, xmlcontext, create); -} - //------------------------------------------------------------------------------ Collision::Collision(const grasp::Manipulator& manipulator, const Desc& desc) : manipulator(manipulator), desc(desc) { @@ -178,18 +105,24 @@ void Collision::create(golem::Rand& rand, const grasp::Cloud::PointSeq& points) std::random_shuffle(this->points.begin(), this->points.end(), rand); - //if (desc.kdtree) { flann::SearchParams search; - flann::KDTreeSingleIndexParams index; -// desc.nnSearchDesc.searchKDTrees = 4; - search.checks = 32; - search.max_neighbors = 10; - desc.nnSearchDesc.getKDTreeSingleIndex(search, index); - typedef grasp::KDTree KDTree; - -// nnSearch.reset(new KDTree(search, index, (Real*)&this->points.front(), this->points.size(), sizeof(golem::Real), Collision::FlannDist::LIN_N, Collision::FlannDist())); - nnSearch.reset(new KDTree(search, index, this->points, Feature::N, Feature::FlannDist())); - //} + flann::KDTreeSingleIndexParams index; + search.checks = 32; + search.max_neighbors = 10; + desc.nnSearchDesc.getKDTreeSingleIndex(search, index); + + typedef grasp::KDTree KDTree; + nnSearch.reset(new KDTree(search, index, this->points, Feature::N, Feature::FlannDist())); + + //if (desc.kdtree) { + // flann::SearchParams search; + // flann::KDTreeSingleIndexParams index; + // search.checks = 32; + // search.max_neighbors = 10; + // desc.nnSearchDesc.getKDTreeSingleIndex(search, index); + // typedef grasp::KDTree KDTree; + // nnSearch.reset(new KDTree(search, index, this->points, Feature::N, Feature::FlannDist())); + //} //else { // std::random_shuffle(this->points.begin(), this->points.end(), rand); //} @@ -1213,101 +1146,4 @@ void spam::XMLData(spam::Collision::Desc& val, golem::XMLContext* xmlcontext, bo spam::XMLData(val.ftContact, xmlcontext->getContextFirst("ft_contact"), create); } catch (const golem::MsgXMLParser&) { } -} - -//------------------------------------------------------------------------------ - -void Hypothesis::BoundsAppearance::draw(const golem::Bounds::Seq& bounds, golem::DebugRenderer& renderer) const { - if (showSolid) { - renderer.setColour(solidColour); - renderer.addSolid(bounds.begin(), bounds.end()); - } - if (showWire) { - renderer.setColour(wireColour); - renderer.setLineWidth(wireWidth); - renderer.addWire(bounds.begin(), bounds.end()); - } -} - -Bounds::Seq Hypothesis::bounds() { - Bounds::Seq bounds; - bounds.push_back(boundsDesc.create()); - return bounds; -} - -//------------------------------------------------------------------------------ - -Hypothesis::Hypothesis(const grasp::Manipulator& manipulator, const Desc& desc) : manipulator(manipulator), desc(desc) { - if (!desc.isValid()) - throw Message(Message::LEVEL_CRIT, "Hypothesis(): invalid description"); - collisionPtr = desc.collisionDescPtr->create(manipulator); -} - -//------------------------------------------------------------------------------ - -void Hypothesis::create(const golem::U32 idx, const golem::Mat34& trn, const grasp::RBPose::Sample& s, golem::Rand& rand, const grasp::Cloud::PointSeq& points) { - collisionPtr->create(rand, points); - index = idx; - modelFrame = trn; - sample = s; - Real x = REAL_ZERO, y = REAL_ZERO, z = REAL_ZERO; - Vec3 sampleFrame = toRBPoseSampleGF().p; - for (grasp::Cloud::PointSeq::const_iterator i = points.begin(); i != points.end(); ++i) { - this->points.push_back(*i); - // calculate max x, y, z values for bounding box - if (x < Math::abs(sampleFrame.x - (float)i->x)) - x = Math::abs(sampleFrame.x - (float)i->x); - if (y < Math::abs(sampleFrame.y - (float)i->y)) - y = Math::abs(sampleFrame.y - (float)i->y); - if (z < Math::abs(sampleFrame.z - (float)i->z)) - z = Math::abs(sampleFrame.z - (float)i->z); - } -// printf("Bounding box dim x=%f, y=%f, z=%f\n", x, y, z); - // set dimension of the bounding box - boundsDesc.dimensions = Vec3(x, y, z); - boundsDesc.pose.p = toRBPoseSampleGF().p; -// printf("Bounding box dim x=%f, y=%f, z=%f, pose=<%f, %f, %f>\n", boundsDesc.dimensions.x, boundsDesc.dimensions.y, boundsDesc.dimensions.z, boundsDesc.pose.p.x, boundsDesc.pose.p.y, boundsDesc.pose.p.z); -} - -void Hypothesis::draw(DebugRenderer &renderer) const { - printf("Belief::Hypothesis::draw(showFrame=%s, showPoints=%s)\n", appearance.showFrames ? "ON" : "OFF", appearance.showPoints ? "ON" : "OFF"); - if (appearance.showFrames || true) - renderer.addAxes(sample.toMat34() * modelFrame, appearance.frameSize); - - size_t t = 0; - if (appearance.showPoints || true) { - for (grasp::Cloud::PointSeq::const_iterator i = points.begin(); i != points.end(); ++i) { - grasp::Cloud::Point point = *i; - //if (++t < 10) printf("Belief::Hypothesis point %d <%.4f %.4f %.4f>\n", t, point.x, point.y, point.z); - grasp::Cloud::setColour(/*appearance.colour*/RGBA::RED, point); - renderer.addPoint(grasp::Cloud::getPoint(point)); - } - } -} - -//------------------------------------------------------------------------------ - -std::string Hypothesis::str() const { - std::stringstream ss; - const grasp::RBPose::Sample pose = toRBPoseSampleGF(); - ss << pose.p.x << "\t" << pose.p.y << "\t" << pose.p.z << "\t" << pose.q.w << "\t" << pose.q.x << "\t" << pose.q.y << "\t" << pose.q.z; - return ss.str(); -} - -//------------------------------------------------------------------------------ - -void Hypothesis::draw(const Collision::Waypoint &waypoint, const grasp::Manipulator::Config& config, golem::DebugRenderer& renderer) const { - collisionPtr->draw(waypoint, config, renderer); -} - -void Hypothesis::draw(golem::DebugRenderer& renderer, const golem::Rand& rand, const grasp::Manipulator::Config& config) const { - collisionPtr->draw(renderer, rand, config, desc.collisionDescPtr->flannDesc); -} - -//------------------------------------------------------------------------------ - -void spam::XMLData(spam::Hypothesis::Desc& val, golem::XMLContext* xmlcontext, bool create) { - spam::XMLData(*val.collisionDescPtr, xmlcontext->getContextFirst("collision"), create); -} - -//------------------------------------------------------------------------------ \ No newline at end of file +} \ No newline at end of file diff --git a/src/Spam/Core/JContact.cpp b/src/Spam/Core/JContact.cpp new file mode 100644 index 0000000..384cf48 --- /dev/null +++ b/src/Spam/Core/JContact.cpp @@ -0,0 +1,74 @@ +/** @file Data.cpp + * + * @author Claudio Zito (The University Of Birmingham) + * + * @version 1.0 + * + */ +#include +#include + +//------------------------------------------------------------------------------ + +using namespace golem; +using namespace spam; + +//------------------------------------------------------------------------------ + +static std::string getGuardName(const U32 finger) { + if (finger == HandChains::UNKNOWN) return "UNKNOWN"; + if (finger == HandChains::THUMB) return "THUMB"; + if (finger == HandChains::INDEX) return "INDEX"; + if (finger == HandChains::MIDDLE) return "MIDDLE"; + if (finger == HandChains::RING) return "RING"; + if (finger == HandChains::PINKY) return "PINKY"; + return ""; +} + +static HandChains bound2Chain(const U32 finger) { + if (7 < finger && finger < 10) + return HandChains::THUMB; + if (11 < finger && finger < 14) + return HandChains::INDEX; + if (15 < finger && finger < 18) + return HandChains::MIDDLE; + if (19 < finger && finger < 22) + return HandChains::RING; + if (22 < finger && finger < 25) + return HandChains::PINKY; + return HandChains::UNKNOWN; + +} + +//------------------------------------------------------------------------------ + +FTGuard::FTGuard(const grasp::Manipulator& manipulator) { + // arm joint indices [0, 6] + armIdx = manipulator.getArmInfo().getJoints().end()-1; + handChains = (U32)manipulator.getHandInfo().getChains().size(); + fingerJoints = U32(manipulator.getHandInfo().getJoints().size() / handChains); +// manipulator.getContext().write("FTGuard armJoints=%u, handChains=%u\n", armIdx, handChains); +} + +std::string FTGuard::str() const { + std::string ss = getGuardName(getHandChain()) + " joint=" + boost::lexical_cast(getHandJoint()) + " measured_force=" + boost::lexical_cast(force) + (type == FTGUARD_ABS ? " |> " : type == FTGUARD_LESSTHAN ? " < " : " > ") + boost::lexical_cast(threshold); + printf("%s\n", ss.c_str()); + return ss; +} + +void FTGuard::create(golem::Configspace::Index& i) { + jointIdx = i; + const size_t handIdx = i - armIdx; + threshold = 0.2; + force = REAL_ZERO; + type = FTGUARD_ABS; +// printf("FTGuard(armJoints=%u, handChains=%u, fingerJoints=%u) chain=%u, chain joint=%u, joint=%u\n", armIdx, handChains, fingerJoints, (handIdx / fingerJoints) + 1, handIdx % fingerJoints, i); +} + +void XMLData(FTGuard &val, XMLContext* xmlcontext, bool create) { + golem::XMLData("name", val.name, xmlcontext, create); +// golem::XMLData("joint", val.joint, xmlcontext, create); +// golem::XMLData("type", val.type, xmlcontext, create); +// golem::XMLData("value", val.value, xmlcontext, create); +} + diff --git a/src/Spam/Data/Belief/Belief.cpp b/src/Spam/Data/Belief/Belief.cpp new file mode 100644 index 0000000..128687b --- /dev/null +++ b/src/Spam/Data/Belief/Belief.cpp @@ -0,0 +1,16 @@ +/** @file PosePlanner.cpp + * + * @author Claudio Zito + * + * @version 1.0 + * + */ + +#include + +//----------------------------------------------------------------------------- + +using namespace spam; + +//----------------------------------------------------------------------------- + diff --git a/src/Spam/Data/R2GTrajectory/R2GTrajectory.cpp b/src/Spam/Data/R2GTrajectory/R2GTrajectory.cpp new file mode 100644 index 0000000..44501cf --- /dev/null +++ b/src/Spam/Data/R2GTrajectory/R2GTrajectory.cpp @@ -0,0 +1,18 @@ +/** @file PosePlanner.cpp + * + * @author Claudio Zito + * + * @version 1.0 + * + */ + +#include + +//----------------------------------------------------------------------------- + +using namespace spam; + +//----------------------------------------------------------------------------- + + +//------------------------------------------------------------------------------ \ No newline at end of file diff --git a/src/Spam/Demo/R2GPlanner/R2GDemo.cpp b/src/Spam/Demo/R2GPlanner/R2GDemo.cpp new file mode 100644 index 0000000..e73fa86 --- /dev/null +++ b/src/Spam/Demo/R2GPlanner/R2GDemo.cpp @@ -0,0 +1,52 @@ +/** @file PosePlanner.cpp + * + * @author Claudio Zito + * + * @version 1.0 + * + */ + +#include + +//------------------------------------------------------------------------------ + +using namespace golem; +using namespace grasp; +using namespace spam; + +//------------------------------------------------------------------------------ + +void R2GDemo::Desc::load(golem::Context& context, const golem::XMLContext* xmlcontext) { + R2GPlanner::Desc::load(context, xmlcontext); + + xmlcontext = xmlcontext->getContextFirst("demo"); +} + +//------------------------------------------------------------------------------ + +R2GDemo::R2GDemo(Scene &scene) : R2GPlanner(scene) { +} + +R2GDemo::~R2GDemo() { +} + +void R2GDemo::create(const Desc& desc) { + desc.assertValid(Assert::Context("R2GDemo::Desc.")); + + // create object + R2GPlanner::create(desc); // throws +} + +//------------------------------------------------------------------------------ + +void R2GDemo::render() const { + R2GPlanner::render(); +} + +//------------------------------------------------------------------------------ + +int main(int argc, char *argv[]) { + return R2GDemo::Desc().main(argc, argv); +} + +//------------------------------------------------------------------------------ diff --git a/src/Spam/Spam/Belief.cpp b/src/Spam/HBPlan/Belief.cpp similarity index 54% rename from src/Spam/Spam/Belief.cpp rename to src/Spam/HBPlan/Belief.cpp index 8794bc4..818fe39 100644 --- a/src/Spam/Spam/Belief.cpp +++ b/src/Spam/HBPlan/Belief.cpp @@ -6,27 +6,8 @@ * */ -#include -#include -#include -#include -#include - -//#ifdef WIN32 -// #pragma warning (push) -// #pragma warning (disable : 4291 4244 4996 4305) -//#endif -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#ifdef WIN32 -// #pragma warning (pop) -//#endif +//#include +#include //------------------------------------------------------------------------------ @@ -515,105 +496,6 @@ void Belief::meanShiftClustering(grasp::RBPose::Sample& solution, grasp::RBPose: for (ClusterMap::iterator j = clusterMap.begin(); j != clusterMap.end(); ++j) if (j->first != best->first) clusters.push_back(j->second.sample); - -// return; -// -// // mean shift parameters. TODO: add them to a descriptor file -// Real maxLinDist = Real(0.1), maxAngDist = Real(0.5); -// grasp::RBDist stopCriterion(0.05, 0.05); -// const size_t attempts = 10; -// // collection of distances for RBPoses -// typedef std::vector RBDistSeq; -// -// // N is the dimension of poses -// U32 numClusters = 0, N = poses.size(); -// Real solutionEval = REAL_MIN; -// // index of non-visited and visited poses -// std::vector visited; visited.assign(N, false); -// clusters.clear(); -// grasp::U32Seq clusterVotes; clusterVotes.clear(); -// -// for (size_t j = 0; j < desc.generationsMax; ++j) { -// grasp::U32Seq members, votes; members.clear(); votes.assign(N, U32(0)); -// //const U32 dim = [&]() -> const U32 { -// // U32 r(0); -// // for (auto i = visited.begin(); i != visited.end(); ++i) -// // if (!*i) ++r; -// // return r; -// //}(); -// const U32 seed = rand.nextUniform(0, N); -// if (seed >= N) -// throw Message(Message::LEVEL_ERROR, "Belief::meanShiftClustering(): Sampling error"); -// -// grasp::RBPose::Sample tmpMean = poses[seed]; -// context.write("Belief::MeanShift(): seed %u, pose [%.2f %.2f %.2f]\n", seed, poses[seed].p.x, poses[seed].p.y, poses[seed].p.z); -// -// for (size_t n = 0; n < attempts; ++n) { -// // compute distance from tmpMean to all the poses still active -// RBDistSeq distances; distances.assign(N, grasp::RBDist()); -// grasp::RBCoord mean(Vec3(REAL_ZERO), Quat(REAL_ZERO, REAL_ZERO, REAL_ZERO, REAL_ZERO)), meanBuf(Vec3(REAL_ZERO), Quat(REAL_ZERO, REAL_ZERO, REAL_ZERO, REAL_ZERO)); -// Real norm = REAL_ZERO, normBuf = REAL_ZERO; -// U32 numVisited(0); -// for (U32 i = 0; i < N; ++i) { -// // skip visited poses -// if (visited[i]) continue; -// // compute distance [lin, ang] -// distances[i].set(tmpMean, poses[i]); -//// context.write("Belief::MeanShift(): %i distance [%f %f]\n", i, distances[i].lin, distances[i].ang); -// // add to new mean if the distance is less then treashold -// if (distances[i].lin < maxLinDist && distances[i].ang < maxAngDist) { -// visited[i] = true; -// ++numVisited; -// for (size_t l = 0; l < grasp::RBCoord::N; ++l) -// golem::kahanSum(mean[l], meanBuf[l], poses[i][l]); -// } -// } -// if (numVisited == 0) -// continue; -// mean.p *= 1/numVisited; -// mean.q *= 1/numVisited; -// grasp::RBDist distance(tmpMean, mean); -// // if mean doesn't move much stop this cluster -// if (distance.lin < stopCriterion.lin && distance.ang < stopCriterion.ang) { -// grasp::RBDist min(REAL_MAX, REAL_MAX); -// I32 mergeWith = -1, idx = 0; -// for (grasp::RBPose::Sample::Seq::const_iterator clMean = clusters.begin(); clMean != clusters.end(); ++clMean, ++idx) { -// grasp::RBDist dist(mean, *clMean); -// if (dist.lin < min.lin && dist.ang < min.ang) { -// mergeWith = idx; -// min.set(dist.lin, dist.ang); -// } -// } -// if (mergeWith > -1) { -// grasp::RBCoord _mean(Vec3(REAL_ZERO), Quat(REAL_ZERO, REAL_ZERO, REAL_ZERO, REAL_ZERO)), _meanBuf(Vec3(REAL_ZERO), Quat(REAL_ZERO, REAL_ZERO, REAL_ZERO, REAL_ZERO)); -// for (size_t l = 0; l < grasp::RBCoord::N; ++l) -// golem::kahanSum(_mean[l], _meanBuf[l], clusters[mergeWith][l]); -// _mean.p *= 0.5; -// _mean.q *= 0.5; -// clusters[mergeWith] = _mean; -// clusterVotes[mergeWith] += numVisited; -// } -// else { -// clusters.push_back(mean); -// clusterVotes.push_back(numVisited); -// } -// break; -// } -// } // loop until converge -// } // while nonVisited is empty -// if (clusterVotes.size() == 0 && clusters.size() == 0) -// throw Message(Message::LEVEL_ERROR, "Belief::meanShiftClustering(): no clusters"); -// -// U32 voteMax = 0, best = 0; -// for (U32 i = 0; i < clusterVotes.size(); ++i) { -// if (clusterVotes[i] > voteMax) { -// voteMax = clusterVotes[i]; -// best = i; -// } -// } -// solution = clusters[best]; -// // print debug message -// context.debug("RBPose::alignGlobal(): Objective value: %f\n", solutionEval); } void Belief::alignGlobal(grasp::RBPose::Sample& solution, Real& solutionEval, U32& votes) { @@ -825,374 +707,4 @@ void Belief::createUpdate(const Collision::Ptr collision, const golem::Waypoint golem::kahanSum(cdf, c, sampledPose->weight); sampledPose->cdf = cdf; } -} - - -//void Belief::createUpdate(const grasp::Manipulator *manipulator, const golem::Controller::State::Info handInfo, const golem::Waypoint &w, const FTGuard::Seq &triggeredGuards, const grasp::RealSeq &force) { -// context.debug("spam::Belief::createUpdate()...\n"); -// bool intersect = false; -// // retrieve wrist's workspace pose and fingers configuration -// //golem::Mat34 poses[grasp::Manipulator::JOINTS]; -// //manipulator->getPoses(manipulator->getPose(w.cpos), poses); -//// grasp::RealSeq forces(force); -// for (Chainspace::Index i = handInfo.getChains().begin(); i != handInfo.getChains().end(); ++i) { -// // check if any of the joint in the current chain (finger) has been triggered -// std::vector triggered; -// triggered.reserve(handInfo.getJoints(i).size()); -//// context.write("Chain %d\n", i); -// // bounds of the entire finger -// golem::Bounds::Seq bounds; -// grasp::RealSeq forces; -// forces.assign(handInfo.getJoints(i).size(), REAL_ZERO); -// for (Configspace::Index j = handInfo.getJoints(i).begin(); j != handInfo.getJoints(i).end(); ++j) { -// const size_t idx = j - handInfo.getJoints(i).begin(); -// triggered[idx] = [&] () -> bool { -// for (FTGuard::Seq::const_iterator i = triggeredGuards.begin(); i < triggeredGuards.end(); ++i) -// if (j == i->jointIdx) { -// forces[idx] = i->force; // i->type == FTGuard_ABS ? REAL_ZERO : i->type == FTGuard_LESSTHAN ? -REAL_ONE : REAL_ONE; -// return true; -// } -// return false; -// } (); -// //const golem::U32 joint = manipulator->getArmJoints() + idx; -// //golem::Bounds::Seq bounds; -// //manipulator->getJointBounds(joint, poses[joint], bounds); -// manipulator->getJointBounds(golem::U32(j - manipulator->getController().getStateInfo().getJoints().begin()), w.wposex[j], bounds); -// } -// //for (golem::Bounds::Seq::const_iterator b = bounds.begin(); b != bounds.end(); ++b) -// // context.write("finger bound frame <%f %f %f>\n", (*b)->getPose().p.x, (*b)->getPose().p.y, (*b)->getPose().p.z); -// golem::Real norm = golem::REAL_ZERO, c = golem::REAL_ZERO; -//// context.debug("Finger %d\n", i); -// for (grasp::RBPose::Sample::Seq::iterator sampledPose = poses.begin(); sampledPose != poses.end(); ++sampledPose) { -//// context.debug("<%f %f %f> <%f %f %f %f>\n", sampledPose->p.x, sampledPose->p.y, sampledPose->p.z, sampledPose->q.w, sampledPose->q.x, sampledPose->q.y, sampledPose->q.z); -// const Real eval = evaluate(bounds, grasp::RBCoord(w.wpos[i]), *sampledPose, forces, triggered, intersect); -// const bool fingerTriggered = triggered[0] || triggered[1] || triggered[2] || triggered[3]; /*[&] () -> bool { -// for (std::vector::const_iterator i = triggered.begin(); i != triggered.end(); ++i) -// if (*i) return true; -// return false; -// }();*/ -//// sampledPose->weight += fingerTriggered ? myDesc.sensory.contactFac*eval : intersect ? myDesc.sensory.noContactFac*(REAL_ONE - eval) : REAL_ZERO; -//// context.debug("pose <%f %f %f>: eval=%f log=%f, trigguered %s intersect %s prob=%f\n", sampledPose->p.x, sampledPose->p.y, sampledPose->p.z, eval, -Math::log10(eval), fingerTriggered ? "Y" : "N", intersect ? "Y" : "N", fingerTriggered ? myDesc.sensory.contactFac*golem::Math::log10(eval) : intersect ? -REAL_MAX : REAL_ZERO); -//// if (fingerTriggered) context.write("Eval = %f, finger triggered = %s, log(eval) [contactFac, log10] = %f [%f, %f]\n", eval, fingerTriggered ? "YES" : "NO", Math::abs(1000.0*golem::Math::log10(eval)), 1000.0, golem::Math::log10(eval)); -// golem::kahanSum(sampledPose->weight, c, fingerTriggered ? 1000.0*golem::Math::log10(eval) : intersect ? -REAL_MAX : REAL_ZERO); -// //golem::kahanSum(sampledPose->weight, c, fingerTriggered ? Math::abs(/*myDesc.sensory.contactFac*/1000.0*golem::Math::log10(eval)) : intersect ? -REAL_MAX : REAL_ZERO); -// //const Real eval = evaluate(bounds, grasp::RBCoord(w.wposex[j]), *sampledPose, forces[idx], j == robot->getStateHandInfo().getJoints(i).begin(), intersect); -// //sampledPose->weight *= (triggered ? myDesc.sensory.contactFac*eval : intersect ? myDesc.sensory.noContactFac*(REAL_ONE - eval) : REAL_ZERO); -// //golem::kahanSum(norm, c, sampledPose->weight); -// //} -// } -//// context.debug("\n---------------------------------------------\n"); -// } -// -// // normalise weights -// golem::Real norm = golem::REAL_ZERO, c = golem::REAL_ZERO, cdf = golem::REAL_ZERO; -// for (grasp::RBPose::Sample::Seq::iterator sampledPose = poses.begin(); sampledPose != poses.end(); ++sampledPose) -// golem::kahanSum(norm, c, sampledPose->weight > 0 ? sampledPose->weight : Math::log10(REAL_EPS)); -// //context.write("norm=%f\n", norm); -// c = golem::REAL_ZERO; -// for (grasp::RBPose::Sample::Seq::iterator sampledPose = poses.begin(); sampledPose != poses.end(); ++sampledPose) { -// sampledPose->weight /= norm; -//// context.write("sample.weight = %f\n", sampledPose->weight); -// golem::kahanSum(cdf, c, sampledPose->weight); -// sampledPose->cdf = cdf; -// } -//// context.write("spam::Belief::createUpdate(): done!\n"); -// -//} -// -//golem::Real Belief::evaluate(const golem::Bounds::Seq &bounds, const grasp::RBCoord &pose, const grasp::RBPose::Sample &sample, const grasp::RealSeq &forces, std::vector &triggered, bool intersect) { -// Real distMin = /*myDesc.sensory.sensoryRange + */REAL_ONE; -// const Real norm = REAL_ONE - density(myDesc.sensory.sensoryRange); //(ftDrivenDesc.ftModelDesc.enabledLikelihood)?/*REAL_ONE/*/(REAL_ONE - density(ftDrivenDesc.ftModelDesc.distMax)):REAL_ONE; -// Mat34 actionFrame(sample.q, sample.p), queryFrame(Mat34(sample.q, sample.p) * modelFrame), queryFrameInv, fromQueryToJoint; -// -// Vec3 boundFrame, surfaceFrame; -// if (pose.p.distance(queryFrame.p) < distMin) { -// const size_t size = modelPoints.size() < myDesc.maxSurfacePoints ? modelPoints.size() : myDesc.maxSurfacePoints; -// for (size_t i = 0; i < size; ++i) { -// // CHECK: Do you assume that a point has a full frame with **orientation**? Where does the orientation come from? -// -// //const Point& point = size < modelPoints.size() ? modelPoints[size_t(rand.next())% -// // modelPoints.size()] : modelPoints[i]; -// //Mat34 pointFrame; -// //pointFrame.multiply(actionFrame, point.frame); -// const Vec3 point = grasp::Cloud::getPoint(size < modelPoints.size() ? modelPoints[size_t(rand.next())%modelPoints.size()] : modelPoints[i]); -// Mat34 pointFrame = actionFrame; -// pointFrame.p += point; // only position is updated -// const Real dist = [&] () -> Real { -// Real min = REAL_MAX; -// for (golem::Bounds::Seq::const_iterator b = bounds.begin(); b != bounds.end(); ++b) { -// const Real d = (*b)->getSurfaceDistance(pointFrame.p); -// if (d < min) { -// min = d; -// boundFrame = (*b)->getPose().p; -// surfaceFrame = pointFrame.p; -// } -// } -// return min; -// } (); -// if (dist <= REAL_ZERO) -// intersect = true; -// if (dist > -REAL_EPS && dist < distMin) -// distMin = dist; -// } -// -// } -// //return norm*density(distMin); // if uncomment here there is no notion of direction of contacts -// -// // compute the direction of contact (from torques) -// Vec3 v; -// Mat34 jointFrame(Mat33(pose.q), pose.p); -// Mat34 jointFrameInv; -// jointFrameInv.setInverse(jointFrame); -// jointFrameInv.multiply(v, queryFrame.p); -// v.normalise(); -// //context.write(" -> dist(to shape)=%5.7f, bound <%f %f %f>, point <%f %f %f>, v <%f %f %f>, intersection %s, triggered <%s %s %s %s>, forces <%f %f %f %f>, ftDrivenDesc.ftModelDesc.coneTheta1 || Math::abs(thy) > ftDrivenDesc.ftModelDesc.coneTheta2) -// -// // The first joint of the finger responds to side movements on the 'y' axis -// if (triggered[0]) -// //if ((v.y < 0 && forces[0] < 0) || (v.y > 0 && forces[0] > 0)) return REAL_ZERO; // justin robot -// if ((v.x < 0 && forces[0] < 0) || (v.x > 0 && forces[0] > 0)) return REAL_ZERO; // bham robot -// // The other joint respond to movements on the 'z' axis -// else -// for (size_t i = 1; i < triggered.size(); ++i) -// //if ( triggered[i] && ((v.z < 0 && forces[i] < 0) || (v.z > 0 && forces[i] > 0))) return REAL_ZERO; // justin robot -// if ( triggered[i] && ((v.z < 0 && forces[i] < 0) || (v.z > 0 && forces[i] > 0))) return REAL_ZERO; // bham robot -// -// //context.write("evaluation pose <%f %f %f> sample <%f %f %f> v.z=%f force=%f\n", -// // pose.p.x, pose.p.y, pose.p.z, queryFrame.p.x, queryFrame.p.y, queryFrame.p.z, v.z, force); -// -// // return the likelihood of observing such a contact for the current joint -// return 1000*norm*density(distMin); -// -// -//// queryFrameInv.setInverse(queryFrame); -//// fromQueryToJoint.multiply(queryFrameInv, Mat34(pose.q, pose.p)); -//// //context.write("pose <%5.7f %5.7f %5.7f>\n sample <%5.7f %5.7f %5.7f>\n", -//// // pose.p.x, pose.p.y, pose.p.z, sample.p.x, sample.p.y, sample.p.z); -//// //context.write("query-to-joint frame <%5.7f %5.7f %5.7f>\n", -//// // fromQueryToJoint.p.x, fromQueryToJoint.p.y, fromQueryToJoint.p.z); -//// -//// //context.write(" -> magnitude=%5.7f\n", fromQueryToJoint.p.magnitude()); -//// //if (fromQueryToJoint.p.magnitude() > 2*ftDrivenDesc.ftModelDesc.distMax) -//// // return golem::REAL_ZERO; //norm*density(distMin); -//// -//// fromQueryToJoint.multiply(trn, fromQueryToJoint); -//// //context.write("new pose <%5.7f %5.7f %5.7f>, model <%5.7f %5.7f %5.7f>, dist=%5.7f\n", -//// // fromQueryToJoint.p.x, fromQueryToJoint.p.y, fromQueryToJoint.p.z, trn.p.x, trn.p.y, trn.p.z, trn.p.distance(fromQueryToJoint.p)); -//// if (fromQueryToJoint.p.magnitude() < distMin) { -//// const size_t size = modelPoints.size() < ftDrivenDesc.ftModelDesc.points ? modelPoints.size() : ftDrivenDesc.ftModelDesc.points; -//// for (size_t i = 0; i < size; ++i) { -//// const Point& point = size < modelPoints.size() ? modelPoints[size_t(rand.next())%modelPoints.size()] : modelPoints[i]; -//// const Real dist = fromQueryToJoint.p.distance(point.frame.p); -//// if (dist < distMin) -//// distMin = dist; -//// } -//// } -////// context.write(" -> dist(to shape)=%5.7f, norm=%5.7f, density=%5.7f, prob of touching=%5.7f\n", distMin, norm, density(distMin), norm*density(distMin)); -//// return /*norm**/density(distMin); -//} - -//bool Belief::intersect(const golem::Bounds::Seq &bounds, const golem::Mat34 &pose) const { -// if (hypotheses.empty()) -// return false; -// grasp::RBPose::Sample frame = (*hypotheses.begin())->toRBPoseSample(); -// Mat34 actionFrame(frame.q, frame.p); -// grasp::Cloud::PointSeq points; -// std::vector distances; -// -// if ((*hypotheses.begin())->nearestKPoints(grasp::RBCoord(pose), points, distances) > 0) { -// //const Vec3 point = grasp::Cloud::getPoint(points[0]); -// for (size_t i = 0; i < points.size(); ++i) { -// const Vec3 point = grasp::Cloud::getPoint(points[i]); -// //Mat34 pointFrame = actionFrame; -// //pointFrame.p += point; // only position is updated -// //context.debug("Joint at pose <%f %f %f>, point at pose <%f %f %f>\n", pose.p.x, pose.p.y, pose.p.z, pointFrame.p.x, pointFrame.p.y, pointFrame.p.z); -// for (golem::Bounds::Seq::const_iterator b = bounds.begin(); b != bounds.end(); ++b) { -// if ((*b)->intersect(point)) { -//// context.debug("Joint at pose <%f %f %f> intersects point at pose <%f %f %f>\n", pose.p.x, pose.p.y, pose.p.z, point.x, point.y, point.z); -// return true; -// } -// } -// } -// //context.debug("returns false\n--------------------------------------\n"); -// } -// -// // quick check of intersection -//// if (pose.p.distance(actionFrame.p) < myDesc.distanceRange) { -//// const size_t size = modelPoints.size() < myDesc.maxSurfacePoints ? modelPoints.size() : myDesc.maxSurfacePoints; -//// for (size_t i = 0; i < size; ++i) { -//// const Vec3 point = grasp::Cloud::getPoint(size < modelPoints.size() ? modelPoints[size_t(rand.next())%modelPoints.size()] : modelPoints[i]); -//// Mat34 pointFrame = actionFrame; -//// pointFrame.p += point; // only position is updated -////// std::cout << "dist "; -//// for (golem::Bounds::Seq::const_iterator b = bounds.begin(); b != bounds.end(); ++b) { -//// const Real d = (*b)->getSurfaceDistance(pointFrame.p); -////// std::cout << d << " "; -//// if (d < REAL_ZERO) { -//// context.debug("Joint at pose <%f %f %f> intersects estimated object at pose <%f %f %f>\n", pose.p.x, pose.p.y, pose.p.z, actionFrame.p.x, actionFrame.p.y, actionFrame.p.z); -//// return true; -//// } -//// } -////// std::cout << "\n"; -//// } -//// } -// return false; -//} - -//void Belief::createUpdate(const grasp::Manipulator *manipulator, const grasp::Robot *robot, const golem::Waypoint &w, const FTGuard::Seq &triggeredGuards, const grasp::RealSeq &force) { -// context.write("spam::Belief::createUpdate()...\n"); -// Real weight = golem::REAL_ONE; -// bool intersect = false; -// // retrieve wrist's workspace pose and fingers configuration -// //golem::Mat34 poses[grasp::Manipulator::JOINTS]; -// //manipulator->getPoses(manipulator->getPose(w.cpos), poses); -// grasp::RealSeq forces(force); -// for (Chainspace::Index i = robot->getStateHandInfo().getChains().begin(); i != robot->getStateHandInfo().getChains().end(); ++i) { -// // check if any of the joint in the current chain (finger) has been triggered -// bool triggered = false; -//// context.write("Chain %d\n", i); -// for (Configspace::Index j = robot->getStateHandInfo().getJoints(i).begin(); j != robot->getStateHandInfo().getJoints(i).end(); ++j) { -// const size_t idx = j - robot->getStateHandInfo().getJoints().begin(); -// triggered = triggered || [&] () -> bool { -// for (FTGuard::Seq::const_iterator i = triggeredGuards.begin(); i < triggeredGuards.end(); ++i) -// if (j == i->jointIdx) { -// forces[idx] = i->type == FTGuard_ABS ? 0 : i->type == FTGuard_LESSTHAN ? -1 : 1; -// return true; -// } -// return false; -// } (); -// //const golem::U32 joint = manipulator->getArmJoints() + idx; -// //golem::Bounds::Seq bounds; -// //manipulator->getJointBounds(joint, poses[joint], bounds); -// golem::Bounds::Seq bounds; -// manipulator->getJointBounds(golem::U32(j - manipulator->getController()->getStateInfo().getJoints().begin()), w.wposex[j], bounds); -// //for (golem::Bounds::Seq::const_iterator b = bounds.begin(); b != bounds.end(); ++b) -// // context.write("finger bound frame <%f %f %f>\n", (*b)->getPose().p.x, (*b)->getPose().p.y, (*b)->getPose().p.z); -// golem::Real norm = golem::REAL_ZERO, c = golem::REAL_ZERO; -// context.write("Updating poses' weights for chain=%d joint=%d(%d) (triggered %s)\n", i, j - robot->getStateHandInfo().getJoints(i).begin(),j, triggered ? "Y" : "N"); -// for (grasp::RBPose::Sample::Seq::iterator sampledPose = poses.begin(); sampledPose != poses.end(); ++sampledPose) { -// const Real eval = triggered ? evaluate(bounds, grasp::RBCoord(w.wposex[j]), *sampledPose, forces[idx], j == robot->getStateHandInfo().getJoints(i).begin(), intersect) : REAL_ONE; -// sampledPose->weight *= -// //const Real eval = evaluate(bounds, grasp::RBCoord(w.wposex[j]), *sampledPose, forces[idx], j == robot->getStateHandInfo().getJoints(i).begin(), intersect); -// //sampledPose->weight *= (triggered ? myDesc.sensory.contactFac*eval : intersect ? myDesc.sensory.noContactFac*(REAL_ONE - eval) : REAL_ZERO); -// //golem::kahanSum(norm, c, sampledPose->weight); -// } -// context.write("\n---------------------------------------------\n"); -// } -// } -// -// // normalise weights -// golem::Real norm = golem::REAL_ZERO, c = golem::REAL_ZERO, cdf = golem::REAL_ZERO; -// for (grasp::RBPose::Sample::Seq::iterator sampledPose = poses.begin(); sampledPose != poses.end(); ++sampledPose) -// golem::kahanSum(norm, c, sampledPose->weight); -// context.write("norm=%f\n", norm); -// for (grasp::RBPose::Sample::Seq::iterator sampledPose = poses.begin(); sampledPose != poses.end(); ++sampledPose) { -// sampledPose->weight /= norm; -// context.write("sample.weight = %f\n", sampledPose->weight); -// cdf += sampledPose->weight; -// sampledPose->cdf = cdf; -// } -// context.write("spam::Belief::createUpdate(): done!\n"); -// -//} -// -//golem::Real Belief::evaluate(const golem::Bounds::Seq &bounds, const grasp::RBCoord &pose, const grasp::RBPose::Sample &sample, const Real &force, bool jointZero, bool intersect) { -// Real distMin = myDesc.sensory.sensoryRange + REAL_ONE; -// const Real norm = REAL_ONE - density(myDesc.sensory.sensoryRange); //(ftDrivenDesc.ftModelDesc.enabledLikelihood)?/*REAL_ONE/*/(REAL_ONE - density(ftDrivenDesc.ftModelDesc.distMax)):REAL_ONE; -// Mat34 actionFrame(sample.q, sample.p), queryFrame(Mat34(sample.q, sample.p) * modelFrame), queryFrameInv, fromQueryToJoint; -// -// Vec3 boundFrame, surfaceFrame; -// if (pose.p.distance(queryFrame.p) < distMin) { -// const size_t size = modelPoints.size() < myDesc.maxSurfacePoints ? modelPoints.size() : myDesc.maxSurfacePoints; -// for (size_t i = 0; i < size; ++i) { -// // CHECK: Do you assume that a point has a full frame with **orientation**? Where does the orientation come from? -// -// //const Point& point = size < modelPoints.size() ? modelPoints[size_t(rand.next())% -// // modelPoints.size()] : modelPoints[i]; -// //Mat34 pointFrame; -// //pointFrame.multiply(actionFrame, point.frame); -// const Vec3 point = grasp::Cloud::getPoint(size < modelPoints.size() ? modelPoints[size_t(rand.next())%modelPoints.size()] : modelPoints[i]); -// Mat34 pointFrame = actionFrame; -// pointFrame.p += point; // only position is updated -// const Real dist = [&] () -> Real { -// Real min = REAL_MAX; -// for (golem::Bounds::Seq::const_iterator b = bounds.begin(); b != bounds.end(); ++b) { -// const Real d = (*b)->getSurfaceDistance(pointFrame.p); -// if (d < min) { -// min = d; -// boundFrame = (*b)->getPose().p; -// surfaceFrame = pointFrame.p; -// } -// } -// return min; -// } (); -// if (dist <= REAL_ZERO) -// intersect = true; -// if (dist > -REAL_EPS && dist < distMin) -// distMin = dist; -// } -// -// } -// //return norm*density(distMin); // if uncomment here there is no notion of direction of contacts -// -// // compute the direction of contact (from torques) -// Vec3 v; -// Mat34 jointFrame(Mat33(pose.q), pose.p); -// Mat34 jointFrameInv; -// jointFrameInv.setInverse(jointFrame); -// jointFrameInv.multiply(v, queryFrame.p); -// v.normalise(); -// context.write(" -> dist(to shape)=%5.7f, bound <%f %f %f>, point <%f %f %f>, v <%f %f %f>, intersection %s, first_joint %s, norm=%5.7f, density=%5.7f, prob of touching=%5.7f\n", distMin, -// boundFrame.x, boundFrame.y, boundFrame.z, surfaceFrame.x, surfaceFrame.y, surfaceFrame.z, v.x, v.y, v.z, -// intersect ? "Y" : "N", jointZero ? "Y" : "N", norm, density(distMin), norm*density(distMin)); -// -//// const Real thx(Math::atan2(v.z, v.x)), thy(Math::atan2(v.z, v.y)); -//// if (v.z < 0 || Math::abs(thx) > ftDrivenDesc.ftModelDesc.coneTheta1 || Math::abs(thy) > ftDrivenDesc.ftModelDesc.coneTheta2) -// -// // The first joint of the finger responds to side movements on the 'y' axis -// if (jointZero) -// if ((v.y < 0 && force < 0) || (v.y > 0 && force > 0)) return REAL_ZERO; -// // The other joint respond to movements on the 'z' axis -// else -// if ((v.z < 0 && force < 0) || (v.z > 0 && force > 0)) return REAL_ZERO; -// //context.write("evaluation pose <%f %f %f> sample <%f %f %f> v.z=%f force=%f\n", -// // pose.p.x, pose.p.y, pose.p.z, queryFrame.p.x, queryFrame.p.y, queryFrame.p.z, v.z, force); -// -// // return the likelihood of observing such a contact for the current joint -// return norm*density(distMin); -// -// -//// queryFrameInv.setInverse(queryFrame); -//// fromQueryToJoint.multiply(queryFrameInv, Mat34(pose.q, pose.p)); -//// //context.write("pose <%5.7f %5.7f %5.7f>\n sample <%5.7f %5.7f %5.7f>\n", -//// // pose.p.x, pose.p.y, pose.p.z, sample.p.x, sample.p.y, sample.p.z); -//// //context.write("query-to-joint frame <%5.7f %5.7f %5.7f>\n", -//// // fromQueryToJoint.p.x, fromQueryToJoint.p.y, fromQueryToJoint.p.z); -//// -//// //context.write(" -> magnitude=%5.7f\n", fromQueryToJoint.p.magnitude()); -//// //if (fromQueryToJoint.p.magnitude() > 2*ftDrivenDesc.ftModelDesc.distMax) -//// // return golem::REAL_ZERO; //norm*density(distMin); -//// -//// fromQueryToJoint.multiply(trn, fromQueryToJoint); -//// //context.write("new pose <%5.7f %5.7f %5.7f>, model <%5.7f %5.7f %5.7f>, dist=%5.7f\n", -//// // fromQueryToJoint.p.x, fromQueryToJoint.p.y, fromQueryToJoint.p.z, trn.p.x, trn.p.y, trn.p.z, trn.p.distance(fromQueryToJoint.p)); -//// if (fromQueryToJoint.p.magnitude() < distMin) { -//// const size_t size = modelPoints.size() < ftDrivenDesc.ftModelDesc.points ? modelPoints.size() : ftDrivenDesc.ftModelDesc.points; -//// for (size_t i = 0; i < size; ++i) { -//// const Point& point = size < modelPoints.size() ? modelPoints[size_t(rand.next())%modelPoints.size()] : modelPoints[i]; -//// const Real dist = fromQueryToJoint.p.distance(point.frame.p); -//// if (dist < distMin) -//// distMin = dist; -//// } -//// } -////// context.write(" -> dist(to shape)=%5.7f, norm=%5.7f, density=%5.7f, prob of touching=%5.7f\n", distMin, norm, density(distMin), norm*density(distMin)); -//// return /*norm**/density(distMin); -//} +} \ No newline at end of file diff --git a/src/Spam/Spam/GraphPlanner.cpp b/src/Spam/HBPlan/GraphPlanner.cpp similarity index 99% rename from src/Spam/Spam/GraphPlanner.cpp rename to src/Spam/HBPlan/GraphPlanner.cpp index e0b8cb4..865ed50 100644 --- a/src/Spam/Spam/GraphPlanner.cpp +++ b/src/Spam/HBPlan/GraphPlanner.cpp @@ -7,13 +7,11 @@ * */ -#include +#include #include #include #include -#include #include // std::setprecision -//#include //------------------------------------------------------------------------------ diff --git a/src/Spam/HBPlan/Heuristic.cpp b/src/Spam/HBPlan/Heuristic.cpp new file mode 100644 index 0000000..5022ce4 --- /dev/null +++ b/src/Spam/HBPlan/Heuristic.cpp @@ -0,0 +1,799 @@ +/** @file Heuristic.cpp + * + * @author Claudio Zito (The University Of Birmingham) + * + * @version 1.0 + * + */ + +#include +#include +#include + +//------------------------------------------------------------------------------ + +using namespace golem; +using namespace grasp; +using namespace spam; + +//------------------------------------------------------------------------------ + +#ifdef _HEURISTIC_PERFMON +U32 FTDrivenHeuristic::perfCollisionWaypoint; +U32 FTDrivenHeuristic::perfCollisionPath; +U32 FTDrivenHeuristic::perfCollisionGroup; +U32 FTDrivenHeuristic::perfCollisionBounds; +U32 FTDrivenHeuristic::perfCollisionSegs; +U32 FTDrivenHeuristic::perfCollisionPointCloud; +U32 FTDrivenHeuristic::perfBoundDist; +U32 FTDrivenHeuristic::perfH; + +void FTDrivenHeuristic::resetLog() { + perfCollisionWaypoint = 0; + perfCollisionPath = 0; + perfCollisionGroup = 0; + perfCollisionBounds = 0; + perfCollisionSegs = 0; + perfCollisionPointCloud = 0; + perfBoundDist = 0; + perfH = 0; +} + +void FTDrivenHeuristic::writeLog(Context &context, const char *str) { + context.debug( + "%s: collision_{waypoint, path, group, bounds, point cloud, } = {%u, %u, %u, %u, %u, %f}, calls_{getBoundedDist(), h()} = {%u, %u}\n", str, perfCollisionWaypoint, perfCollisionPath, perfCollisionGroup, perfCollisionBounds, perfCollisionPointCloud, perfCollisionPath > 0 ? Real(perfCollisionSegs) / perfCollisionPath : REAL_ZERO, perfBoundDist, perfH + ); +} +#endif + +//------------------------------------------------------------------------------ + +inline golem::Vec3 dot(const golem::Vec3 &v, const golem::Vec3 &diagonal) { + golem::Vec3 result; + for (size_t i = 0; i < 3; ++i) + result[i] = v[i] * diagonal[i]; + return result; +} + +void dotInverse(const std::vector& v0, const std::vector& v1, std::vector &result) { + if (v0.size() != v1.size()) + throw MsgIncompatibleVectors(Message::LEVEL_CRIT, "spam::dot(std::vector, std::vector): Invalid vectors size."); +// printf("spam::dotInverse()\n"); + result.reserve(v0.size()); + for (size_t i = 0; i < v0.size(); ++i) { + const Real r = v0[i] * (REAL_ONE/v1[i]); + result.push_back(r); +// printf("result[%d] = %.5f * %.5f = %.5f\n", i, v0[i], (REAL_ONE/v1[i]), r); + } +} + +Real dot(const std::vector& v0, const std::vector& v1) { + if (v0.size() != v1.size()) + throw MsgIncompatibleVectors(Message::LEVEL_CRIT, "spam::dot(std::vector, std::vector): Invalid vectors size."); + + Real result; + result = REAL_ZERO; + for (size_t i = 0; i < v0.size(); ++i) + result += v0[i] * v1[i]; +// printf("dot(v0, v1) %.5f\n", result); + return result; +} + +//------------------------------------------------------------------------------ + +FTDrivenHeuristic::FTDrivenHeuristic(golem::Controller &controller) : Heuristic(controller), rand(context.getRandSeed()) {} + +bool FTDrivenHeuristic::create(const Desc &desc) { +// context.debug("FTDrivenHeuristic::create()...\n"); + Heuristic::create((Heuristic::Desc&)desc); + + // printing of debug +// context.write("contact_fac=%f, max_points=%d, covariance=%f\n", desc.contactFac, desc.maxSurfacePoints, desc.covariance.p[0]); + + ftDrivenDesc = desc; +// context.write("Heuristic params: dflt=%f limits=%f root=%f\n", desc.costDesc.distDfltFac, desc.costDesc.distLimitsFac, desc.costDesc.distRootFac); + ftDrivenDesc.ftModelDesc = desc.ftModelDesc; + //ftDrivenDesc.ftModelDesc.dim = 0; + //for (Chainspace::Index i = stateInfo.getChains().begin(); i < stateInfo.getChains().end(); ++i) { + // const ChainDesc* cdesc = getChainDesc()[i]; + // if (cdesc->enabledObs) + // ++ftDrivenDesc.ftModelDesc.dim; + //} + + // controllers + MultiCtrl *multiCtrl = dynamic_cast(&controller); + if (multiCtrl == NULL) + throw Message(Message::LEVEL_CRIT, "Unknown controller"); + // which has two other controllers: arm + hand + if (multiCtrl->getControllers().size() < 2) + throw Message(Message::LEVEL_CRIT, "Arm and hand controllers expected"); + arm = dynamic_cast(multiCtrl->getControllers()[0]); + hand = dynamic_cast(multiCtrl->getControllers()[1]); + if (arm == NULL || hand == NULL) + throw Message(Message::LEVEL_CRIT, "Robot::create(): Robot requires SingleCtrl"); + + armInfo = arm->getStateInfo(); + handInfo = hand->getStateInfo(); + + jointFac = desc.ftModelDesc.jointFac; + + manipulator.reset(); + + enableUnc = false; + + pointCloudCollision = false; + + collision.reset(); + waypoint.setToDefault(); + waypoint.points = 100000;//ftDrivenDesc.ftModelDesc.points; + + hypothesisBoundsSeq.clear(); + + testCollision = false; + + return true; +} + +void FTDrivenHeuristic::setDesc(const Desc &desc) { +// Heuristic::setDesc((Heuristic::Desc&)desc); +// (Heuristic::Desc&)this->desc = this->Heuristic::desc; +// this->bsDesc.beliefDesc = desc.beliefDesc; + this->ftDrivenDesc = desc; + this->ftDrivenDesc.ftModelDesc = desc.ftModelDesc; +} + +void FTDrivenHeuristic::setDesc(const Heuristic::Desc &desc) { + Heuristic::setDesc(desc); +} + +//------------------------------------------------------------------------------ + +golem::Real FTDrivenHeuristic::cost(const golem::Waypoint &w, const golem::Waypoint &root, const golem::Waypoint &goal) const { + golem::Real c = golem::REAL_ZERO; + const bool enable = enableUnc && pBelief->getHypotheses().size() > 0; + + if (desc.costDesc.distRootFac > golem::REAL_ZERO) + c += desc.costDesc.distRootFac*getDist(w, root); + +// if (desc.costDesc.distGoalFac > golem::REAL_ZERO) { + const Real dist = (enable)?getMahalanobisDist(w, goal):getWorkspaceDist(w, goal); + c += /*desc.costDesc.distGoalFac**/dist; +// c += desc.costDesc.distGoalFac*getMahalanobisDist(w, goal); getWorkspaceDist +// context.getMessageStream()->write(Message::LEVEL_DEBUG, "MyHeuristic::cost(w, root, goal) goal <%.2f, %.2f, %.2f>\n", goal.wpos[chainArm].p.x, goal.wpos[chainArm].p.y, goal.wpos[chainArm].p.z); +// } + + if (desc.costDesc.distLimitsFac > golem::REAL_ZERO) + c += desc.costDesc.distLimitsFac*getConfigspaceLimitsDist(w.cpos); + if (desc.costDesc.distDfltFac > golem::REAL_ZERO) + c += desc.costDesc.distDfltFac*getConfigspaceDist(w.cpos, dfltPos); + + return c; +} + +Real FTDrivenHeuristic::cost(const golem::Waypoint &w0, const golem::Waypoint &w1) const { +// SecTmReal init = context.getTimer().elapsed(); + //if (collides(w0, w1)) + // return Node::COST_INF; + Real c = REAL_ZERO, d; + const bool enable = enableUnc && (pBelief.get() && pBelief->getHypotheses().size() > 0); + + const Chainspace::Index chainArm = stateInfo.getChains().begin(); +// if (desc.costDesc.distPathFac > REAL_ZERO) { + d = Heuristic::getBoundedDist(w0, w1); + if (d >= Node::COST_INF) + return Node::COST_INF; + + //context.debug("FTHeuristic::cost(w0 <%.2f %.2f %.2f>, w1 <%.2f %.2f %.2f>, root <%.2f %.2f %.2f>, goal <%.2f %.2f %.2f>\n", + // w0.wpos[chainArm].p.x, w0.wpos[chainArm].p.y, w0.wpos[chainArm].p.z, w1.wpos[chainArm].p.x, w1.wpos[chainArm].p.y, w1.wpos[chainArm].p.z, + // root.wpos[chainArm].p.x, root.wpos[chainArm].p.y, root.wpos[chainArm].p.z, goal.wpos[chainArm].p.x, root.wpos[chainArm].p.y, root.wpos[chainArm].p.z); +// const Real bound = getBoundedDist(w1); + const Real r = (enable && getBoundedDist(w1) < Node::COST_INF) ? expectedObservationCost(w0, w1) : 1;//(d < 5*bsDesc.ftModelDesc.tactileRange)?getObservationalCost(w0, w1):1; +// if (r < 1) context.write("rewarded trajectory dist=%f reward %f (enalbe=%s)\n", bound < Node::COST_INF ? bound : -1, r, enable ? "ON" : "OFF"); + c += /*desc.costDesc.distPathFac**/d*r; +// } + // rewards more natural approaches (palm towards object) +// c *= directionApproach(w1); + + if (desc.costDesc.distLimitsFac > REAL_ZERO) + c += desc.costDesc.distLimitsFac*getConfigspaceLimitsDist(w1.cpos); + if (desc.costDesc.distDfltFac > REAL_ZERO) + c += desc.costDesc.distDfltFac*getConfigspaceDist(w1.cpos, dfltPos); + + //if (getWorkspaceDist(w1, goal) < ftDrivenDesc.ftModelDesc.distMax) + // c += getCollisionCost(w0, w1, samples.begin()); + + //context.debug(" bounded distance %.4f\n", d); + //context.debug(" Euclid. distance %.4f\n", getWorkspaceDist(w1, goal)); + //context.debug(" Mahala. distance %.4f\n", w); + //context.debug(" total cost func. %.4f\n\n", c); + +// context.debug("FTDrivenHeuristic::cost(enable=%s): preforming time %.7f\n", enableUnc ? "ON" : "OFF", context.getTimer().elapsed() - init); + return c; +} + +/** +computes the bounded distance between the chains' poses (only for chains with enabled obs) +and the reference frame of the samples. If at least one of the distance is <= of the max distance +it returns the distance.. otherwise it returns INF. +*/ +golem::Real FTDrivenHeuristic::getBoundedDist(const golem::Waypoint& w) const { +#ifdef _HEURISTIC_PERFMON + ++perfBoundDist; +#endif + const Real ret = golem::Node::COST_INF; + if (pBelief.get() && pBelief->getHypotheses().empty()) + return ret; + + Hypothesis::Seq::const_iterator maxLhdPose = pBelief->getHypotheses().begin(); + for (Chainspace::Index i = stateInfo.getChains().end() - 1; i >= stateInfo.getChains().begin(); --i){ + const RBCoord c(w.wpos[i]); + const Real dist = c.p.distance((*maxLhdPose)->toRBPoseSampleGF().p); + if (dist < ftDrivenDesc.ftModelDesc.distMax) + return dist; + } + + //for (Belief::Hypothesis::Seq::const_iterator s = ++pBelief->getHypotheses().begin(); s != pBelief->getHypotheses().end(); ++s) { + // for (Chainspace::Index i = stateInfo.getChains().begin(); i < stateInfo.getChains().end(); ++i) { + // const ChainDesc* cdesc = getChainDesc()[i]; + // if (cdesc->enabledObs) { + // const RBCoord c(w.wpos[i]); + // const Real dist = c.p.distance((*s)->toRBPoseSampleGF().p); + // if (dist < this->ftDrivenDesc.ftModelDesc.distMax) + // return dist; + // } + // } + //} + return ret; +} + +golem::Real FTDrivenHeuristic::getMahalanobisDist(const golem::Waypoint& w0, const golem::Waypoint& goal) const { +// context.debug("FTHeuristic::getMahalanobisDist()\n"); + golem::Real dist = golem::REAL_ZERO; + + for (golem::Chainspace::Index i = stateInfo.getChains().begin(); i < stateInfo.getChains().end(); ++i) { + const ChainDesc* desc = getChainDesc()[i]; + + const grasp::RBCoord a(w0.wpos[i]), b(goal.wpos[i]); + // linear distance + if (desc->enabledLin) { + //const Real d0 = covarianceInv[0]*golem::Math::sqr(a[0] - b[0]) + covarianceInv[1]*golem::Math::sqr(a[1] - b[1]) + covarianceInv[2]*golem::Math::sqr(a[2] - b[2]); + //const Real d1 = covarianceInv[3]*golem::Math::sqr(a[3] - b[3]) + covarianceInv[4]*golem::Math::sqr(a[4] - b[4]) + covarianceInv[5]*golem::Math::sqr(a[5] - b[5]) + covarianceInv[6]*golem::Math::sqr(a[6] - b[6]); + //const Real d2 = covarianceInv[3]*golem::Math::sqr(a[3] + b[3]) + covarianceInv[4]*golem::Math::sqr(a[4] + b[4]) + covarianceInv[5]*golem::Math::sqr(a[5] + b[5]) + covarianceInv[6]*golem::Math::sqr(a[6] + b[6]); + //return golem::REAL_HALF*(d0 + std::min(d1, d2)); + const golem::Vec3 p = w0.wpos[i].p - goal.wpos[i].p; + const golem::Real d = dot(p, pBelief->getSampleProperties().covarianceInv.p).dot(p); +// context.debug(" Vec p<%.4f,%.4f,%.4f>, p^TC^-1<%.4f,%.4f,%.4f>, d=%.4f\n", +// p.x, p.y, p.z, dot(p, covarianceInv.p).x, dot(p, covarianceInv.p).y, dot(p, covarianceInv.p).z, +// d); + /*if (d < 0.001) + context.getMessageStream()->write(Message::LEVEL_DEBUG, "MyHeuristic::getMahalanobisDist() w0 <%.2f, %.2f, %.2f> Md %.4f, d %.4f\n", + w0.wpos[i].p.x, w0.wpos[i].p.y, w0.wpos[i].p.z, d, Heuristic::getLinearDist(w0.wpos[i].p, goal.wpos[i].p));*/ + dist += golem::Math::sqrt(d); + } + // angular distance + if (desc->enabledAng) { + const golem::Real q = Heuristic::getAngularDist(w0.qrot[i], goal.qrot[i]); + dist += (golem::REAL_ONE - desc->distNorm)*q; +// context.debug(" Ang distance %.4f\n", q); + } + } +// context.debug(" Mahalanobis distance %.4f\n", ftDrivenDesc.ftModelDesc.mahalanobisDistFac*dist); + return ftDrivenDesc.ftModelDesc.mahalanobisDistFac*dist; +} + +/* + J(wi, wj)=SUM_k e^-PSI(wi, wj, k) + where PSI(wi, wj, k)=||h(wj,p^k)-h(wj,p^1)||^2_Q +*/ +Real FTDrivenHeuristic::expectedObservationCost(const golem::Waypoint &wi, const golem::Waypoint &wj) const { +// context.write("FTDrivenHeuristic::getObservationalCost(const Waypoint &wi, const Waypoint &wj)\n"); + if (pBelief->getHypotheses().size() == 0) + return REAL_ONE; + + Real cost = REAL_ZERO; + /*Real boundDist = REAL_ZERO; + for (Chainspace::Index i = stateInfo.getChains().begin(); i < stateInfo.getChains().end(); ++i) { + for (U32 k = 1; k <= desc.beliefDesc.nParticles; ++k) + boundDist += getLinearDist(wj.wpos[i].p, particleSet[k].object.shapes[0]->pose.p); + } + if (boundDist > desc.ftModelDesc.boundFTDist) + return -1;*/ + + std::stringstream str; + std::vector hij; +// for (HypSample::Map::const_iterator k = ++samples.begin(); k != samples.end(); ++k) { +// Real value = Math::exp(-REAL_HALF*psi(wi, wj, k)); + // for debug purposes I compute psi function here. + hij.clear(); + h(wi, wj, /*k,*/ hij); + Real value(REAL_ZERO); + for (std::vector::const_iterator i = hij.begin(); i != hij.end(); ++i) + value += Math::sqr(*i); + //str << "h(size=" << hij.size() << ") <"; + //for (std::vector::const_iterator i = hij.begin(); i != hij.end(); ++i) + // str << *i << " "; + //str << "> magnitude=" << Math::sqrt(value) << " e=" << Math::exp(-REAL_HALF*Math::sqrt(value)) << "\n"; + cost += Math::exp(-Math::sqrt(value)); // REAL_HALF* +// } + +// str << "cost=" << cost/(samples.size() - 1) << "\n"; +// std::printf("%s\n", str.str().c_str()); + return cost/(pBelief->getHypotheses().size() - 1); +} + +//Real FTDrivenHeuristic::psi(const Waypoint &wi, const Waypoint &wj, HypSample::Map::const_iterator p) const { +// context.getMessageStream()->write(Message::LEVEL_DEBUG, "psi(wi, wj) = ||h(p^i) - h(p^0)||_gamma\n"); +// std::vector hij, gamma; +// h(wi, wj, p, hij); +// +// Real magnitude(REAL_ZERO); +// for (std::vector::const_iterator i = hij.begin(); i != hij.end(); ++i) +// magnitude += Math::sqr(*i); +// +// context.write("h <"); +// for (std::vector::const_iterator i = hij.begin(); i != hij.end(); ++i) +// context.write("%f ", *i); +// context.write("> magnitude=%f \n", Math::sqrt(magnitude)); +// return Math::sqrt(magnitude); +// //// generate diagonal matrix +// //gamma.clear(); +// //gamma.reserve(hij.size()); +// //const Real noise = 2*covarianceDet; +// //for (size_t i = 0; i < hij.size(); ++i) +// // gamma.push_back(noise); +// // +// //// sqrt((h(wi,wj,p^k)-h(wi,wj,p^-1)) \ GAMMA * (h(wi,wj,p^k)-h(wi,wj,p^-1))) +// //std::vector v; +// //dotInverse(hij, gamma, v); +// //Real d = dot(v, hij); +// ////std::printf("FTHeuristic::h(hij):"); +// ////for (std::vector::const_iterator i = hij.begin(); i != hij.end(); ++i) +// //// std::printf(" %.10f", *i); +// ////std::printf("\n"); +// ////std::printf("FTHeuristic::h(v):"); +// ////for (std::vector::const_iterator i = v.begin(); i != v.end(); ++i) +// //// std::printf(" %.10f", *i); +// ////std::printf("\n"); +// ////std::printf("FTHeuristic::psi():"); +// ////for (std::vector::const_iterator i = gamma.begin(); i != gamma.end(); ++i) +// //// std::printf(" %.10f", *i); +// ////std::printf("\n"); +// ////std::printf("||h(p^%d) - h(p^0)||_gamma = %.10f\n", p->first, d); +// //return REAL_HALF*d; +//} + +void FTDrivenHeuristic::h(const golem::Waypoint &wi, const golem::Waypoint &wj, std::vector &y) const { +// context.write("FTDrivenHeuristic::h(const Waypoint &wi, const Waypoint &wj, std::vector &y)\n"); +#ifdef _HEURISTIC_PERFMON + ++perfH; +#endif + auto kernel = [&] (Real x, Real lambda) -> Real { + return /*lambda**/golem::Math::exp(-lambda*x); + }; + Hypothesis::Seq::const_iterator maxLhdPose = pBelief->getHypotheses().begin(); + const Real norm = (REAL_ONE - /*pBelief->*/kernel(ftDrivenDesc.ftModelDesc.distMax, /*pBelief->*/ftDrivenDesc.ftModelDesc.lambda));//(ftDrivenDesc.ftModelDesc.enabledLikelihood)?/*REAL_ONE/*/(REAL_ONE - kernel(ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.lambda)):REAL_ONE; + y.clear(); + const U32 meanIdx = 0; + U32 steps = 1; + + // computes the estimate of contact for the starting PRM node + //grasp::RealSeq estimates; estimates.assign(pBelief->getHypotheses().size(), REAL_ZERO); U32 hypIndex = 0; + //for (auto p = pBelief->getHypotheses().cbegin(); p != pBelief->getHypotheses().cend(); ++p) + // estimates[hypIndex++] = estimate(p, wi); + + if (false) { + const Real dist = getDist(wi, wj); + steps = (U32)Math::round(dist/(desc.collisionDesc.pathDistDelta)); + } + y.reserve((pBelief->getHypotheses().size() - 1)*steps/**(armInfo.getChains().size() + handInfo.getJoints().size())*/); + + golem::Waypoint w; + U32 i = (steps == 1) ? 0 : 1; + for (; i < steps; ++i) { + if (steps != 1) { + for (Configspace::Index j = stateInfo.getJoints().begin(); j < stateInfo.getJoints().end(); ++j) + w.cpos[j] = wj.cpos[j] - (wj.cpos[j] - wi.cpos[j])*Real(i)/Real(steps); + + // skip reference pose computation + w.setup(controller, false, true); + } + else w = wj; + +// Mat34 closestShape_idx, closestShape_mean; +// context.getMessageStream()->write(Message::LEVEL_DEBUG, "||h(wj, p^%d) - h(wj, p^0)||\n", idx); + // evaluated only the end effector(s) of the arm (wrist) +#ifdef _PER_JOINT_ + //for (Chainspace::Index i = armInfo.getChains().begin(); i != armInfo.getChains().end(); ++i) { + // const ChainDesc* cdesc = getChainDesc()[i]; + // if (cdesc->enabledObs) { + // // computes the workspace coordinate of the relative chain, if it should be in the obs + // const RBCoord c(w.wpos[i]); + // // computes the likelihood of contacting the mean pose + // const Real likelihood_p1 = norm*pBelief->density((*maxLhdPose)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); + + // // computes the likelihood of contacting the other hypotheses + // for (Belief::Hypothesis::Seq::const_iterator p = ++pBelief->getHypotheses().begin(); p != pBelief->getHypotheses().end(); ++p) { + // const Real likelihood_pi = norm*pBelief->density((*p)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); + // // the gain is saved in a vector + // y.push_back(likelihood_pi - likelihood_p1); + // } + // } + //} + + + CriticalSection cs; + Chainspace::Index armIndex = armInfo.getChains().begin(); + ParallelsTask((golem::Parallels*)context.getParallels(), [&] (ParallelsTask*) { + for (Chainspace::Index i = armInfo.getChains().begin();;) { + { + CriticalSectionWrapper csw(cs); + if (armIndex == armInfo.getChains().end()) + break; + i = armIndex++; + } + const ChainDesc* cdesc = getChainDesc()[i]; + if (cdesc->enabledObs) { + // computes the workspace coordinate of the relative chain, if it should be in the obs + const RBCoord c(w.wpos[i]); + // computes the likelihood of contacting the mean pose + const Real likelihood_p1 = norm*pBelief->density((*maxLhdPose)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); + + // computes the likelihood of contacting the other hypotheses + for (Belief::Hypothesis::Seq::const_iterator p = ++pBelief->getHypotheses().begin(); p != pBelief->getHypotheses().end(); ++p) { + const Real likelihood_pi = norm*pBelief->density((*p)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); + // the gain is saved in a vector + CriticalSectionWrapper csw(cs); + y.push_back(likelihood_pi - likelihood_p1); + } + } + } + }); + + //for (Chainspace::Index i = handInfo.getChains().begin(); i != handInfo.getChains().end(); ++i) { + // const ChainDesc* cdesc = getChainDesc()[i]; + // // if enable obs is ON than we compute observations (first for the finger [chain] and then for the single joint in the finger) + // if (cdesc->enabledObs) { + // // compute the observation for the finger tip + // const RBCoord c(w.wpos[i]); + // // computes the likelihood of contacting the mean pose + // const Real likelihood_p1 = norm*pBelief->density((*maxLhdPose)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); + // // std::printf("chain %d pose<%f %f %f> density_sample=%f density_hyp=%f\n", *i, c.p.x, c.p.y, c.p.z, likelihood_pi, likelihood_p1); + // // store the computed observations for the finger tip + // // computes the likelihood of contacting the other hypotheses + // for (Belief::Hypothesis::Seq::const_iterator p = ++pBelief->getHypotheses().begin(); p != pBelief->getHypotheses().end(); ++p) { + // const Real likelihood_pi = norm*pBelief->density((*p)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); + // // the gain is saved in a vector + // y.push_back(likelihood_pi - likelihood_p1); + // } + // } + //} + + + Chainspace::Index handIndex = handInfo.getChains().begin(); + ParallelsTask((golem::Parallels*)context.getParallels(), [&] (ParallelsTask*) { + // evaluated all the joints of the hand + for (Chainspace::Index i = handInfo.getChains().begin();;) { + { + CriticalSectionWrapper csw(cs); + if (handIndex == handInfo.getChains().end()) + break; + i = handIndex++; + } + const ChainDesc* cdesc = getChainDesc()[i]; + // if enable obs is ON than we compute observations (first for the finger [chain] and then for the single joint in the finger) + if (cdesc->enabledObs) { + // compute the observation for the finger tip + const RBCoord c(w.wpos[i]); + // computes the likelihood of contacting the mean pose + const Real likelihood_p1 = norm*pBelief->density((*maxLhdPose)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); + // std::printf("chain %d pose<%f %f %f> density_sample=%f density_hyp=%f\n", *i, c.p.x, c.p.y, c.p.z, likelihood_pi, likelihood_p1); + // store the computed observations for the finger tip + // computes the likelihood of contacting the other hypotheses + for (Belief::Hypothesis::Seq::const_iterator p = ++pBelief->getHypotheses().begin(); p != pBelief->getHypotheses().end(); ++p) { + const Real likelihood_pi = norm*pBelief->density((*p)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); + // the gain is saved in a vector + CriticalSectionWrapper csw(cs); + y.push_back(likelihood_pi - likelihood_p1); + } + } + } + +}); // end parallels task +#endif + //U32 index = 0; + + //const Parallels* parallels = context.getParallels(); + //if (parallels != NULL) { + // const Job* job = parallels->getCurrentJob(); + // if (job != NULL) { + // index = job->getJobId(); + // } + //} + //auto discountedEstimation = [&](const Hypothesis::Seq::const_iterator& ptr, const Waypoint& w, const U32 prev) -> Real { + // const Real next = estimate(ptr, w); + // const Real delta = Math::exp(next - prev); + // return next * (delta > 1 ? 1 : delta); + //}; + //const Real estimate_p1 = estimate(pBelief->getHypotheses().cbegin(), w); + const Real likelihood_p1 = estimate(pBelief->getHypotheses().cbegin(), w); //Math::exp(estimate_p1 - estimates[0]);//pBelief->getHypotheses().front()->evaluate(ftDrivenDesc.evaluationDesc, manipulator->getPose(w.cpos), testCollision); + //hypIndex = 1; + for (auto p = ++pBelief->getHypotheses().cbegin(); p != pBelief->getHypotheses().cend(); ++p) { + //for (auto j = 1; j < pBelief->getHypotheses().size(); ++j) { + //const Real likelihood_pi = pBelief->getHypotheses()[j]->evaluate(ftDrivenDesc.evaluationDesc, manipulator->getPose(w.cpos), testCollision); + // y.push_back(likelihood_pi - likelihood_p1); + //y.push_back(Math::exp(estimates[hypIndex++] - estimate(p, w)) - likelihood_p1); + y.push_back(estimate(p, w) - likelihood_p1); + } + + //for (Belief::Hypothesis::Seq::const_iterator p = ++pBelief->getHypotheses().begin(); p != pBelief->getHypotheses().end(); ++p) { + // const Real likelihood_pi = collision->evaluate(waypoint, (*p)->getCloud(), golem::Rand(rand), manipulator->getPose(w.cpos), testCollision); + // y.push_back(likelihood_pi - likelihood_p1); + //} + } +} + +Real FTDrivenHeuristic::evaluate(const Hypothesis::Seq::const_iterator &hypothesis, const golem::Waypoint &w) const { + Real eval = REAL_ZERO; + golem::Controller::State state = manipulator->getController().createState(); + manipulator->getController().lookupState(golem::SEC_TM_REAL_MAX, state); + // position control + state.cpos = w.cpos; + grasp::Manipulator::Config config(w.cpos); + if (intersect(manipulator->getBounds(config.config, config.frame.toMat34()), (*hypothesis)->bounds(), false)) + eval = (*hypothesis)->evaluate(ftDrivenDesc.evaluationDesc, config.config); + else { + const Real dist = config.frame.toMat34().p.distance((*hypothesis)->toRBPoseSampleGF().p); + if (dist < ftDrivenDesc.ftModelDesc.distMax) + eval = dist; + } + return eval; +} + +//------------------------------------------------------------------------------ + +Real FTDrivenHeuristic::directionApproach(const golem::Waypoint &w) const { + const Chainspace::Index armIndex = armInfo.getChains().begin(); + + Vec3 v; + Mat34 tcpFrameInv, hypothesis; + hypothesis = (*pBelief->getHypotheses().begin())->toRBPoseSampleGF().toMat34(); + tcpFrameInv.setInverse(w.wpos[armIndex]); + tcpFrameInv.multiply(v, hypothesis.p); + v.normalise(); + return v.z > 0 ? REAL_ONE - ftDrivenDesc.directionFac : REAL_ONE; +} + +//------------------------------------------------------------------------------ + +bool FTDrivenHeuristic::collides(const golem::Waypoint &w, ThreadData* data) const { +#ifdef _HEURISTIC_PERFMON + ++perfCollisionWaypoint; +#endif + // check for collisions with the object to grasp. only the hand + grasp::Manipulator::Config config(w.cpos); + if (pointCloudCollision && !pBelief->getHypotheses().empty()) { + Hypothesis::Seq::const_iterator maxLhdPose = pBelief->getHypotheses().begin(); + if (intersect(manipulator->getBounds(config.config, config.frame.toMat34()), (*maxLhdPose)->bounds(), false)) { +#ifdef _HEURISTIC_PERFMON + ++perfCollisionPointCloud; +#endif + //if ((*maxLhdPose)->check(waypoint, manipulator->getPose(w.cpos))) + if ((*maxLhdPose)->check(ftDrivenDesc.checkDesc, rand, config.config)) + return true; + } +// } + } + + return Heuristic::collides(w, data); +} + +bool FTDrivenHeuristic::collides(const golem::Waypoint &w0, const golem::Waypoint &w1, ThreadData* data) const { + const Real dist = getDist(w0, w1); + const U32 size = (U32)Math::round(dist / desc.collisionDesc.pathDistDelta) + 1; + Real p[2]; + golem::Waypoint w; + +#ifdef _HEURISTIC_PERFMON + ++perfCollisionPath; + perfCollisionSegs += size - 1; +#endif + + // test for collisions in the range (w0, w1) - excluding w0 and w1 + for (U32 i = 1; i < size; ++i) { + p[0] = Real(i) / Real(size); + p[1] = REAL_ONE - p[0]; + + // lineary interpolate coordinates + for (Configspace::Index j = stateInfo.getJoints().begin(); j < stateInfo.getJoints().end(); ++j) + w.cpos[j] = p[0] * w0.cpos[j] + p[1] * w1.cpos[j]; + + // skip reference pose computation + w.setup(controller, false, true); + + // test for collisions + if (collides(w, data)) + return true; + } + + return false; +} + +//------------------------------------------------------------------------------ + +bool FTDrivenHeuristic::collides(const golem::Waypoint &w) const { + if (!desc.collisionDesc.enabled && !pointCloudCollision) + return false; + + // find data pointer for the current thread + ThreadData* data = getThreadData(); + + return collides(w, data); +} + +bool FTDrivenHeuristic::collides(const golem::Waypoint &w0, const golem::Waypoint &w1) const { + if (!desc.collisionDesc.enabled && !pointCloudCollision) + return false; + + // find data pointer for the current thread + ThreadData* data = getThreadData(); + + return collides(w0, w1, data); +} + + +//------------------------------------------------------------------------------ + +golem::Real FTDrivenHeuristic::getCollisionCost(const golem::Waypoint &wi, const golem::Waypoint &wj, Hypothesis::Seq::const_iterator p) const { + // Create the normal estimation class, and pass the input dataset to it + //pcl::KdTree::PointCloudConstPtr cloud = p->second->pTree->getInputCloud(); + //pcl::NormalEstimation ne; + //ne.setInputCloud(cloud); + + //// Create an empty kdtree representation, and pass it to the normal estimation object. + //// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given). + //pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); + //ne.setSearchMethod(tree); + + //// Output datasets + //pcl::PointCloud::Ptr cloudNormals(new pcl::PointCloud); + + //// Use all neighbors in a sphere of radius 3cm + //ne.setRadiusSearch (0.01); + + //// Compute the features + //ne.compute(*cloudNormals); // compute the normal at the closest point of the surface + auto kernel = [&](Real x, Real lambda) -> Real { + return /*lambda**/golem::Math::exp(-lambda*x); + }; + + auto density = [&](const Real dist) -> Real { + return (dist > ftDrivenDesc.ftModelDesc.distMax) ? REAL_ZERO : (dist < REAL_EPS) ? kernel(REAL_EPS, ftDrivenDesc.ftModelDesc.lambda) : kernel(dist, ftDrivenDesc.ftModelDesc.lambda); // esponential up to norm factor + }; + + Hypothesis::Seq::const_iterator maxLhdPose = pBelief->getHypotheses().begin(); + const Real norm = (REAL_ONE - density(ftDrivenDesc.ftModelDesc.distMax/*pBelief->myDesc.sensory.sensoryRange*/));//const Real norm = //(ftDrivenDesc.ftModelDesc.enabledLikelihood)?/*REAL_ONE/*/(REAL_ONE - kernel(ftDrivenDesc.ftModelDesc.lambda*ftDrivenDesc.ftModelDesc.distMax)):REAL_ONE; + Real threshold(0.01), cost(REAL_ZERO); + for (Chainspace::Index i = handInfo.getChains().begin(); i < handInfo.getChains().end(); ++i) { + const RBCoord c(wj.wpos[i]); + + const Real d = 0;// (*maxLhdPose)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces); +// context.write("FTDrivenHeuristic::getCollisionCost(): distance=%4.5f, cost=%4.5f\n", d, d < threshold ? Node::COST_INF : REAL_ZERO); + // penalise collisions with the shape of the mean pose + if (d < threshold) + return golem::Node::COST_INF; + + //Real distMin(REAL_MAX), dist; + //size_t k = 0, kMin; + //for (; k != cloud->size(); ++k) { + // if (dist = c.p.distance(Vec3(cloud->points[k].x, cloud->points[k].y, cloud->points[k].z)) < distMin) { + // distMin = dist; + // kMin = k; + // } + //} + // compute the cost relative to the approaching direction + //Vec3 approachDir = wi.wpos[i].p - wj.wpos[i].p; + //Vec3 normal(cloudNormals->points[kMin].normal[0], cloudNormals->points[kMin].normal[1], cloudNormals->points[kMin].normal[2]); + //cost += normal.dot(approachDir); + } + + return cost; +} + +Real FTDrivenHeuristic::testObservations(const grasp::RBCoord &pose, const bool normal) const { +// const Real norm = (ftDrivenDesc.ftModelDesc.enabledLikelihood)?/*REAL_ONE/*/(REAL_ONE - density(ftDrivenDesc.ftModelDesc.distMax)):REAL_ONE; + auto kernel = [&](Real x, Real lambda) -> Real { + return /*lambda**/golem::Math::exp(-lambda*x); + }; + + auto density = [&](const Real dist) -> Real { + return (dist > ftDrivenDesc.ftModelDesc.distMax) ? REAL_ZERO : (dist < REAL_EPS) ? kernel(REAL_EPS, ftDrivenDesc.ftModelDesc.lambda) : kernel(dist, ftDrivenDesc.ftModelDesc.lambda); // esponential up to norm factor + }; + const Real norm = (REAL_ONE - density(ftDrivenDesc.ftModelDesc.distMax));//(ftDrivenDesc.ftModelDesc.enabledLikelihood)?/*REAL_ONE/*/(REAL_ONE - kernel(ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.lambda)):REAL_ONE; + pcl::PointXYZ searchPoint; + searchPoint.x = (float)pose.p.x; + searchPoint.y = (float)pose.p.y; + searchPoint.z = (float)pose.p.z; + + std::vector indeces; + std::vector distances; +// pcl::KdTree::PointCloudConstPtr cloud = (*pBelief->getHypotheses().begin())->pTree->getInputCloud(); + Real result = REAL_ZERO; + Vec3 median; + median.setZero(); + //if ((*pBelief->getHypotheses().begin())->pTree->nearestKSearch(searchPoint, ftDrivenDesc.ftModelDesc.k, indeces, distances) > 0) { + // for (size_t i = 0; i < indeces.size(); ++i) { + // //Point point; + // //point.frame.setId(); + // //point.frame.p.set(cloud->points[indeces[i]].x, cloud->points[indeces[i]].y, cloud->points[indeces[i]].z); + // //result += pose.p.distance(point.frame.p); + // //median += Vec3(cloud->points[indeces[i]].x, cloud->points[indeces[i]].y, cloud->points[indeces[i]].z); + // result += pose.p.distance(Vec3(cloud->points[indeces[i]].x, cloud->points[indeces[i]].y, cloud->points[indeces[i]].z)); + // median += Vec3(cloud->points[indeces[i]].x, cloud->points[indeces[i]].y, cloud->points[indeces[i]].z); + // } + // result /= indeces.size(); + // median /= indeces.size(); + //} + + context.write("FTDrivenHeuristic:testObs(normal=%s):\n", normal ? "ON" : "OFF"); + if (normal) { + Vec3 v; + Mat34 jointFrame(Mat33(pose.q), pose.p); + Mat34 jointFrameInv; + jointFrameInv.setInverse(jointFrame); + jointFrameInv.multiply(v, (*pBelief->getHypotheses().begin())->sample.p); + v.normalise(); +// const Real thx(Math::atan2(v.z, v.x)), thy(Math::atan2(v.z, v.y)); +// if (v.z < 0 || Math::abs(thx) > ftDrivenDesc.ftModelDesc.coneTheta1 || Math::abs(thy) > ftDrivenDesc.ftModelDesc.coneTheta2) + if (v.z < 0 /*|| v.z < ftDrivenDesc.ftModelDesc.coneTheta1*/) + result = ftDrivenDesc.ftModelDesc.distMax + 1; + + context.write("joint pose <%f %f %f> mug frame <%f %f %f> v <%f %f %f> distance=%f result=%f density=%f\n", + pose.p.x, pose.p.y, pose.p.z, (*pBelief->getHypotheses().begin())->sample.p.x, (*pBelief->getHypotheses().begin())->sample.p.y, (*pBelief->getHypotheses().begin())->sample.p.z, + v.x, v.y, v.z, pose.p.distance(median), result, norm*density(result)); + //else { + // Mat33 rot; + // Real roll, pitch, yaw; + // Quat quat(pose.q); + // quat.toMat33(rot); + // rot.toEuler(roll, pitch, yaw); + // std::printf("nearest(): pose [%.4f,%.4f,%.4f]<%.4f,%.4f,%.4f>; median <%.4f,%.4f,%.4f>, thx %.4f, thy %.4f\n", + // roll, pitch, yaw, pose.p.x, pose.p.y, pose.p.z, median.x, median.y, median.z, thx, thy); + //} + } + else { + context.write("joint pose <%f %f %f> mug frame <%f %f %f> distance=%f result=%f density=%f\n", + pose.p.x, pose.p.y, pose.p.z, (*pBelief->getHypotheses().begin())->sample.p.x, (*pBelief->getHypotheses().begin())->sample.p.y, (*pBelief->getHypotheses().begin())->sample.p.z, + pose.p.distance(median), result, norm*density(result)); + } + return result; +} + + +//------------------------------------------------------------------------------ + +void spam::XMLData(FTDrivenHeuristic::FTModelDesc& val, golem::XMLContext* context, bool create) { + golem::XMLData("dist_max", val.distMax, context, create); + golem::XMLData("cone_theta_orizontal_axis", val.coneTheta1, context, create); + golem::XMLData("cone_theta_vertical_axis", val.coneTheta2, context, create); + golem::XMLData("num_nearest_points", val.k, context, create); + golem::XMLData("num_points", val.points, context, create); + golem::XMLData("mahalanobis_fac", val.mahalanobisDistFac, context, create); + //golem::XMLData("enable_arm_chain", val.enabledArmChain, context, create); + //golem::XMLData("enable_hand_joints", val.enabledHandJoints, context, create); + // golem::XMLData("enabled_steps", val.enabledSteps, context, create); + golem::XMLData("enabled_likelihood", val.enabledLikelihood, context, create); + golem::XMLData("intrinsic_exp_parameter", val.lambda, context, create); + golem::XMLData(val.jointFac, context->getContextFirst("joint_fac"), create); +} + +void spam::XMLData(FTDrivenHeuristic::Desc& val, golem::XMLContext* context, bool create) { + // golem::XMLData((golem::Heuristic::Desc)val, context, create); + // spam::XMLData(val.beliefDesc, context->getContextFirst("belief_space"), create); + golem::XMLData("contact_fac", val.contactFac, context, create); + golem::XMLData("non_contact_fac", val.nonContactFac, context, create); + golem::XMLData("max_surface_points_inkd", val.maxSurfacePoints, context, create); + spam::XMLData(val.ftModelDesc, context->getContextFirst("ftmodel"), create); + golem::XMLData(&val.covariance[0], &val.covariance[grasp::RBCoord::N], "c", context->getContextFirst("covariance"), create); + XMLData(val.evaluationDesc, context->getContextFirst("evaluation_model"), create); + XMLData(val.checkDesc, context->getContextFirst("check_model"), create); +} diff --git a/src/Spam/HBPlan/Hypothesis.cpp b/src/Spam/HBPlan/Hypothesis.cpp new file mode 100644 index 0000000..533d48a --- /dev/null +++ b/src/Spam/HBPlan/Hypothesis.cpp @@ -0,0 +1,127 @@ +/** @file Data.cpp + * + * @author Claudio Zito (The University Of Birmingham) + * + * @version 1.0 + * + */ + +#include + +#ifdef WIN32 +#pragma warning (push) +#pragma warning (disable : 4291 4244 4996 4305) +#endif +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef WIN32 +#pragma warning (pop) +#endif + +//------------------------------------------------------------------------------ + +using namespace golem; +using namespace spam; + +//------------------------------------------------------------------------------ + +void Hypothesis::BoundsAppearance::draw(const golem::Bounds::Seq& bounds, golem::DebugRenderer& renderer) const { + if (showSolid) { + renderer.setColour(solidColour); + renderer.addSolid(bounds.begin(), bounds.end()); + } + if (showWire) { + renderer.setColour(wireColour); + renderer.setLineWidth(wireWidth); + renderer.addWire(bounds.begin(), bounds.end()); + } +} + +Bounds::Seq Hypothesis::bounds() { + Bounds::Seq bounds; + bounds.push_back(boundsDesc.create()); + return bounds; +} + +//------------------------------------------------------------------------------ + +Hypothesis::Hypothesis(const grasp::Manipulator& manipulator, const Desc& desc) : manipulator(manipulator), desc(desc) { + if (!desc.isValid()) + throw Message(Message::LEVEL_CRIT, "Hypothesis(): invalid description"); + collisionPtr = desc.collisionDescPtr->create(manipulator); +} + +//------------------------------------------------------------------------------ + +void Hypothesis::create(const golem::U32 idx, const golem::Mat34& trn, const grasp::RBPose::Sample& s, golem::Rand& rand, const grasp::Cloud::PointSeq& points) { + collisionPtr->create(rand, points); + index = idx; + modelFrame = trn; + sample = s; + Real x = REAL_ZERO, y = REAL_ZERO, z = REAL_ZERO; + Vec3 sampleFrame = toRBPoseSampleGF().p; + for (grasp::Cloud::PointSeq::const_iterator i = points.begin(); i != points.end(); ++i) { + this->points.push_back(*i); + // calculate max x, y, z values for bounding box + if (x < Math::abs(sampleFrame.x - (float)i->x)) + x = Math::abs(sampleFrame.x - (float)i->x); + if (y < Math::abs(sampleFrame.y - (float)i->y)) + y = Math::abs(sampleFrame.y - (float)i->y); + if (z < Math::abs(sampleFrame.z - (float)i->z)) + z = Math::abs(sampleFrame.z - (float)i->z); + } +// printf("Bounding box dim x=%f, y=%f, z=%f\n", x, y, z); + // set dimension of the bounding box + boundsDesc.dimensions = Vec3(x, y, z); + boundsDesc.pose.p = toRBPoseSampleGF().p; +// printf("Bounding box dim x=%f, y=%f, z=%f, pose=<%f, %f, %f>\n", boundsDesc.dimensions.x, boundsDesc.dimensions.y, boundsDesc.dimensions.z, boundsDesc.pose.p.x, boundsDesc.pose.p.y, boundsDesc.pose.p.z); +} + +void Hypothesis::draw(DebugRenderer &renderer) const { + printf("Belief::Hypothesis::draw(showFrame=%s, showPoints=%s)\n", appearance.showFrames ? "ON" : "OFF", appearance.showPoints ? "ON" : "OFF"); + if (appearance.showFrames || true) + renderer.addAxes(sample.toMat34() * modelFrame, appearance.frameSize); + + size_t t = 0; + if (appearance.showPoints || true) { + for (grasp::Cloud::PointSeq::const_iterator i = points.begin(); i != points.end(); ++i) { + grasp::Cloud::Point point = *i; + //if (++t < 10) printf("Belief::Hypothesis point %d <%.4f %.4f %.4f>\n", t, point.x, point.y, point.z); + grasp::Cloud::setColour(/*appearance.colour*/RGBA::RED, point); + renderer.addPoint(grasp::Cloud::getPoint(point)); + } + } +} + +//------------------------------------------------------------------------------ + +std::string Hypothesis::str() const { + std::stringstream ss; + const grasp::RBPose::Sample pose = toRBPoseSampleGF(); + ss << pose.p.x << "\t" << pose.p.y << "\t" << pose.p.z << "\t" << pose.q.w << "\t" << pose.q.x << "\t" << pose.q.y << "\t" << pose.q.z; + return ss.str(); +} + +//------------------------------------------------------------------------------ + +void Hypothesis::draw(const Collision::Waypoint &waypoint, const grasp::Manipulator::Config& config, golem::DebugRenderer& renderer) const { + collisionPtr->draw(waypoint, config, renderer); +} + +void Hypothesis::draw(golem::DebugRenderer& renderer, const golem::Rand& rand, const grasp::Manipulator::Config& config) const { + collisionPtr->draw(renderer, rand, config, desc.collisionDescPtr->flannDesc); +} + +//------------------------------------------------------------------------------ + +void spam::XMLData(spam::Hypothesis::Desc& val, golem::XMLContext* xmlcontext, bool create) { + spam::XMLData(*val.collisionDescPtr, xmlcontext->getContextFirst("collision"), create); +} + +//------------------------------------------------------------------------------ \ No newline at end of file diff --git a/src/Spam/ShapePlanner/ShapePlanner.cpp b/src/Spam/ShapePlanner/ShapePlanner.cpp deleted file mode 100644 index 6ee2353..0000000 --- a/src/Spam/ShapePlanner/ShapePlanner.cpp +++ /dev/null @@ -1,950 +0,0 @@ -/** @file ShapePlanner.cpp - * - * @author Claudio Zito - * - * @version 1.0 - * - */ - -#include -#include -#include -#include -#include -#include -#include - -using namespace golem; -using namespace spam; - -//------------------------------------------------------------------------------ - -void spam::ShapePlanner::Data::xmlData(golem::XMLContext* context, bool create) const { - PosePlanner::Data::xmlData(context, create); - - try { - if (!create || !graspConfigs.empty()) { - { - const std::string name("grasp_config"); - create ? xmlDataSave(context->getContextFirst(name.c_str(), create), grasp::makeString("%s%s%s%s", getName().c_str(), sepName.c_str(), name.c_str(), extGraspConfig.c_str()), graspConfigs) : xmlDataLoad(context->getContextFirst(name.c_str(), create), this->path, const_cast(graspConfigs)); - } - { - const std::string name("grasp_cluster"); - create ? xmlDataSave(context->getContextFirst(name.c_str(), create), grasp::makeString("%s%s%s%s", getName().c_str(), sepName.c_str(), name.c_str(), extGraspCluster.c_str()), graspClusters) : xmlDataLoad(context->getContextFirst(name.c_str(), create), this->path, const_cast(graspClusters)); - } - } - } - catch (const golem::MsgXMLParser& msg) { - if (create) - throw msg; - } -} - -grasp::Director::Data::Ptr spam::ShapePlanner::createData() const { - return Data::Ptr(new Data(*grasp::to(data))); // assuming data has been initialised properly -} - -void spam::ShapePlanner::Data::setGraspData(const grasp::Grasp::Map& map, grasp::Grasp::Data::Map::const_iterator data) { - golem::U32 n = 0; - - // search for index - for (grasp::Grasp::Map::const_iterator i = map.begin(); i != map.end(); ++i) - for (grasp::Grasp::Data::Map::const_iterator j = i->second->getDataMap().begin(); j != i->second->getDataMap().end(); ++j, ++n) - if (j == data) { - graspDataPtr = n; - return; - } - - throw Message(Message::LEVEL_ERROR, "ShapePlanner::Data::setGraspData(): unable to find grasp data pointer"); -} - -void spam::ShapePlanner::Data::printGraspInfo(const grasp::Manipulator& manipulator) const { - if (!hasGraspConfigs()) { - manipulator.getContext().write("No grasp configs available\n"); - return; - } - - const grasp::Grasp::Config& graspConfig = **getGraspConfig(); - const grasp::Manipulator::Pose& graspPose = graspConfig.path.getGrip(); - - std::stringstream str; - for (size_t i = 0; i < manipulator.getJoints(); ++i) - str << " c" << (i + 1) << "=\"" << graspPose.jc[i] << "\""; - - //manipulator.getContext().write("\n", frame.p.x, frame.p.y, frame.p.z, frame.q.q0, frame.q.q1, frame.q.q2, frame.q.q3, robot->getStateInfo().getJoints().size(), str.str().c_str()); - //manipulator.getContext().write("Grasp config: type=%s, cluster=%d/%d, config=%d/%d, likelihood_{value=%f, active=%f, inactive=%f, contacts=%f, config=%f, collision=%f}, valid=%s\n", - // graspConfig.type.c_str(), - // graspClusterPtr + 1, (golem::U32)graspClusters.size(), graspConfigPtr + 1, (graspClusters[graspClusterPtr].end - graspClusters[graspClusterPtr].begin), - // graspConfig.likelihood.value, graspConfig.likelihood.valueActive, graspConfig.likelihood.valueInactive, graspConfig.likelihood.getActiveContactsAverageValue(), graspConfig.likelihood.config, graspConfig.likelihood.collision, - // graspConfig.getGrasp() ? graspConfig.likelihood.isValid(graspConfig.getGrasp()->getDesc().optimisationDesc->epsilon) ? "yes" : "no" : "unknown"); -} - -//------------------------------------------------------------------------------ - -const char* spam::ShapePlanner::ModeName[MODE_SIZE + 1] = { "Disabled", "Training data", "Cluster", "Config", "Config Model", "Contact Model", }; - -spam::ShapePlanner::ShapePlanner(Scene &scene) : PosePlanner(scene) { -} - -bool spam::ShapePlanner::create(const Desc& desc) { - PosePlanner::create(desc); // throws - - manipulator = desc.manipulatorDesc->create(*robot->getPlanner()); - clusterDesc = desc.clusterDesc; - - demoMap = desc.demoMap; - - extGraspClass = desc.extGraspClass; - - grasp::to(data)->appearanceData = desc.appearanceData; - grasp::to(data)->appearanceConfig = desc.appearanceConfig; - - classifierDescMap = desc.classifierDescMap; - classifier = load(*classifierDescMap.begin()->second); - - resetDataPointers(); - - scene.getHelp().insert(Scene::StrMapVal("090", " G grasp operations\n")); - scene.getHelp().insert(Scene::StrMapVal("091", " 7 grasp mode\n")); - scene.getHelp().insert(Scene::StrMapVal("091", " 8 grasp training features\n")); - scene.getHelp().insert(Scene::StrMapVal("091", " 9 grasp training path\n")); - scene.getHelp().insert(Scene::StrMapVal("092", " grasp path subspace\n")); - scene.getHelp().insert(Scene::StrMapVal("092", " () grasp mode index\n")); - - return true; -} - -void spam::ShapePlanner::render() { - PosePlanner::render(); - - golem::CriticalSectionWrapper csw(csDataRenderer); - graspRenderer.render(); -} - -void spam::ShapePlanner::mouseHandler(int button, int state, int x, int y) { - grasp::Player::mouseHandler(button, state, x, y); - - if (state != 1 || !(button == 1 || button == 3 || button == 4)) return; - - grasp::Configuration::Path::Appearance* appearance = - grasp::to(currentDataPtr)->graspMode == MODE_DATA ? & grasp::to(currentDataPtr)->appearanceData.path : - grasp::to(currentDataPtr)->graspMode == MODE_CLUSTER || grasp::to(currentDataPtr)->graspMode == MODE_CONFIG || grasp::to(currentDataPtr)->graspMode == MODE_CONFIG_MODEL || grasp::to(currentDataPtr)->graspMode == MODE_CONTACT_MODEL ? & grasp::to(currentDataPtr)->appearanceConfig.configPath : - nullptr; - - if (!appearance) return; - - appearance->subspaceDist = button == 3 ? appearance->subspaceDist + 1 : button == 4 ? appearance->subspaceDist - 1 : 0; - - auto printPath = [=](const char* name, const grasp::Configuration::Path::Appearance& appearance, const grasp::Configuration::Path& path) { - std::stringstream str; - const golem::ConfigspaceCoord& c = manipulator->getConfig(appearance.subspacePose); - for (golem::Configspace::Index i = robot->getStateInfo().getJoints().begin(); i < robot->getStateInfo().getJoints().end(); ++i) - str << " c" << (*i - *robot->getStateInfo().getJoints().begin() + 1) << "=\"" << c[i] << "\""; - context.write("%s: size=%u, dist_{first=%f, last=%f, curr=%f}, dist_range_{lo=%f, hi=%f}, \n", name, path.size(), path.front().getDistance(), path.back().getDistance(), appearance.subspaceDistVal, appearance.subspaceDistLo, appearance.subspaceDistHi, robot->getStateInfo().getJoints().size(), str.str().c_str()); - }; - - golem::CriticalSectionWrapper csw(csDataRenderer); - // data pointer - switch (grasp::to(currentDataPtr)->graspMode) { - case MODE_DATA: - { - grasp::Grasp::Map::const_iterator grasp; - grasp::Grasp::Data::Map::const_iterator data; - if (grasp::to(currentDataPtr)->getGraspData(classifier->getGraspMap(), grasp, data)) { - graspRenderer.reset(); - grasp::to(currentDataPtr)->appearanceData.path.pathDelta = grasp->second->getConfiguration()->getDesc().distanceStdDev; - data->second.draw(*grasp->second, grasp::to(currentDataPtr)->appearanceData, graspRenderer); - printPath("Training data path", grasp::to(currentDataPtr)->appearanceData.path, data->second.path); - } - break; - } - case MODE_CLUSTER: - case MODE_CONFIG: - case MODE_CONFIG_MODEL: - case MODE_CONTACT_MODEL: - if (grasp::to(currentDataPtr)->hasGraspConfigs()) { - graspRenderer.reset(); - grasp::Grasp::Config::Seq::const_iterator ptr = grasp::to(currentDataPtr)->getGraspConfig(); - - if ((*ptr)->getGrasp() && (grasp::to(currentDataPtr)->graspMode == MODE_CLUSTER || grasp::to(currentDataPtr)->graspMode == MODE_CONFIG)) - grasp::to(currentDataPtr)->appearanceConfig.configPath.pathDelta = (*ptr)->getGrasp()->getConfiguration()->getDesc().distanceStdDev; - -// (*ptr)->draw(*manipulator, grasp::to(currentDataPtr)->appearanceConfig, graspRenderer); - - if (grasp::to(currentDataPtr)->graspMode == MODE_CLUSTER || grasp::to(currentDataPtr)->graspMode == MODE_CONFIG) - printPath("Grasp config path", grasp::to(currentDataPtr)->appearanceConfig.configPath, (*ptr)->path); - } - else - context.write("Grasp configs not available!\n"); - break; - }; -} - -//------------------------------------------------------------------------------ - -void spam::ShapePlanner::renderData(Data::Map::const_iterator dataPtr) { - PosePlanner::renderData(dataPtr); - - golem::CriticalSectionWrapper csw(csDataRenderer); - graspRenderer.reset(); - - if (grasp::to(dataPtr)->graspMode == MODE_DISABLED) - return; - - if (grasp::to(dataPtr)->graspMode == MODE_DATA) { - grasp::Grasp::Map::const_iterator grasp; - grasp::Grasp::Data::Map::const_iterator data; - if (grasp::to(dataPtr)->getGraspData(classifier->getGraspMap(), grasp, data)) { - grasp::to(currentDataPtr)->appearanceData.path.pathDelta = grasp->second->getConfiguration()->getDesc().distanceStdDev; - data->second.draw(*grasp->second, grasp::to(dataPtr)->appearanceData, graspRenderer); - context.write("Training data: estimator=%s, type=%s, name=%s\n", grasp->second->getName().c_str(), grasp->first.c_str(), data->first.c_str()); - } - else - context.write("Training data not available!\n"); - } - else if (!grasp::to(dataPtr)->hasGraspConfigs()) - context.write("Grasp configs not available!\n"); - else { - grasp::Grasp::Config::Seq::const_iterator ptr = grasp::to(dataPtr)->getGraspConfig(); - const grasp::Grasp* grasp = grasp::to(dataPtr)->hasGraspConfigs() ? (*ptr)->getGrasp() : nullptr; - - grasp::to(dataPtr)->appearanceConfig.showConfig = false; - grasp::to(dataPtr)->appearanceConfig.showModelConfig = false; - grasp::to(dataPtr)->appearanceConfig.showModelContact = false; - - // data pointer - switch (grasp::to(dataPtr)->graspMode) { - case MODE_CLUSTER: - case MODE_CONFIG: - grasp::to(dataPtr)->appearanceConfig.showConfig = true; - grasp::to(dataPtr)->printGraspInfo(*manipulator); - grasp = nullptr; - break; - case MODE_CONFIG_MODEL: - grasp::to(dataPtr)->appearanceConfig.showModelConfig = true; - break; - case MODE_CONTACT_MODEL: - grasp::to(dataPtr)->appearanceConfig.showModelContact = true; - break; - }; - - if (grasp) { - U32& modelSelectionIndex = grasp::to(dataPtr)->appearanceConfig.modelSelectionIndex; - if (modelSelectionIndex > grasp->getContacts().size()) modelSelectionIndex = grasp->getContacts().size(); - const grasp::Contact* contact = modelSelectionIndex < grasp->getContacts().size() && !grasp->getContacts()[modelSelectionIndex]->getPoses().empty() ? grasp->getContacts()[modelSelectionIndex] : nullptr; - if (contact) { - grasp::to(dataPtr)->appearanceData.path.manipulator.selectionIndex = grasp::to(dataPtr)->appearanceConfig.configPath.manipulator.selectionIndex = grasp::to(dataPtr)->appearanceConfig.modelManipulator.selectionIndex = (U32)contact->getTypeDesc().index > manipulator->getJoints() ? manipulator->getJoints() : (U32)contact->getTypeDesc().index; - context.write("Contact index=%u: type=%s, name=%s, joint=%u, size_{model=%u, query=%u}\n", modelSelectionIndex + 1, grasp->getType().c_str(), contact->getTypeDesc().getName().c_str(), contact->getTypeDesc().index + 1, contact->getFeatures().size(), contact->getPoses().size()); - } - else { - grasp::to(dataPtr)->appearanceData.path.manipulator.selectionIndex = grasp::to(dataPtr)->appearanceConfig.configPath.manipulator.selectionIndex = grasp::to(dataPtr)->appearanceConfig.modelManipulator.selectionIndex = (U32)-1; - if (modelSelectionIndex < grasp->getContacts().size()) - context.write("Contact index=%u: disabled\n", modelSelectionIndex + 1); - else - context.write("Contact: disabled\n"); - } - } - - (*ptr)->draw(*manipulator, grasp::to(dataPtr)->appearanceConfig, rand, graspRenderer); - } - -// golem::CriticalSectionWrapper csw(csDataRenderer); -// graspRenderer.reset(); -// -// // data pointer -// switch (grasp::to(dataPtr)->graspMode) { -// case MODE_DATA: -// { -// grasp::Grasp::Map::const_iterator grasp; -// grasp::Grasp::Data::Map::const_iterator data; -// if (grasp::to(dataPtr)->getGraspData(classifier->getGraspMap(), grasp, data)) { -// grasp::to(currentDataPtr)->appearanceData.path.pathDelta = grasp->second->getConfiguration()->getDesc().distanceStdDev; -// data->second.draw(*grasp->second, grasp::to(dataPtr)->appearanceData, graspRenderer); -// context.write("Training data: estimator=%s, type=%s, name=%s\n", grasp->second->getName().c_str(), grasp->first.c_str(), data->first.c_str()); -// } -// else -// context.write("Training data not available!\n"); -// break; -// } -// case MODE_CLUSTER: -// case MODE_CONFIG: -// case MODE_CONFIG_MODEL: -// case MODE_CONTACT_MODEL: -// if (grasp::to(dataPtr)->hasGraspConfigs()) { -// grasp::Grasp::Config::Seq::const_iterator ptr = grasp::to(dataPtr)->getGraspConfig(); -// -// grasp::to(dataPtr)->appearanceConfig.showConfig = false; -// grasp::to(dataPtr)->appearanceConfig.showModelConfig = false; -// grasp::to(dataPtr)->appearanceConfig.showModelContact = false; -// switch (grasp::to(dataPtr)->graspMode) { -// case MODE_CLUSTER: -// case MODE_CONFIG: -// grasp::to(dataPtr)->appearanceConfig.showConfig = true; -// grasp::to(dataPtr)->printGraspInfo(*manipulator); -// break; -// case MODE_CONFIG_MODEL: -// grasp::to(dataPtr)->appearanceConfig.showModelConfig = true; -// break; -// case MODE_CONTACT_MODEL: -// grasp::to(dataPtr)->appearanceConfig.showModelContact = true; -// const grasp::Grasp* grasp = (*ptr)->getGrasp(); -// if (grasp) { -// U32& ptr = grasp::to(dataPtr)->appearanceConfig.modelSelectionIndex; -// while (ptr < grasp->getContacts().size() && grasp->getContacts()[ptr]->getPoses().empty()) ++ptr; -// if (ptr > grasp->getContacts().size()) ptr = grasp->getContacts().size(); -// const grasp::Contact* contact = ptr < grasp->getContacts().size() ? grasp->getContacts()[ptr] : nullptr; -// if (contact) -// context.write("Contact: type=%s, name=%s, size_{model=%u, query=%u}\n", grasp->getType().c_str(), contact->getTypeDesc().getName().c_str(), contact->getFeatures().size(), contact->getPoses().size()); -// else -// context.write("Contact: disabled\n"); -// } -// break; -// }; -// -//// (*ptr)->draw(*manipulator, grasp::to(dataPtr)->appearanceConfig, graspRenderer); -// } -// else -// context.write("Grasp configs not available!\n"); -// break; -// }; -} - -void spam::ShapePlanner::resetDataPointers() { - PosePlanner::resetDataPointers(); -} - -//------------------------------------------------------------------------------ - -grasp::Classifier::Ptr spam::ShapePlanner::load(const grasp::Classifier::Desc& desc) { - // create classifier - grasp::Classifier::Ptr classifier(desc.create(*manipulator)); - - // load data - const std::string path = grasp::makeString("%s%s", desc.name.c_str(), extGraspClass.c_str()); - try { - FileReadStream(path.c_str()) >> (golem::Serializable&)*classifier; - context.info("ShapePlanner::load(): Classifier data loaded from: %s\n", path.c_str()); - } - catch (const std::exception&) { - context.warning("ShapePlanner::load(): Unable to load classifier data from: %s\n", path.c_str()); - } - - return classifier; -} - -void spam::ShapePlanner::run(const Demo::Map::value_type& demo) { -#ifdef _GRASP_CAMERA_DEPTH - auto breakPoint = [&]() { if (waitKey(10) == 27) throw Cancel("Demo cancelled"); }; - - context.write("================================================================================================================================\nRunning demo %s\n", demo.first.c_str()); - - // manipulation trajectory - golem::Controller::State::Seq trajectory; - - { - grasp::ScopeGuard cleanup([&]() { - grasp::to(currentDataPtr)->graspMode = MODE_DISABLED; - renderData(currentDataPtr); - }); - - grasp::Cloud::RawPointSeqVal val; - val.first = grasp::to(currentDataPtr)->labelMap.empty() ? grasp::Cloud::LABEL_OBJECT : grasp::to(currentDataPtr)->labelMap.rbegin()->first + 1; - const grasp::Cloud::RawPointSeqMultiMap::iterator points = grasp::to(currentDataPtr)->insertPoints(val, demo.first); - grasp::Cloud::RawPointSeq prev, next; - grasp::Cloud::RawPointSeqQueue rawPointSeqQueue(cloudDesc.filterDesc.window); - - // capture depth image - for (Demo::ObjectDetection::Seq::const_iterator j = demo.second.objectDetectionSeq.begin(); j != demo.second.objectDetectionSeq.end(); ++j) { - // go to object detection pose - gotoPose(j->scanPose); - breakPoint(); - grasp::RobotPose robotPose; - getPose(robotPose); - - // find camera - grasp::Recorder* recorder = nullptr; - grasp::DepthCamera* camera = nullptr; - for (grasp::Recorder::Seq::iterator i = cameraSeq.begin(); i != cameraSeq.end(); ++i) - if ((*i)->getCamera()->getName() == j->cameraName) { - recorder = i->get(); - camera = recorder->getDepthCamera(); - break; - } - if (!camera) - throw Message(Message::LEVEL_ERROR, "ShapePlanner::run(): unknown camera %s", j->cameraName.c_str()); - // set camera frame - robotPose.w.multiply(camera->getCalibration()->getDeformation(robotPose), robotPose.w); - Mat34 cameraFrame = camera->getCalibration()->getParameters().pose; - if (recorder->isHand()) cameraFrame.multiply(robotPose.w, cameraFrame); - camera->setFrame(cameraFrame); - // Object region in global coordinates - golem::Bounds::Seq objectRegion; - for (golem::Bounds::Desc::Seq::const_iterator i = cloudDesc.objectRegionDesc.begin(); i != cloudDesc.objectRegionDesc.end(); ++i) - objectRegion.push_back((*i)->create()); - // Move bounds to camera frame - Mat34 cameraFrameInv; - cameraFrameInv.setInverse(cameraFrame); - for (golem::Bounds::Seq::const_iterator i = objectRegion.begin(); i != objectRegion.end(); ++i) - (*i)->multiplyPose(cameraFrameInv, (*i)->getPose()); - // start camera - grasp::ScopeGuard guardCamera([&]() { camera->set(grasp::Camera::CMD_STOP); }); - camera->setProperty(camera->getDepthProperty()); - camera->set(grasp::Camera::CMD_VIDEO); - // detect changes - grasp::Image::Ptr image; - rawPointSeqQueue.clear(); - prev.clear(); - U32 prevSize = 0; - for (bool accept = false; !accept;) { - breakPoint(); - // grab image - camera->peek_back(image); - grasp::Image::assertImageData(image->dataDepth); - try { - grasp::Cloud::copy(objectRegion, false, *image->dataDepth, next); - } - catch (const Message&) {} - grasp::Cloud::transform(cameraFrame, next, next); - grasp::Cloud::setSensorFrame(cameraFrame, next); - rawPointSeqQueue.push_back(next); - if (!rawPointSeqQueue.full()) - continue; - grasp::Cloud::filter(context, cloudDesc.filterDesc, rawPointSeqQueue, next); - // render points - { - golem::CriticalSectionWrapper csw(csDataRenderer); - pointRenderer.reset(); - grasp::to(currentDataPtr)->draw(next, pointRenderer); - } - // count points - U32 nextSize = 0; - for (grasp::Cloud::RawPointSeq::const_iterator i = next.begin(); i != next.end(); ++i) - if (!grasp::Cloud::isNanXYZ(*i)) - ++nextSize; - if (nextSize < j->minSize) - continue; - //context.debug("ShapePlanner::run(): object detected (size=%u)\n", nextSize); - if (prev.size() == next.size()) { - size_t sharedSize = 0; - Real deltaDepth = REAL_ZERO; - for (size_t i = 0; i < next.size(); ++i) { - const grasp::Cloud::RawPoint p = prev[i], n = next[i]; - if (grasp::Cloud::isNanXYZ(p) || grasp::Cloud::isNanXYZ(n)) - continue; - ++sharedSize; - deltaDepth += Math::sqrt(Math::sqr(p.x - n.x) + Math::sqr(p.y - n.y) + Math::sqr(p.z - n.z)); - } - if (nextSize - sharedSize < j->deltaSize && deltaDepth / sharedSize < j->deltaDepth) { - context.write("Object detected (size=%u, delta_size=%u, delta_depth=%f)\n", nextSize, nextSize - sharedSize, deltaDepth / sharedSize); - grasp::Cloud::nanRem(context, next, grasp::Cloud::isNanXYZ); // required by flann in linux - grasp::Cloud::normal(context, cloudDesc.normal, next, next); - grasp::Cloud::nanRem(context, next, grasp::Cloud::isNanXYZNormal); - points->second.insert(points->second.end(), next.begin(), next.end()); - accept = true; - // render - renderData(currentDataPtr); - } - } - if (!accept) { - prev = next; - prevSize = nextSize; - } - } - } - - breakPoint(); - - // find approach trajectory - golem::Real err = REAL_MAX; - golem::Controller::State::Seq approach; - if (demo.second.poseEstimation) { - // load model - grasp::Cloud::RawPointSeq modelPoints; - Data::pcdRead(demo.second.modelObject, demo.second.modelObject, modelPoints); - pRBPose->createModel(modelPoints); - // estimate pose - breakPoint(); - pRBPose->createQuery(points->second); - const Mat34 queryTransform = pRBPose->maximum().toMat34(); - grasp::Cloud::transform(queryTransform, modelPoints, modelPoints); - points->second.insert(points->second.end(), modelPoints.begin(), modelPoints.end()); - // render - renderData(currentDataPtr); - // find feasible trajectory - for (grasp::StringSeq::const_iterator i = demo.second.modelTrajectories.begin(); i != demo.second.modelTrajectories.end() && err > REAL_ONE; ++i) { - breakPoint(); - grasp::RobotState::Seq raw; - golem::FileReadStream frs(i->c_str()); - frs.read(raw, raw.end(), grasp::RobotState(*robot->getController())); - golem::Controller::State::Seq inp = grasp::RobotState::makeCommand(raw); - // choose first with acceptable error - approach.clear(); - const grasp::RBDist dist = robot->transformTrajectory(queryTransform, inp.begin(), inp.end(), approach); - err = manipulator->getDesc().trajectoryErr.dot(dist); - context.write("%s: Trajectory error: lin=%.9f, ang=%.9f, total=%.9f\n", i->c_str(), dist.lin, dist.ang, err); - } - } - else { - grasp::Classifier::Desc::Map::const_iterator ptr = classifierDescMap.find(demo.second.classifierName); - if (ptr == classifierDescMap.end()) - throw Message(Message::LEVEL_ERROR, "ShapePlanner::run(): unknown classifier %s", demo.second.classifierName.c_str()); - classifier = load(*ptr->second); - // estimate curvatures - grasp::Cloud::PointSeq features; - grasp::Cloud::copy(Bounds::Seq(), false, points->second, features); - grasp::Cloud::curvature(context, cloudDesc.curvature, features); - grasp::Cloud::nanRem(context, features, grasp::Cloud::isNanXYZNormalCurvature); - // estimate grasp - classifier->find(features, grasp::to(currentDataPtr)->graspConfigs); - grasp::Cluster::findLikelihood(clusterDesc, grasp::to(currentDataPtr)->graspConfigs, grasp::to(currentDataPtr)->graspClusters); - // display - grasp::to(currentDataPtr)->resetDataPointers(); - grasp::to(currentDataPtr)->graspMode = MODE_CONFIG; - // trajectory - CollisionBounds collisionBounds(*this, true); - golem::U32 size = std::min(grasp::to(currentDataPtr)->getGraspConfigSize() - grasp::to(currentDataPtr)->graspConfigPtr, manipulator->getDesc().trajectoryClusterSize), j = grasp::to(currentDataPtr)->graspConfigPtr; - const std::pair val = manipulator->find(grasp::to(currentDataPtr)->getGraspConfigBegin(), grasp::to(currentDataPtr)->getGraspConfigBegin() + size, [&](const grasp::Grasp::Config::Ptr& config) -> grasp::RBDistEx { - breakPoint(); - grasp::to(currentDataPtr)->graspConfigPtr = j++; - renderData(currentDataPtr); - const grasp::RBDist dist(manipulator->find(config->path)); - const grasp::RBDistEx distex(dist, manipulator->getDesc().trajectoryErr.collision && collisionBounds.collides(manipulator->getConfig(config->path.getApproach()))); - err = manipulator->getDesc().trajectoryErr.dot(dist); - context.write("#%03u: Trajectory error: lin=%.9f, ang=%.9f, collision=%s, total=%f\n", j, distex.lin, distex.ang, distex.collision ? "yes" : "no", err); - return distex; - }); - manipulator->copy((*val.first)->path, approach); - grasp::to(currentDataPtr)->graspConfigPtr = val.first - grasp::to(currentDataPtr)->getGraspConfigBegin(); - context.write("#%03u: Best trajectory\n", grasp::to(currentDataPtr)->graspConfigPtr + 1); - renderData(currentDataPtr); - } - if (err > REAL_ONE) { - context.write("No feasible trajectory has been found\n"); - return; - } - grasp::to(currentDataPtr)->trajectory[demo.first] = grasp::RobotState::make(manipulator->getController(), approach); // update trajectory collection - - // overwrite not used joints - const grasp::RealSeq& cpos = demo.second.objectDetectionSeq.back().scanPose.c; - for (golem::Controller::State::Seq::iterator i = approach.begin(); i != approach.end(); ++i) - i->cpos.set(cpos.data() + manipulator->getJoints(), cpos.data() + cpos.size(), manipulator->getJoints()); - - // find manipulation trajectory - createWithManipulation(approach, trajectory, true); - breakPoint(); - - // collision bounds - CollisionBounds collisionBounds(*this, points->second); - - // perform - try { - perform(demo.first, trajectory, true); - } - catch (const Message& msg) { - context.write("%s\n", msg.str().c_str()); - return; - } - breakPoint(); - - // cleanup - } - - // goto remaining poses - for (Robot::Pose::Seq::const_iterator i = demo.second.poses.begin(); i != demo.second.poses.end(); ++i) { - grasp::RobotPose pose = *i; - // use hand configuration - const Real* c = pose.flags == "open" ? trajectory.front().cpos.data() : pose.flags == "close" ? trajectory.back().cpos.data() : nullptr; - if (c) std::copy(c + manipulator->getArmJoints(), c + manipulator->getJoints(), pose.c.data() + manipulator->getArmJoints()); - // go to pose - gotoPose(pose); - breakPoint(); - } - -#endif -} - -void spam::ShapePlanner::function(Data::Map::iterator& dataPtr, int key) { - switch (key) { - case 'G': - { - switch (waitKey("LSARECUBXOD", "Press a key to (L)oad/(S)ave classifier, (A)dd/(R)emove training data, (E)stimate/(C)luster grasp, find c(U)rrent/(B)est trajectory, e(X)port data, c(O)llision test, run (D)emo...")) { - case 'L': - classifier = load(*select(classifierDescMap.begin(), classifierDescMap.end(), "Available classifiers:\n", [](grasp::Classifier::Desc::Map::const_iterator ptr) -> const std::string&{ - return ptr->first; - })->second); - break; - case 'S': - { - const std::string path = grasp::makeString("%s%s", classifier->getName().c_str(), extGraspClass.c_str()); - // save to file - context.write("Saving training data to: %s\n", path.c_str()); - FileWriteStream fws(path.c_str()); - fws << (const golem::Serializable&)*classifier; - break; - } - case 'A': - { - // point cloud - const grasp::Cloud::PointSeq& points = queryDataPtr != dataPtr || grasp::to(queryDataPtr)->queryPoints.empty() ? getPoints(dataPtr)->second : grasp::to(queryDataPtr)->queryPoints; - // trajectory - const grasp::RobotState::Map::const_iterator trajectory = selectTrajectory(grasp::to(dataPtr)->trajectory.begin(), grasp::to(dataPtr)->trajectory.end()); - // grasp type - const grasp::Grasp::Map::const_iterator grasp = select(classifier->getGraspMap().begin(), classifier->getGraspMap().end(), "Available grasp types:\n", [](grasp::Grasp::Map::const_iterator ptr) -> const std::string&{ - return ptr->first; - }); - // training data name - std::string name = grasp::to(dataPtr)->getName() + '_' + grasp->first; - readString("Enter training data name: ", name); - - // parameters - grasp::ParameterGuard distanceScale(grasp->second->getDesc().configurationDesc->distanceScale); - readNumber("Enter distance scale: ", distanceScale); - - // add - context.write("Adding training data...\n"); - grasp::to(dataPtr)->setGraspData(classifier->getGraspMap(), classifier->add(grasp->first, name, points, grasp::RobotState::makeCommand(trajectory->second), 0)); - - // display - grasp::to(dataPtr)->resetDataPointers(); - grasp::to(dataPtr)->graspMode = MODE_DATA; - grasp::to(dataPtr)->appearanceData.featureRelation = grasp::Feature::RELATION_DFLT; - renderData(dataPtr); - break; - } - case 'R': - if (grasp::to(dataPtr)->graspMode == MODE_DATA) { - grasp::Grasp::Map::iterator grasp; - grasp::Grasp::Data::Map::iterator data; - if (grasp::to(dataPtr)->getGraspData(classifier->getGraspMap(), grasp, data)) { - context.write("Removing training data: estimator=%s, type=%s, name=%s\n", grasp->second->getName().c_str(), grasp->first.c_str(), data->first.c_str()); - grasp->second->getDataMap().erase(data); - renderData(dataPtr); - } - } - else - context.write("Select %s mode\n", ModeName[(size_t)MODE_DATA]); - break; - case 'E': - { - // available point cloud - const grasp::Cloud::PointSeq& points = queryDataPtr != dataPtr || grasp::to(queryDataPtr)->queryPoints.empty() ? getPoints(dataPtr)->second : grasp::to(queryDataPtr)->queryPoints; - // available grasp types - grasp::StringSet availables; - for (grasp::Grasp::Map::const_iterator i = classifier->getGraspMap().begin(); i != classifier->getGraspMap().end(); ++i) - if (!i->second->getDataMap().empty()) - availables.insert(i->first); - if (availables.empty()) - throw Message(Message::LEVEL_NOTICE, "No available grasp estimators with training data!"); - - grasp::StringSet types; - try { - types.insert(*select(availables.begin(), availables.end(), "Select grasp type:\n [0] all types\n", [](grasp::StringSet::const_iterator ptr) -> const std::string&{ return *ptr; }, -1)); - } - catch (const Message&) {} - - /*ParameterSeqGuard kernels(grasp.second->getContacts().begin(), grasp.second->getContacts().end(), [] (Contact* contact) -> U32* { return &contact->getDesc().kernels; }); - readNumber("Enter kernels scale: ", kernels); - kernels.update(); - ParameterSeqGuard samples(grasp.second->getContacts().begin(), grasp.second->getContacts().end(), [] (Contact* contact) -> U32* { return &contact->getDesc().samples; }); - readNumber("Enter samples scale: ", samples); - samples.update(); - ParameterGuard runs(grasp.second->getDesc().optimisationDesc->runs); - readNumber("Enter runs: ", runs); - ParameterGuard steps(grasp.second->getDesc().optimisationDesc->steps); - readNumber("Enter steps: ", steps);*/ - - context.write("Estimating grasp...\n"); - classifier->find(points, grasp::to(dataPtr)->graspConfigs, &types); - - grasp::to(dataPtr)->resetDataPointers(); - grasp::to(dataPtr)->graspMode = MODE_CONFIG; - - if (types.empty()) - grasp::Cluster::findLikelihood(clusterDesc, grasp::to(dataPtr)->graspConfigs, grasp::to(dataPtr)->graspClusters); - else { - grasp::Cluster::findType(clusterDesc, grasp::to(dataPtr)->graspConfigs, grasp::to(dataPtr)->graspClusters); - while (grasp::to(dataPtr)->graspClusterPtr < grasp::to(dataPtr)->graspClusters.size() && grasp::to(dataPtr)->getGraspConfig()->get()->type != *types.begin()) ++grasp::to(dataPtr)->graspClusterPtr; - } - - // display - renderData(dataPtr); - break; - } - case 'C': - { - if (!grasp::to(dataPtr)->hasGraspConfigs()) - throw Message(Message::LEVEL_ERROR, "No grasp data!"); - switch (waitKey("TLC", "Sort by (T)ype/(L)ikelihood, (C)luster...")) { - case 'T': - grasp::Cluster::findType(clusterDesc, grasp::to(dataPtr)->graspConfigs, grasp::to(dataPtr)->graspClusters); - grasp::to(dataPtr)->resetDataPointers(); - grasp::to(dataPtr)->graspMode = MODE_CLUSTER; - break; - case 'L': - grasp::Cluster::findLikelihood(clusterDesc, grasp::to(dataPtr)->graspConfigs, grasp::to(dataPtr)->graspClusters); - grasp::to(dataPtr)->resetDataPointers(); - grasp::to(dataPtr)->graspMode = MODE_CONFIG; - break; - case 'C': { - Real radius = clusterDesc.radius; - readNumber("Enter cluster radius: ", radius); - size_t density = clusterDesc.density; - readNumber("Enter cluster density: ", density); - grasp::Cluster::findConfiguration(clusterDesc, grasp::to(dataPtr)->graspConfigs, grasp::to(dataPtr)->graspClusters, radius, density); - grasp::to(dataPtr)->resetDataPointers(); - grasp::to(dataPtr)->graspMode = MODE_CLUSTER; - break; - } - }; - // display - renderData(dataPtr); - break; - } - case 'U': - { - if (!grasp::to(dataPtr)->hasGraspConfigs()) - throw Message(Message::LEVEL_ERROR, "No grasp data!"); - if (grasp::to(dataPtr)->graspMode == MODE_CLUSTER || grasp::to(dataPtr)->graspMode == MODE_CONFIG) { - CollisionBounds collisionBounds(*this, true); - grasp::Grasp::Config* config = grasp::to(dataPtr)->getGraspConfig()->get(); - const grasp::RBDist dist(manipulator->find(config->path)); - const grasp::RBDistEx distex(dist, manipulator->getDesc().trajectoryErr.collision && collisionBounds.collides(manipulator->getConfig(config->path.getApproach()))); - context.write("#%03u: Trajectory error: lin=%.9f, ang=%.9f, collision=%s\n", grasp::to(dataPtr)->graspConfigPtr + 1, distex.lin, distex.ang, distex.collision ? "yes" : "no"); - Controller::State::Seq trajectory; - manipulator->copy((*grasp::to(dataPtr)->getGraspConfig())->path, trajectory); - std::string name = (*grasp::to(dataPtr)->getGraspConfig())->type; - readString("Enter trajectory name: ", name); - grasp::to(dataPtr)->trajectory[name] = grasp::RobotState::make(manipulator->getController(), trajectory); // update trajectory collection - } - else - context.write("Select %s mode\n", ModeName[(size_t)MODE_CONFIG]); - break; - } - case 'B': - { - if (!grasp::to(dataPtr)->hasGraspConfigs()) - throw Message(Message::LEVEL_ERROR, "No grasp data!"); - if (grasp::to(dataPtr)->graspMode == MODE_CLUSTER || grasp::to(dataPtr)->graspMode == MODE_CONFIG) { - const golem::U32 graspConfigPtr = grasp::to(dataPtr)->graspConfigPtr; - const grasp::Grasp::Config::Seq::const_iterator begin = grasp::to(dataPtr)->getGraspConfigBegin() + grasp::to(dataPtr)->graspConfigPtr; - const grasp::Grasp::Config::Seq::const_iterator end = begin + std::min(grasp::to(dataPtr)->getGraspConfigSize() - grasp::to(dataPtr)->graspConfigPtr, manipulator->getDesc().trajectoryClusterSize); - golem::U32 j = grasp::to(currentDataPtr)->graspConfigPtr; - CollisionBounds collisionBounds(*this, true); - const std::pair val = manipulator->find(begin, end, [&](const grasp::Grasp::Config::Ptr& config) -> grasp::RBDistEx { - grasp::to(currentDataPtr)->graspConfigPtr = j++; - renderData(currentDataPtr); - const grasp::RBDist dist(manipulator->find(config->path)); - const grasp::RBDistEx distex(dist, manipulator->getDesc().trajectoryErr.collision && collisionBounds.collides(manipulator->getConfig(config->path.getApproach()))); - context.write("#%03u: Trajectory error: lin=%.9f, ang=%.9f, collision=%s\n", j, distex.lin, distex.ang, distex.collision ? "yes" : "no"); - return distex; - }); - grasp::to(dataPtr)->graspConfigPtr = graspConfigPtr + val.first - begin; - context.write("#%03u: Best trajectory\n", grasp::to(dataPtr)->graspConfigPtr + 1); - renderData(dataPtr); - Controller::State::Seq trajectory; - manipulator->copy((*val.first)->path, trajectory); - std::string name = (*val.first)->type; - readString("Enter trajectory name: ", name); - grasp::to(dataPtr)->trajectory[name] = grasp::RobotState::make(manipulator->getController(), trajectory); // update trajectory collection - } - else - context.write("Select %s mode\n", ModeName[(size_t)MODE_CONFIG]); - break; - } - /*case 'X': - { - const char SEP = ' '; - size_t n = 0; - std::for_each(grasp.second->getContacts().begin(), grasp.second->getContacts().end(), [&] (const Contact* contact) { - ++n; - if (contact->getFeatures().empty()) return; - context.debug("Writing contact distribution #%d\n", n); - // model distribution - std::ofstream mfile(makeString("%s%s_model_%02d_%05d.txt", data->dir.c_str(), dataPtr->first.c_str(), n, contact->getFeatures().size())); - mfile << std::scientific << "#" << SEP << "weight" << SEP << "p.x" << SEP << "p.y" << SEP << "p.z" << SEP << "q.x" << SEP << "q.y" << SEP << "q.z" << SEP << "q.w" << SEP << "r.x" << SEP << "r.y" << std::endl; - std::for_each(contact->getFeatures().begin(), contact->getFeatures().end(), [&] (const Feature& feature) { - mfile << feature.weight << SEP << feature.frame.p.x << SEP << feature.frame.p.y << SEP << feature.frame.p.z << SEP << feature.frame.q.x << SEP << feature.frame.q.y << SEP << feature.frame.q.z << SEP << feature.frame.q.w << SEP << feature.curvature.x << SEP << feature.curvature.y << std::endl; - }); - // query distribution - std::ofstream qfile(makeString("%s%s_query_%02d_%05d.txt", data->dir.c_str(), dataPtr->first.c_str(), n, contact->getPoses().size())); - qfile << std::scientific << "#" << SEP << "weight" << SEP << "p.x" << SEP << "p.y" << SEP << "p.z" << SEP << "q.x" << SEP << "q.y" << SEP << "q.z" << SEP << "q.w" << std::endl; - std::for_each(contact->getPoses().begin(), contact->getPoses().end(), [&] (const Contact::Pose& pose) { - qfile << pose.weight << SEP << pose.p.x << SEP << pose.p.y << SEP << pose.p.z << SEP << pose.q.x << SEP << pose.q.y << SEP << pose.q.z << SEP << pose.q.w << std::endl; - }); - }); - break; - }*/ - case 'D': - while (!demoMap.empty()) - for (Demo::Map::const_iterator i = demoMap.begin(); i != demoMap.end(); ++i) - run(*i); - break; - } - context.write("Done!\n"); - return; - } - case '7': - grasp::to(dataPtr)->graspMode = (grasp::to(dataPtr)->graspMode + 1) % (MODE_SIZE + 1); - context.write("Mode %s\n", ModeName[(size_t)grasp::to(dataPtr)->graspMode]); - renderData(dataPtr); - return; - case '8': - switch (grasp::to(dataPtr)->graspMode) { - case MODE_DATA: - { - grasp::to(dataPtr)->appearanceData.featureRelation = grasp::to(dataPtr)->appearanceData.featureRelation == grasp::Feature::RELATION_VERTEX ? grasp::Feature::RELATION_NONE : (grasp::Feature::Relation)(grasp::to(dataPtr)->appearanceData.featureRelation + 1); - context.write("Training data features %s\n", grasp::Feature::name[(size_t)grasp::to(dataPtr)->appearanceData.featureRelation]); - break; - } - case MODE_CLUSTER: - break; - case MODE_CONFIG: - break; - case MODE_CONFIG_MODEL: - break; - case MODE_CONTACT_MODEL: - break; - }; - renderData(dataPtr); - return; - case '9': - switch (grasp::to(dataPtr)->graspMode) { - case MODE_DATA: - { - grasp::to(dataPtr)->appearanceData.showPath = !grasp::to(dataPtr)->appearanceData.showPath; - context.write("Training data path %s\n", grasp::to(dataPtr)->appearanceData.showPath ? "ON" : "OFF"); - break; - } - case MODE_CLUSTER: - break; - case MODE_CONFIG: - break; - case MODE_CONFIG_MODEL: - break; - case MODE_CONTACT_MODEL: - break; - }; - renderData(dataPtr); - return; - case '(': case ')': - if (grasp::to(dataPtr)->graspMode != MODE_DISABLED && grasp::to(dataPtr)->graspMode != MODE_CONFIG_MODEL) { - U32& ptr = - grasp::to(dataPtr)->graspMode == MODE_DATA ? grasp::to(dataPtr)->graspDataPtr : - grasp::to(dataPtr)->graspMode == MODE_CLUSTER ? grasp::to(dataPtr)->graspClusterPtr : - grasp::to(dataPtr)->graspMode == MODE_CONFIG ? grasp::to(dataPtr)->graspConfigPtr : grasp::to(dataPtr)->appearanceConfig.modelSelectionIndex; - if (key == '(' && ptr > 0) --ptr; - if (key == ')') ++ptr; - - if (grasp::to(dataPtr)->graspMode == MODE_CONTACT_MODEL) { - const grasp::Grasp* grasp = (*grasp::to(dataPtr)->getGraspConfig())->getGrasp(); - while (grasp && key == '(' && ptr > 0 && ptr < grasp->getContacts().size() && grasp->getContacts()[ptr]->getPoses().empty()) --ptr; - } - } - renderData(dataPtr); - return; - // collision debug - case 'O': - { - const grasp::Cloud::PointSeq& points = getPoints(dataPtr)->second; - //Collision collision(context, *manipulator); - //grasp::Manipulator::Pose pose = manipulator->getPose(robot->recvState().config); - //Collision::Waypoint waypoint; - //waypoint.points = 1000000; - //(void)collision.evaluate(waypoint, points, rand, pose, true); - break; - } - } - - PosePlanner::function(dataPtr, key); -} - -//------------------------------------------------------------------------------ - -void spam::XMLData(ShapePlanner::Demo::ObjectDetection &val, golem::XMLContext* xmlcontext, bool create) { - XMLData(val.scanPose, xmlcontext->getContextFirst("scan_pose"), create); - golem::XMLData("camera_name", val.cameraName, xmlcontext, create); - golem::XMLData("min_size", val.minSize, xmlcontext, create); - golem::XMLData("delta_size", val.deltaSize, xmlcontext, create); - golem::XMLData("delta_depth", val.deltaDepth, xmlcontext, create); -} - -void spam::XMLData(ShapePlanner::Demo &val, golem::XMLContext* xmlcontext, bool create) { - golem::XMLData("pose_estimation", val.poseEstimation, xmlcontext, create); - golem::XMLData("classifier_name", val.classifierName, xmlcontext, create); - golem::XMLData("model_object", val.modelObject, xmlcontext, create); - - val.modelTrajectories.clear(); - std::pair range = xmlcontext->getContextMap().equal_range("model_trajectory"); - for (XMLContext::XMLContextMap::const_iterator i = range.first; i != range.second; ++i) { - std::string name; - golem::XMLData("name", name, const_cast(&i->second), create); - val.modelTrajectories.push_back(name); - } - - golem::XMLData(val.objectDetectionSeq, val.objectDetectionSeq.max_size(), xmlcontext, "object_detection", create); - - try { - golem::XMLData(val.poses, val.poses.max_size(), xmlcontext, "pose", create); - } - catch (const golem::MsgXMLParser&) {} -} - -void spam::XMLData(ShapePlanner::Demo::Map::value_type &val, golem::XMLContext* xmlcontext, bool create) { - golem::XMLData("name", const_cast(val.first), xmlcontext, create); - XMLData(val.second, xmlcontext, create); -} - -void spam::XMLData(ShapePlanner::Desc &val, Context* context, XMLContext* xmlcontext, bool create) { - XMLData((PosePlanner::Desc&)val, context, xmlcontext, create); - - xmlcontext = xmlcontext->getContextFirst("shape_planner"); - - val.manipulatorDesc.reset(new grasp::Manipulator::Desc); - grasp::XMLData(*val.manipulatorDesc, xmlcontext->getContextFirst("manipulator"), create); - - val.classifierDescMap.clear(); - golem::XMLData(val.classifierDescMap, val.classifierDescMap.max_size(), xmlcontext, "classifier", create); - golem::XMLData("ext_grasp_class", val.extGraspClass, xmlcontext, create); - - val.demoMap.clear(); - try { - golem::XMLData(val.demoMap, val.demoMap.max_size(), xmlcontext, "demo", create); - } - catch (const golem::MsgXMLParser&) {} - - grasp::XMLData(val.appearanceData, xmlcontext->getContextFirst("appearance data"), create); - grasp::XMLData(val.appearanceConfig, xmlcontext->getContextFirst("appearance"), create); -} - - -//------------------------------------------------------------------------------ - -void ShapePlannerApp::run(int argc, char *argv[]) { - // Setup ShapePlanner - ShapePlanner::Desc shapeHandDesc; - XMLData(shapeHandDesc, context(), xmlcontext()); - - ShapePlanner *pShapePlanner = dynamic_cast(scene()->createObject(shapeHandDesc)); // throws - if (pShapePlanner == nullptr) - throw Message(Message::LEVEL_CRIT, "ShapePlannerApp::run(): Unable to cast to ShapePlanner"); - - // Random number generator seed - context()->info("Random number generator seed %d\n", context()->getRandSeed()._U32[0]); - - try { - pShapePlanner->main(); - } - catch (const grasp::Interrupted&) { - } - - context()->info("Good bye!\n"); - scene()->releaseObject(*pShapePlanner); -}; - -//------------------------------------------------------------------------------ - -#ifdef _SPAM_SHAPEPLANNER_MAIN_ -int main(int argc, char *argv[]) { - return spam::ShapePlannerApp().main(argc, argv); -} -#endif // _SPAM_SHAPEPLANNER_MAIN_ \ No newline at end of file diff --git a/src/Spam/Spam/Data.cpp b/src/Spam/Spam/Data.cpp deleted file mode 100644 index ee2c850..0000000 --- a/src/Spam/Spam/Data.cpp +++ /dev/null @@ -1,229 +0,0 @@ -/** @file Data.cpp - * - * @author Claudio Zito (The University Of Birmingham) - * - * @version 1.0 - * - */ -#include -#include - -#ifdef WIN32 - #pragma warning (push) - #pragma warning (disable : 4244) - #pragma warning (disable : 4996) -#endif -#include -#include -#ifdef WIN32 - #pragma warning (pop) -#endif - -#include - -//------------------------------------------------------------------------------ - -using namespace golem; -using namespace spam; -// -////------------------------------------------------------------------------------ -// -//void spam::XMLData(FTDrivenHeuristic::FTModelDesc& val, golem::XMLContext* context, bool create) { -// golem::XMLData("dist_max", val.distMax, context, create); -// golem::XMLData("cone_theta_orizontal_axis", val.coneTheta1, context, create); -// golem::XMLData("cone_theta_vertical_axis", val.coneTheta2, context, create); -// golem::XMLData("num_nearest_points", val.k, context, create); -// golem::XMLData("num_points", val.points, context, create); -// golem::XMLData("mahalanobis_fac", val.mahalanobisDistFac, context, create); -// //golem::XMLData("enable_arm_chain", val.enabledArmChain, context, create); -// //golem::XMLData("enable_hand_joints", val.enabledHandJoints, context, create); -//// golem::XMLData("enabled_steps", val.enabledSteps, context, create); -// golem::XMLData("enabled_likelihood", val.enabledLikelihood, context, create); -// golem::XMLData("intrinsic_exp_parameter", val.lambda, context, create); -// golem::XMLData(val.jointFac, context->getContextFirst("joint_fac"), create); -//} -// -//void spam::XMLData(FTDrivenHeuristic::Desc& val, golem::XMLContext* context, bool create) { -//// golem::XMLData((golem::Heuristic::Desc)val, context, create); -//// spam::XMLData(val.beliefDesc, context->getContextFirst("belief_space"), create); -// golem::XMLData("contact_fac", val.contactFac, context, create); -// golem::XMLData("non_contact_fac", val.nonContactFac, context, create); -// golem::XMLData("max_surface_points_inkd", val.maxSurfacePoints, context, create); -// spam::XMLData(val.ftModelDesc, context->getContextFirst("ftmodel"), create); -// golem::XMLData(&val.covariance[0], &val.covariance[grasp::RBCoord::N], "c", context->getContextFirst("covariance"), create); -//} - -//------------------------------------------------------------------------------ - -//void spam::XMLData(Robot::Desc &val, golem::Context* context, golem::XMLContext* xmlcontext, bool create) { -//// grasp::XMLData((grasp::Robot::Desc&)val, context, xmlcontext, create); -// RagGraphPlanner::Desc* pRagGraphPlanner(new RagGraphPlanner::Desc); -// (golem::Planner::Desc&)*pRagGraphPlanner = *val.physPlannerDesc->pPlannerDesc; -// val.physPlannerDesc->pPlannerDesc.reset(pRagGraphPlanner); -// val.physPlannerDesc->pPlannerDesc = RagGraphPlanner::Desc::load(context, xmlcontext->getContextFirst("planner")); -// -// FTDrivenHeuristic::Desc* pFTDrivenHeuristic(new FTDrivenHeuristic::Desc); -// (golem::Heuristic::Desc&)*pFTDrivenHeuristic = *val.physPlannerDesc->pPlannerDesc->pHeuristicDesc; -// val.physPlannerDesc->pPlannerDesc->pHeuristicDesc.reset(pFTDrivenHeuristic); -// spam::XMLData((FTDrivenHeuristic::Desc&)*val.physPlannerDesc->pPlannerDesc->pHeuristicDesc, xmlcontext->getContextFirst("rag_planner heuristic"), create); -//} - -//------------------------------------------------------------------------------ - -//void spam::XMLData(RagGraphPlanner::Desc &val, golem::XMLContext* context, bool create) { -// golem::XMLData((golem::GraphPlanner::Desc&)val, context, create); -//} - -//------------------------------------------------------------------------------ - -const char spam::TrialData::headerName [] = "spam::TrialData"; -const golem::U32 spam::TrialData::headerVersion = 1; - -template <> void golem::Stream::read(spam::TrialData& trialData) const { - char name[sizeof(trialData.headerName)]; - read(name, sizeof(trialData.headerName)); - name[sizeof(trialData.headerName) - 1] = '\0'; - if (strncmp(trialData.headerName, name, sizeof(trialData.headerName)) != 0) - throw Message(Message::LEVEL_CRIT, "Stream::read(spam::TrialData&): Unknown file name: %s", name); - - golem::U32 version; - *this >> version; - if (version != trialData.headerVersion) - throw Message(Message::LEVEL_CRIT, "Stream::read(spam::TrialData&): Unknown file version: %d", version); - - //trialData.approachAction.clear(); - //read(trialData.approachAction, trialData.approachAction.begin(), grasp::RobotState(trialData.controller)); - //trialData.manipAction.clear(); - //read(trialData.manipAction, trialData.manipAction.begin(), grasp::RobotState(trialData.controller)); - - //trialData.approachWithdraw.clear(); - //read(trialData.approachWithdraw, trialData.approachWithdraw.begin(), grasp::RobotState(trialData.controller)); - ////trialData.action.clear(); - ////read(trialData.action, trialData.action.begin(), trialData.controller.createState()); - - //trialData.density.clear(); - //read(trialData.density, trialData.density.begin()); - - //trialData.hypotheses.clear(); - //read(trialData.hypotheses, trialData.hypotheses.begin()); -} - -template <> void golem::Stream::write(const spam::TrialData& trialData) { - *this << trialData.headerName << trialData.headerVersion; - - //write(trialData.approachAction.begin(), trialData.approachAction.end()); - //write(trialData.manipAction.begin(), trialData.manipAction.end()); - - //write(trialData.approachWithdraw.begin(), trialData.approachWithdraw.end()); - ////write(trialData.action.begin(), trialData.action.end()); - //write(trialData.density.begin(), trialData.density.end()); - //write(trialData.hypotheses.begin(), trialData.hypotheses.end()); -} - -////------------------------------------------------------------------------------ -// -//void Collision::Bounds::create(const golem::Bounds::Seq& bounds) { -// for (size_t i = 0; i < bounds.size(); ++i) { -// const golem::BoundingConvexMesh* mesh = dynamic_cast(bounds[i].get()); -// if (mesh != nullptr) { -// surfaces.resize(surfaces.size() + 1); -// triangles.resize(triangles.size() + 1); -// surfaces.back().resize(mesh->getTriangles().size()); -// triangles.back().resize(mesh->getTriangles().size()); -// for (size_t j = 0; j < mesh->getTriangles().size(); ++j) { -// triangles.back()[j].normal = surfaces.back()[j].normal = mesh->getNormals()[j]; -// surfaces.back()[j].point = mesh->getVertices()[mesh->getTriangles()[j].t1]; // e.g. first triangle -// //surfaces.back()[j].point = (mesh->getVertices()[mesh->getTriangles()[j].t1] + mesh->getVertices()[mesh->getTriangles()[j].t2] + mesh->getVertices()[mesh->getTriangles()[j].t3])/Real(3.0); // centroid -// triangles.back()[j].distance = mesh->getDistances()[j]; -// } -// } -// } -//} -// -//Collision::Collision(const grasp::Manipulator& manipulator) : manipulator(manipulator) { -// for (U32 i = manipulator.getArmJoints(); i < manipulator.getJoints() + 1; ++i) -// bounds[i].create(i < manipulator.getJoints() ? manipulator.getJointBounds(i) : manipulator.getBaseBounds()); -//} -// -//golem::Real Collision::evaluate(const Waypoint& waypoint, const grasp::Cloud::PointSeq& points, golem::Rand& rand, const grasp::Manipulator::Pose& rbpose, bool debug) { -// const Mat34 pose(rbpose.toMat34()); -// Mat34 poses[grasp::Manipulator::JOINTS]; -// manipulator.getPoses(rbpose, pose, poses); -// -// const size_t size = points.size() < waypoint.points ? points.size() : waypoint.points; -// Real norm = REAL_ZERO; -// Real eval = REAL_ZERO; -// for (U32 i = manipulator.getArmJoints(); i < manipulator.getJoints() + 1; ++i) { -// Bounds& bounds = this->bounds[i]; -// if (bounds.empty()) -// continue; -// -// bounds.setPose(i < manipulator.getJoints() ? poses[i] : pose); -// for (size_t j = 0; j < size; ++j) { -// const grasp::Cloud::Point& point = size < points.size() ? points[size_t(rand.next()) % points.size()] : points[j]; -// const Vec3 p = grasp::Cloud::getPoint(point); -// const Real depth = Math::abs(bounds.getDepth(p)); -// //if (depth > REAL_EPS) -// // printf("depth"); -// eval += Math::exp(-waypoint.depthStdDev*depth); -// } -// norm += REAL_ONE; -// } -// norm = REAL_ONE / (size*norm); -// eval *= norm; -// -// if (debug) -// manipulator.getContext().debug("Collision::evaluate(): points=%d, eval=%f, likelihhod=%f\n", size, eval, waypoint.likelihood*(eval - REAL_ONE)); -// -// return waypoint.likelihood*(eval - REAL_ONE); -//} -// -// -//golem::Real Collision::evaluate(const Robot *robot, const Waypoint& waypoint, const grasp::Cloud::PointSeq& points, golem::Rand& rand, const grasp::Manipulator::Pose& rbpose, std::vector &joints, bool debug) { -// if (points.empty()) -// return REAL_ZERO; -// -// joints.clear(); -// const Mat34 pose(rbpose.toMat34()); -// Mat34 poses[grasp::Manipulator::JOINTS]; -// manipulator.getPoses(rbpose, pose, poses); -// -// const size_t size = points.size() < waypoint.points ? points.size() : waypoint.points; -// Real norm = REAL_ZERO; -// Real eval = REAL_ZERO; -//// for (U32 i = manipulator.getArmJoints(); i < manipulator.getJoints() + 1; ++i) { -// for (Configspace::Index i = robot->getStateHandInfo().getJoints().begin(); i != robot->getStateHandInfo().getJoints().end(); ++i) { -// size_t k = i - robot->getStateArmInfo().getJoints().begin(); -// Bounds& bounds = this->bounds[k]; -// if (bounds.empty()) -// continue; -// -// bounds.setPose(i < manipulator.getJoints() ? poses[k] : pose); -// for (size_t j = 0; j < size; ++j) { -// const grasp::Cloud::Point& point = size < points.size() ? points[size_t(rand.next()) % points.size()] : points[j]; -// const Vec3 p = grasp::Cloud::getPoint(point); -// const Real depth = Math::abs(bounds.getDepth(p)); -// eval += Math::exp(-waypoint.depthStdDev*depth); -// if (depth > REAL_ZERO) { -// // printf("collision\n"); -// joints.push_back(i); -// Vec3 v; -// Mat34 jointFrameInv; -// jointFrameInv.setInverse(pose); -// jointFrameInv.multiply(v, Vec3(point.x, point.y, point.z)); -// v.normalise(); -// return v.z > REAL_ZERO ? -REAL_ONE : REAL_ONE; -// } -// } -// norm += REAL_ONE; -// } -// norm = REAL_ONE / (size*norm); -// eval *= norm; -// -// if (debug) -// manipulator.getContext().debug("Collision::evaluate2(): points=%d, eval=%f, likelihhod=%f joints=%d\n", size, eval, waypoint.likelihood*(eval - REAL_ONE), joints.size()); -// -// return waypoint.likelihood*(eval - REAL_ONE); -//} -// -////------------------------------------------------------------------------------ diff --git a/src/Spam/Spam/Heuristic.cpp b/src/Spam/Spam/Heuristic.cpp deleted file mode 100644 index c934d9a..0000000 --- a/src/Spam/Spam/Heuristic.cpp +++ /dev/null @@ -1,1793 +0,0 @@ -/** @file Heuristic.cpp - * - * @author Claudio Zito (The University Of Birmingham) - * - * @version 1.0 - * - */ - -#include -#include -#include - -//#ifdef WIN32 -// #pragma warning (push) -// #pragma warning (disable : 4244) -// #pragma warning (disable : 4996) -//#endif -//#include -//#include -//#include -//#include -//#include -//#ifdef WIN32 -// #pragma warning (pop) -//#endif - -//------------------------------------------------------------------------------ - -using namespace golem; -using namespace grasp; -using namespace spam; - -//------------------------------------------------------------------------------ - -#ifdef _HEURISTIC_PERFMON -U32 FTDrivenHeuristic::perfCollisionWaypoint; -U32 FTDrivenHeuristic::perfCollisionPath; -U32 FTDrivenHeuristic::perfCollisionGroup; -U32 FTDrivenHeuristic::perfCollisionBounds; -U32 FTDrivenHeuristic::perfCollisionSegs; -U32 FTDrivenHeuristic::perfCollisionPointCloud; -U32 FTDrivenHeuristic::perfBoundDist; -U32 FTDrivenHeuristic::perfH; - -void FTDrivenHeuristic::resetLog() { - perfCollisionWaypoint = 0; - perfCollisionPath = 0; - perfCollisionGroup = 0; - perfCollisionBounds = 0; - perfCollisionSegs = 0; - perfCollisionPointCloud = 0; - perfBoundDist = 0; - perfH = 0; -} - -void FTDrivenHeuristic::writeLog(Context &context, const char *str) { - context.debug( - "%s: collision_{waypoint, path, group, bounds, point cloud, } = {%u, %u, %u, %u, %u, %f}, calls_{getBoundedDist(), h()} = {%u, %u}\n", str, perfCollisionWaypoint, perfCollisionPath, perfCollisionGroup, perfCollisionBounds, perfCollisionPointCloud, perfCollisionPath > 0 ? Real(perfCollisionSegs) / perfCollisionPath : REAL_ZERO, perfBoundDist, perfH - ); -} -#endif - -//------------------------------------------------------------------------------ - -//bool FTDrivenHeuristic::HypSample::build() { -// pTree.reset(new pcl::KdTreeFLANN>); -// -// pcl::PointCloud::Ptr cloud(new pcl::PointCloud); -// cloud->width = points.size(); -// cloud->height = 1; -// cloud->points.resize (cloud->width * cloud->height); -// int j = 0; -// for (Cloud::PointSeq::const_iterator point = points.begin(); point != points.end(); ++point) { -// cloud->points[j].x = (float)point->x; -// cloud->points[j].y = (float)point->y; -// cloud->points[j++].z = (float)point->z; -// } -// pTree->setInputCloud(cloud); -// -// return true; -//} -// -//bool FTDrivenHeuristic::HypSample::buildMesh() { -// pTriangles.reset(new pcl::PolygonMesh); -// // build point cloud form set of points -// pcl::PointCloud::Ptr cloud(new pcl::PointCloud); -// cloud->width = points.size(); -// cloud->height = 1; -// cloud->points.resize(cloud->width * cloud->height); -// int j = 0; -// for (Cloud::PointSeq::const_iterator point = points.begin(); point != points.end(); ++point) { -// cloud->points[j].x = (float)point->x; -// cloud->points[j].y = (float)point->y; -// cloud->points[j++].z = (float)point->z; -// } -// -// // Normal estimation* -// pcl::NormalEstimation n; -// pcl::PointCloud::Ptr normals(new pcl::PointCloud); -// pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); -// tree->setInputCloud(cloud); -// n.setInputCloud(cloud); -// n.setSearchMethod(tree); -// n.setKSearch(20); -// n.compute(*normals); -// //* normals should not contain the point normals + surface curvatures -// -// // Concatenate the XYZ and normal fields* -// pcl::PointCloud::Ptr cloud_with_normals(new pcl::PointCloud); -// pcl::concatenateFields(*cloud, *normals, *cloud_with_normals); -// //* cloud_with_normals = cloud + normals -// -// // Create search tree* -// pcl::search::KdTree::Ptr tree2(new pcl::search::KdTree); -// tree2->setInputCloud(cloud_with_normals); -// -// // Initialize objects -// pcl::GreedyProjectionTriangulation gp3; -// pcl::PolygonMesh triangles; -// -// // Set the maximum distance between connected points (maximum edge length) -// gp3.setSearchRadius(0.025); -// -// // Set typical values for the parameters -// gp3.setMu(2.5); -// gp3.setMaximumNearestNeighbors(1000); -// gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees -// gp3.setMinimumAngle(M_PI/18); // 10 degrees -// gp3.setMaximumAngle(2*M_PI/3); // 120 degrees -// gp3.setNormalConsistency(false); -// -// // Get result -// gp3.setInputCloud(cloud_with_normals); -// gp3.setSearchMethod(tree2); -// gp3.reconstruct(*pTriangles.get()); -// std::printf("HypSample::buildMesh: Triangle vertices %d\n", pTriangles->polygons.size()); -// //for (std::vector::iterator i = pTriangles->polygons.begin(); i != pTriangles->polygons.end(); ++i) -// // std::printf("Vertex n.%d size %d\n", i, i->vertices.size()); -// -// return true; -//} - - -//------------------------------------------------------------------------------ -// -//inline golem::Real getLinearDist(const golem::Vec3& v0, const golem::Vec3& v1) { -// return v0.distance(v1); -//} -// -//inline golem::Real getAngularDist(const golem::Quat& q0, const golem::Quat& q1) { -// const golem::Real d = q0.dot(q1); -// return golem::REAL_ONE - golem::Math::abs(d); -//} -// -//inline golem::Real distance(const golem::Mat34 &wpos, const GraspableObject::Desc &p) { -// golem::Real min = golem::REAL_MAX; -// for (golem::Bounds::Desc::Seq::const_iterator i = p.shapes.begin(); i != p.shapes.end(); ++i) { -// const golem::Real d = getLinearDist(wpos.p, i->get()->pose.p); -// if (d < min) -// min = d; -// } -// return min; -//} -// -//inline golem::Mat34 getClosestShapePose(const golem::Mat34 &wpos, const GraspableObject::Desc &p) { -// golem::Real min = Node::COST_INF; -// golem::Mat34 minPose; -// minPose.setId(); -// for (golem::Bounds::Desc::Seq::const_iterator i = p.shapes.begin(); i != p.shapes.end(); ++i) { -// const golem::Real d = getLinearDist(wpos.p, i->get()->pose.p); -// if (d < min) { -// min = d; -// minPose.p.set(i->get()->pose.p); -// minPose.R.set(i->get()->pose.R); -// } -// } -//// printf("ClosestShapePose() <%.2f %.2f %.2f>\n", minPose.p.x, minPose.p.y, minPose.p.z); -// return minPose; -//} - -inline golem::Vec3 dot(const golem::Vec3 &v, const golem::Vec3 &diagonal) { - golem::Vec3 result; - for (size_t i = 0; i < 3; ++i) - result[i] = v[i] * diagonal[i]; - return result; -} - -void dotInverse(const std::vector& v0, const std::vector& v1, std::vector &result) { - if (v0.size() != v1.size()) - throw MsgIncompatibleVectors(Message::LEVEL_CRIT, "spam::dot(std::vector, std::vector): Invalid vectors size."); -// printf("spam::dotInverse()\n"); - result.reserve(v0.size()); - for (size_t i = 0; i < v0.size(); ++i) { - const Real r = v0[i] * (REAL_ONE/v1[i]); - result.push_back(r); -// printf("result[%d] = %.5f * %.5f = %.5f\n", i, v0[i], (REAL_ONE/v1[i]), r); - } -} - -Real dot(const std::vector& v0, const std::vector& v1) { - if (v0.size() != v1.size()) - throw MsgIncompatibleVectors(Message::LEVEL_CRIT, "spam::dot(std::vector, std::vector): Invalid vectors size."); - - Real result; - result = REAL_ZERO; - for (size_t i = 0; i < v0.size(); ++i) - result += v0[i] * v1[i]; -// printf("dot(v0, v1) %.5f\n", result); - return result; -} - -//inline void toMat34(const RBCoord &c, Mat34 &m) { -// m.p.set(c.p); -// m.R.fromQuat(c.q); -//} - -//inline void generateGamma(const size_t size, const Real &noise, std::vector &gamma) { -// gamma.clear(); -// gamma.reserve(size); -// const Real two_noise = 2*noise; -// for (size_t i = 0; i < size; ++i) -// gamma.push_back(two_noise); -//// std::fill(gamma.begin(), gamma.end(), noiseSqr); -//} -// -//inline Real density(const Real &x, const Real &lambda, const Real bound) { -// if (x > bound) -// return REAL_ZERO; -// -// const Real eta = REAL_ONE/(REAL_ONE - Math::exp(-lambda*bound)); -// return eta*Math::exp(-lambda*x); -//} -// -//}; // namespace spam for static functions -// -//Real spam::density(const RBCoord &x, const RBCoord &mean, const RBCoord &covariance, const Real bound) { -// Real determinant, eta, e; -// const Real dist = getLinearDist(x.p, mean.p); -// if (dist > bound) { -//// printf("density() 0 (||x-p||=%.4f>%.4f)\n", dist, bound); -// return 0; -// } -// RBCoord p = x - mean; -// p.q.setZero(); -// determinant = Math::sqrt(covariance.det()); -// eta = (REAL_ONE/(Math::pow(REAL_2_PI, grasp::RBCoord::N/2.0)*determinant)); -// e = (p/covariance).dot(p); -//// printf("det %.4f, eta %.4f, e %.4f distance lin %.6f ang %.6f\n", determinant, eta, e, getLinearDist(x.p, mean.p), getAngularDist(x.q, mean.q)); -// Real c = eta*Math::exp(-REAL_HALF*e); -//// printf("density() %.10f (||x-p||=%.4f)\n", c, dist); -// return c; -//} - -//------------------------------------------------------------------------------ - -FTDrivenHeuristic::FTDrivenHeuristic(golem::Controller &controller) : Heuristic(controller), rand(context.getRandSeed()) {} - -bool FTDrivenHeuristic::create(const Desc &desc) { -// context.debug("FTDrivenHeuristic::create()...\n"); - Heuristic::create((Heuristic::Desc&)desc); - - // printing of debug -// context.write("contact_fac=%f, max_points=%d, covariance=%f\n", desc.contactFac, desc.maxSurfacePoints, desc.covariance.p[0]); - - ftDrivenDesc = desc; -// context.write("Heuristic params: dflt=%f limits=%f root=%f\n", desc.costDesc.distDfltFac, desc.costDesc.distLimitsFac, desc.costDesc.distRootFac); - ftDrivenDesc.ftModelDesc = desc.ftModelDesc; - //ftDrivenDesc.ftModelDesc.dim = 0; - //for (Chainspace::Index i = stateInfo.getChains().begin(); i < stateInfo.getChains().end(); ++i) { - // const ChainDesc* cdesc = getChainDesc()[i]; - // if (cdesc->enabledObs) - // ++ftDrivenDesc.ftModelDesc.dim; - //} - - // controllers - MultiCtrl *multiCtrl = dynamic_cast(&controller); - if (multiCtrl == NULL) - throw Message(Message::LEVEL_CRIT, "Unknown controller"); - // which has two other controllers: arm + hand - if (multiCtrl->getControllers().size() < 2) - throw Message(Message::LEVEL_CRIT, "Arm and hand controllers expected"); - arm = dynamic_cast(multiCtrl->getControllers()[0]); - hand = dynamic_cast(multiCtrl->getControllers()[1]); - if (arm == NULL || hand == NULL) - throw Message(Message::LEVEL_CRIT, "Robot::create(): Robot requires SingleCtrl"); - - armInfo = arm->getStateInfo(); - handInfo = hand->getStateInfo(); - - jointFac = desc.ftModelDesc.jointFac; - - manipulator.reset(); - - enableUnc = false; - - pointCloudCollision = false; - - collision.reset(); - waypoint.setToDefault(); - waypoint.points = 100000;//ftDrivenDesc.ftModelDesc.points; - - hypothesisBoundsSeq.clear(); - - testCollision = false; - - return true; -} - -void FTDrivenHeuristic::setDesc(const Desc &desc) { -// Heuristic::setDesc((Heuristic::Desc&)desc); -// (Heuristic::Desc&)this->desc = this->Heuristic::desc; -// this->bsDesc.beliefDesc = desc.beliefDesc; - this->ftDrivenDesc = desc; - this->ftDrivenDesc.ftModelDesc = desc.ftModelDesc; -} - -void FTDrivenHeuristic::setDesc(const Heuristic::Desc &desc) { - Heuristic::setDesc(desc); -} - -//------------------------------------------------------------------------------ - -//void FTDrivenHeuristic::setModel(Cloud::PointSeq::const_iterator begin, Cloud::PointSeq::const_iterator end, const Mat34 &transform) { -// //modelPoints.clear(); -// //for (Cloud::PointSeq::const_iterator i = begin; i != end; ++i) { -// // Point point = *i; -// // point.frame.multiply(transform, point.frame); -// // modelPoints.push_back(point); -// //} -// modelPoints.clear(); -// modelPoints.insert(modelPoints.begin(), begin, end); -// Cloud::transform(transform, modelPoints, modelPoints); -//} -// -//void FTDrivenHeuristic::setBeliefState(RBPose::Sample::Seq &samples, const golem::Mat34 &transform) { -// U32 idx = 0; -// this->samples.clear(); -// context.write("Heuristic:setBeliefState(model size = %d, max points = %d): samples: cont_fac = %f\n", modelPoints.size(), ftDrivenDesc.maxSurfacePoints, ftDrivenDesc.contactFac); -// for (RBPose::Sample::Seq::const_iterator s = samples.begin(); s != samples.end(); ++s) { -// const Mat34 sampleFrame(Mat34(s->q, s->p) * transform); -// RBPose::Sample sample(RBCoord(sampleFrame), s->weight, s->cdf); -//// this->samples.push_back(RBPose::Sample(RBCoord(sampleFrame), s->weight, s->cdf)); -// Cloud::PointSeq sampleCloud; -// //for (Point::Seq::const_iterator i = modelPoints.begin(); i != modelPoints.end(); ++i) { -// // Point p = *i; -// // p.frame.multiply(sampleFrame, p.frame); -// // sampleCloud.push_back(p); -// //} -// -// // limits the number of point inserted into the kd-tree to save performance while quering it -// const size_t size = modelPoints.size() < ftDrivenDesc.maxSurfacePoints ? modelPoints.size() : ftDrivenDesc.maxSurfacePoints; -// for (size_t i = 0; i < size; ++i) { -// // CHECK: Points from modelPoints are modified multiple times (loop over samples, then nested loop)! -// -// //Point& p = size < modelPoints.size() ? modelPoints[size_t(rand.next())%modelPoints.size()] : modelPoints[i]; // this is reference! -// //p.frame.multiply(sampleFrame, p.frame); -// //sampleCloud.push_back(p); -// Cloud::Point p = size < modelPoints.size() ? modelPoints[size_t(rand.next())%modelPoints.size()] : modelPoints[i]; // make a copy here -// Cloud::setPoint(sampleFrame * Cloud::getPoint(p), p); -// sampleCloud.push_back(p); -// } -// this->samples.insert(HypSample::Map::value_type(idx, HypSample::Ptr(new HypSample(idx, sample, sampleCloud)))); -// context.write(" - sample n.%d <%.4f %.4f %.4f> <%.4f %.4f %.4f %.4f>\n", idx, sample.p.x, sample.p.y, sample.p.z, sample.q.w, sample.q.x, sample.q.y, sample.q.z); -// idx++; -//// samplePoints.insert(std::map::value_type(idx++, sampleCloud)); -// //pcl::KdTreeFLANN kdtree; -// //kdtree.setInputCloud(cloud); -// //queryPoints.insert(std::map>::value_type(s, kdtree)); -// } -// -// // mean and covariance -// if (!sampleProperties.create(ftDrivenDesc.covariance, samples)) -// throw Message(Message::LEVEL_ERROR, "spam::RBPose::createQuery(): Unable to create mean and covariance for the high dimensional representation"); -// -// context.write("spam::Belief::createQuery(TEST): covariance mfse = {(%f, %f, %f), (%f, %f, %f, %f)}\n", sampleProperties.covariance[0], sampleProperties.covariance[1], sampleProperties.covariance[2], sampleProperties.covariance[3], sampleProperties.covariance[4], sampleProperties.covariance[5], sampleProperties.covariance[6]); -// -// covarianceDet = REAL_ONE; -// for (golem::U32 j = 0; j < RBCoord::N; ++j) -// covarianceDet *= sampleProperties.covariance[j]; -// -// // compute the covariance matrix -// //RBCoord mean, covariance, c; -// //const Real norm = REAL_ONE/samples.size(); -// -// //mean.setZero(); -// //c.setZero(); -// //for (RBPose::Sample::Seq::const_iterator i = samples.begin(); i != samples.end(); ++i) -// // golem::kahanSum(&mean[0], &c[0], &(*i)[0], RBCoord::N); -// //for (golem::U32 j = 0; j < RBCoord::N; ++j) -// // mean[j] *= norm; -// // -// //covariance.setZero(); -// //c.setZero(); -// //for (RBPose::Sample::Seq::const_iterator i = samples.begin(); i != samples.end(); ++i) -// // for (golem::U32 j = 0; j < RBCoord::N; ++j) -// // golem::kahanSum(covariance[j], c[j], golem::Math::sqr((*i)[j] - mean[j])); -// //for (golem::U32 j = 0; j < RBCoord::N; ++j) -// // covariance[j] *= norm*ftDrivenDesc.covariance[j]; -// ////covariance.q.setId(); -// ////covariance.p.set(Vec3(.04, .025, .0001)); -// -// //covarianceDet = REAL_ONE; -// //for (golem::U32 j = 0; j < RBCoord::N; ++j) -// // covarianceDet *= covariance[j]; -// -// //if (covarianceDet < REAL_EPS) -// // covarianceDet = REAL_EPS; -// -// //while (covarianceDet < ftDrivenDesc.covarianceDetMin) -// // covarianceDet *= ftDrivenDesc.covarianceDet; -// //context.debug("FTDrivenHeuristic::setBeliefState: covariance hypothesis = {(%.f, %f, %f) (%f, %f, %f, %f) det=%.10f}\n", covariance[3], covariance[4], covariance[5], covariance[6], covariance[0], covariance[1], covariance[2], covarianceDet); -// -// //for (golem::U32 j = 0; j < RBCoord::N; ++j) { -// // covarianceInv[j] = REAL_ONE/(covariance[j]); -// // covarianceSqrt[j] = Math::sqrt(covariance[j]); -// //} -//} - -//------------------------------------------------------------------------------ - -golem::Real FTDrivenHeuristic::cost(const golem::Waypoint &w, const golem::Waypoint &root, const golem::Waypoint &goal) const { - golem::Real c = golem::REAL_ZERO; - const bool enable = enableUnc && pBelief->getHypotheses().size() > 0; - - if (desc.costDesc.distRootFac > golem::REAL_ZERO) - c += desc.costDesc.distRootFac*getDist(w, root); - -// if (desc.costDesc.distGoalFac > golem::REAL_ZERO) { - const Real dist = (enable)?getMahalanobisDist(w, goal):getWorkspaceDist(w, goal); - c += /*desc.costDesc.distGoalFac**/dist; -// c += desc.costDesc.distGoalFac*getMahalanobisDist(w, goal); getWorkspaceDist -// context.getMessageStream()->write(Message::LEVEL_DEBUG, "MyHeuristic::cost(w, root, goal) goal <%.2f, %.2f, %.2f>\n", goal.wpos[chainArm].p.x, goal.wpos[chainArm].p.y, goal.wpos[chainArm].p.z); -// } - - if (desc.costDesc.distLimitsFac > golem::REAL_ZERO) - c += desc.costDesc.distLimitsFac*getConfigspaceLimitsDist(w.cpos); - if (desc.costDesc.distDfltFac > golem::REAL_ZERO) - c += desc.costDesc.distDfltFac*getConfigspaceDist(w.cpos, dfltPos); - - return c; -} - -Real FTDrivenHeuristic::cost(const golem::Waypoint &w0, const golem::Waypoint &w1) const { -// SecTmReal init = context.getTimer().elapsed(); - //if (collides(w0, w1)) - // return Node::COST_INF; - Real c = REAL_ZERO, d; - const bool enable = enableUnc && (pBelief.get() && pBelief->getHypotheses().size() > 0); - - const Chainspace::Index chainArm = stateInfo.getChains().begin(); -// if (desc.costDesc.distPathFac > REAL_ZERO) { - d = Heuristic::getBoundedDist(w0, w1); - if (d >= Node::COST_INF) - return Node::COST_INF; - - //context.debug("FTHeuristic::cost(w0 <%.2f %.2f %.2f>, w1 <%.2f %.2f %.2f>, root <%.2f %.2f %.2f>, goal <%.2f %.2f %.2f>\n", - // w0.wpos[chainArm].p.x, w0.wpos[chainArm].p.y, w0.wpos[chainArm].p.z, w1.wpos[chainArm].p.x, w1.wpos[chainArm].p.y, w1.wpos[chainArm].p.z, - // root.wpos[chainArm].p.x, root.wpos[chainArm].p.y, root.wpos[chainArm].p.z, goal.wpos[chainArm].p.x, root.wpos[chainArm].p.y, root.wpos[chainArm].p.z); -// const Real bound = getBoundedDist(w1); - const Real r = (enable && getBoundedDist(w1) < Node::COST_INF) ? expectedObservationCost(w0, w1) : 1;//(d < 5*bsDesc.ftModelDesc.tactileRange)?getObservationalCost(w0, w1):1; -// if (r < 1) context.write("rewarded trajectory dist=%f reward %f (enalbe=%s)\n", bound < Node::COST_INF ? bound : -1, r, enable ? "ON" : "OFF"); - c += /*desc.costDesc.distPathFac**/d*r; -// } - // rewards more natural approaches (palm towards object) -// c *= directionApproach(w1); - - if (desc.costDesc.distLimitsFac > REAL_ZERO) - c += desc.costDesc.distLimitsFac*getConfigspaceLimitsDist(w1.cpos); - if (desc.costDesc.distDfltFac > REAL_ZERO) - c += desc.costDesc.distDfltFac*getConfigspaceDist(w1.cpos, dfltPos); - - //if (getWorkspaceDist(w1, goal) < ftDrivenDesc.ftModelDesc.distMax) - // c += getCollisionCost(w0, w1, samples.begin()); - - //context.debug(" bounded distance %.4f\n", d); - //context.debug(" Euclid. distance %.4f\n", getWorkspaceDist(w1, goal)); - //context.debug(" Mahala. distance %.4f\n", w); - //context.debug(" total cost func. %.4f\n\n", c); - -// context.debug("FTDrivenHeuristic::cost(enable=%s): preforming time %.7f\n", enableUnc ? "ON" : "OFF", context.getTimer().elapsed() - init); - return c; -} - -/** -computes the bounded distance between the chains' poses (only for chains with enabled obs) -and the reference frame of the samples. If at least one of the distance is <= of the max distance -it returns the distance.. otherwise it returns INF. -*/ -golem::Real FTDrivenHeuristic::getBoundedDist(const golem::Waypoint& w) const { -#ifdef _HEURISTIC_PERFMON - ++perfBoundDist; -#endif - const Real ret = golem::Node::COST_INF; - if (pBelief.get() && pBelief->getHypotheses().empty()) - return ret; - - Hypothesis::Seq::const_iterator maxLhdPose = pBelief->getHypotheses().begin(); - for (Chainspace::Index i = stateInfo.getChains().end() - 1; i >= stateInfo.getChains().begin(); --i){ - const RBCoord c(w.wpos[i]); - const Real dist = c.p.distance((*maxLhdPose)->toRBPoseSampleGF().p); - if (dist < ftDrivenDesc.ftModelDesc.distMax) - return dist; - } - - //for (Belief::Hypothesis::Seq::const_iterator s = ++pBelief->getHypotheses().begin(); s != pBelief->getHypotheses().end(); ++s) { - // for (Chainspace::Index i = stateInfo.getChains().begin(); i < stateInfo.getChains().end(); ++i) { - // const ChainDesc* cdesc = getChainDesc()[i]; - // if (cdesc->enabledObs) { - // const RBCoord c(w.wpos[i]); - // const Real dist = c.p.distance((*s)->toRBPoseSampleGF().p); - // if (dist < this->ftDrivenDesc.ftModelDesc.distMax) - // return dist; - // } - // } - //} - return ret; -} - -golem::Real FTDrivenHeuristic::getMahalanobisDist(const golem::Waypoint& w0, const golem::Waypoint& goal) const { -// context.debug("FTHeuristic::getMahalanobisDist()\n"); - golem::Real dist = golem::REAL_ZERO; - - for (golem::Chainspace::Index i = stateInfo.getChains().begin(); i < stateInfo.getChains().end(); ++i) { - const ChainDesc* desc = getChainDesc()[i]; - - const grasp::RBCoord a(w0.wpos[i]), b(goal.wpos[i]); - // linear distance - if (desc->enabledLin) { - //const Real d0 = covarianceInv[0]*golem::Math::sqr(a[0] - b[0]) + covarianceInv[1]*golem::Math::sqr(a[1] - b[1]) + covarianceInv[2]*golem::Math::sqr(a[2] - b[2]); - //const Real d1 = covarianceInv[3]*golem::Math::sqr(a[3] - b[3]) + covarianceInv[4]*golem::Math::sqr(a[4] - b[4]) + covarianceInv[5]*golem::Math::sqr(a[5] - b[5]) + covarianceInv[6]*golem::Math::sqr(a[6] - b[6]); - //const Real d2 = covarianceInv[3]*golem::Math::sqr(a[3] + b[3]) + covarianceInv[4]*golem::Math::sqr(a[4] + b[4]) + covarianceInv[5]*golem::Math::sqr(a[5] + b[5]) + covarianceInv[6]*golem::Math::sqr(a[6] + b[6]); - //return golem::REAL_HALF*(d0 + std::min(d1, d2)); - const golem::Vec3 p = w0.wpos[i].p - goal.wpos[i].p; - const golem::Real d = dot(p, pBelief->getSampleProperties().covarianceInv.p).dot(p); -// context.debug(" Vec p<%.4f,%.4f,%.4f>, p^TC^-1<%.4f,%.4f,%.4f>, d=%.4f\n", -// p.x, p.y, p.z, dot(p, covarianceInv.p).x, dot(p, covarianceInv.p).y, dot(p, covarianceInv.p).z, -// d); - /*if (d < 0.001) - context.getMessageStream()->write(Message::LEVEL_DEBUG, "MyHeuristic::getMahalanobisDist() w0 <%.2f, %.2f, %.2f> Md %.4f, d %.4f\n", - w0.wpos[i].p.x, w0.wpos[i].p.y, w0.wpos[i].p.z, d, Heuristic::getLinearDist(w0.wpos[i].p, goal.wpos[i].p));*/ - dist += golem::Math::sqrt(d); - } - // angular distance - if (desc->enabledAng) { - const golem::Real q = Heuristic::getAngularDist(w0.qrot[i], goal.qrot[i]); - dist += (golem::REAL_ONE - desc->distNorm)*q; -// context.debug(" Ang distance %.4f\n", q); - } - } -// context.debug(" Mahalanobis distance %.4f\n", ftDrivenDesc.ftModelDesc.mahalanobisDistFac*dist); - return ftDrivenDesc.ftModelDesc.mahalanobisDistFac*dist; -} - -/* - J(wi, wj)=SUM_k e^-PSI(wi, wj, k) - where PSI(wi, wj, k)=||h(wj,p^k)-h(wj,p^1)||^2_Q -*/ -Real FTDrivenHeuristic::expectedObservationCost(const golem::Waypoint &wi, const golem::Waypoint &wj) const { -// context.write("FTDrivenHeuristic::getObservationalCost(const Waypoint &wi, const Waypoint &wj)\n"); - if (pBelief->getHypotheses().size() == 0) - return REAL_ONE; - - Real cost = REAL_ZERO; - /*Real boundDist = REAL_ZERO; - for (Chainspace::Index i = stateInfo.getChains().begin(); i < stateInfo.getChains().end(); ++i) { - for (U32 k = 1; k <= desc.beliefDesc.nParticles; ++k) - boundDist += getLinearDist(wj.wpos[i].p, particleSet[k].object.shapes[0]->pose.p); - } - if (boundDist > desc.ftModelDesc.boundFTDist) - return -1;*/ - - std::stringstream str; - std::vector hij; -// for (HypSample::Map::const_iterator k = ++samples.begin(); k != samples.end(); ++k) { -// Real value = Math::exp(-REAL_HALF*psi(wi, wj, k)); - // for debug purposes I compute psi function here. - hij.clear(); - h(wi, wj, /*k,*/ hij); - Real value(REAL_ZERO); - for (std::vector::const_iterator i = hij.begin(); i != hij.end(); ++i) - value += Math::sqr(*i); - //str << "h(size=" << hij.size() << ") <"; - //for (std::vector::const_iterator i = hij.begin(); i != hij.end(); ++i) - // str << *i << " "; - //str << "> magnitude=" << Math::sqrt(value) << " e=" << Math::exp(-REAL_HALF*Math::sqrt(value)) << "\n"; - cost += Math::exp(-Math::sqrt(value)); // REAL_HALF* -// } - -// str << "cost=" << cost/(samples.size() - 1) << "\n"; -// std::printf("%s\n", str.str().c_str()); - return cost/(pBelief->getHypotheses().size() - 1); -} - -//Real FTDrivenHeuristic::psi(const Waypoint &wi, const Waypoint &wj, HypSample::Map::const_iterator p) const { -// context.getMessageStream()->write(Message::LEVEL_DEBUG, "psi(wi, wj) = ||h(p^i) - h(p^0)||_gamma\n"); -// std::vector hij, gamma; -// h(wi, wj, p, hij); -// -// Real magnitude(REAL_ZERO); -// for (std::vector::const_iterator i = hij.begin(); i != hij.end(); ++i) -// magnitude += Math::sqr(*i); -// -// context.write("h <"); -// for (std::vector::const_iterator i = hij.begin(); i != hij.end(); ++i) -// context.write("%f ", *i); -// context.write("> magnitude=%f \n", Math::sqrt(magnitude)); -// return Math::sqrt(magnitude); -// //// generate diagonal matrix -// //gamma.clear(); -// //gamma.reserve(hij.size()); -// //const Real noise = 2*covarianceDet; -// //for (size_t i = 0; i < hij.size(); ++i) -// // gamma.push_back(noise); -// // -// //// sqrt((h(wi,wj,p^k)-h(wi,wj,p^-1)) \ GAMMA * (h(wi,wj,p^k)-h(wi,wj,p^-1))) -// //std::vector v; -// //dotInverse(hij, gamma, v); -// //Real d = dot(v, hij); -// ////std::printf("FTHeuristic::h(hij):"); -// ////for (std::vector::const_iterator i = hij.begin(); i != hij.end(); ++i) -// //// std::printf(" %.10f", *i); -// ////std::printf("\n"); -// ////std::printf("FTHeuristic::h(v):"); -// ////for (std::vector::const_iterator i = v.begin(); i != v.end(); ++i) -// //// std::printf(" %.10f", *i); -// ////std::printf("\n"); -// ////std::printf("FTHeuristic::psi():"); -// ////for (std::vector::const_iterator i = gamma.begin(); i != gamma.end(); ++i) -// //// std::printf(" %.10f", *i); -// ////std::printf("\n"); -// ////std::printf("||h(p^%d) - h(p^0)||_gamma = %.10f\n", p->first, d); -// //return REAL_HALF*d; -//} - -void FTDrivenHeuristic::h(const golem::Waypoint &wi, const golem::Waypoint &wj, std::vector &y) const { -// context.write("FTDrivenHeuristic::h(const Waypoint &wi, const Waypoint &wj, std::vector &y)\n"); -#ifdef _HEURISTIC_PERFMON - ++perfH; -#endif - auto kernel = [&] (Real x, Real lambda) -> Real { - return /*lambda**/golem::Math::exp(-lambda*x); - }; - Hypothesis::Seq::const_iterator maxLhdPose = pBelief->getHypotheses().begin(); - const Real norm = (REAL_ONE - /*pBelief->*/kernel(ftDrivenDesc.ftModelDesc.distMax, /*pBelief->*/ftDrivenDesc.ftModelDesc.lambda));//(ftDrivenDesc.ftModelDesc.enabledLikelihood)?/*REAL_ONE/*/(REAL_ONE - kernel(ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.lambda)):REAL_ONE; - y.clear(); - const U32 meanIdx = 0; - U32 steps = 1; - - // computes the estimate of contact for the starting PRM node - //grasp::RealSeq estimates; estimates.assign(pBelief->getHypotheses().size(), REAL_ZERO); U32 hypIndex = 0; - //for (auto p = pBelief->getHypotheses().cbegin(); p != pBelief->getHypotheses().cend(); ++p) - // estimates[hypIndex++] = estimate(p, wi); - - if (false) { - const Real dist = getDist(wi, wj); - steps = (U32)Math::round(dist/(desc.collisionDesc.pathDistDelta)); - } - y.reserve((pBelief->getHypotheses().size() - 1)*steps/**(armInfo.getChains().size() + handInfo.getJoints().size())*/); - - golem::Waypoint w; - U32 i = (steps == 1) ? 0 : 1; - for (; i < steps; ++i) { - if (steps != 1) { - for (Configspace::Index j = stateInfo.getJoints().begin(); j < stateInfo.getJoints().end(); ++j) - w.cpos[j] = wj.cpos[j] - (wj.cpos[j] - wi.cpos[j])*Real(i)/Real(steps); - - // skip reference pose computation - w.setup(controller, false, true); - } - else w = wj; - -// Mat34 closestShape_idx, closestShape_mean; -// context.getMessageStream()->write(Message::LEVEL_DEBUG, "||h(wj, p^%d) - h(wj, p^0)||\n", idx); - // evaluated only the end effector(s) of the arm (wrist) -#ifdef _PER_JOINT_ - //for (Chainspace::Index i = armInfo.getChains().begin(); i != armInfo.getChains().end(); ++i) { - // const ChainDesc* cdesc = getChainDesc()[i]; - // if (cdesc->enabledObs) { - // // computes the workspace coordinate of the relative chain, if it should be in the obs - // const RBCoord c(w.wpos[i]); - // // computes the likelihood of contacting the mean pose - // const Real likelihood_p1 = norm*pBelief->density((*maxLhdPose)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); - - // // computes the likelihood of contacting the other hypotheses - // for (Belief::Hypothesis::Seq::const_iterator p = ++pBelief->getHypotheses().begin(); p != pBelief->getHypotheses().end(); ++p) { - // const Real likelihood_pi = norm*pBelief->density((*p)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); - // // the gain is saved in a vector - // y.push_back(likelihood_pi - likelihood_p1); - // } - // } - //} - - - CriticalSection cs; - Chainspace::Index armIndex = armInfo.getChains().begin(); - ParallelsTask((golem::Parallels*)context.getParallels(), [&] (ParallelsTask*) { - for (Chainspace::Index i = armInfo.getChains().begin();;) { - { - CriticalSectionWrapper csw(cs); - if (armIndex == armInfo.getChains().end()) - break; - i = armIndex++; - } - const ChainDesc* cdesc = getChainDesc()[i]; - if (cdesc->enabledObs) { - // computes the workspace coordinate of the relative chain, if it should be in the obs - const RBCoord c(w.wpos[i]); - // computes the likelihood of contacting the mean pose - const Real likelihood_p1 = norm*pBelief->density((*maxLhdPose)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); - - // computes the likelihood of contacting the other hypotheses - for (Belief::Hypothesis::Seq::const_iterator p = ++pBelief->getHypotheses().begin(); p != pBelief->getHypotheses().end(); ++p) { - const Real likelihood_pi = norm*pBelief->density((*p)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); - // the gain is saved in a vector - CriticalSectionWrapper csw(cs); - y.push_back(likelihood_pi - likelihood_p1); - } - } - } - }); - - //for (Chainspace::Index i = handInfo.getChains().begin(); i != handInfo.getChains().end(); ++i) { - // const ChainDesc* cdesc = getChainDesc()[i]; - // // if enable obs is ON than we compute observations (first for the finger [chain] and then for the single joint in the finger) - // if (cdesc->enabledObs) { - // // compute the observation for the finger tip - // const RBCoord c(w.wpos[i]); - // // computes the likelihood of contacting the mean pose - // const Real likelihood_p1 = norm*pBelief->density((*maxLhdPose)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); - // // std::printf("chain %d pose<%f %f %f> density_sample=%f density_hyp=%f\n", *i, c.p.x, c.p.y, c.p.z, likelihood_pi, likelihood_p1); - // // store the computed observations for the finger tip - // // computes the likelihood of contacting the other hypotheses - // for (Belief::Hypothesis::Seq::const_iterator p = ++pBelief->getHypotheses().begin(); p != pBelief->getHypotheses().end(); ++p) { - // const Real likelihood_pi = norm*pBelief->density((*p)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); - // // the gain is saved in a vector - // y.push_back(likelihood_pi - likelihood_p1); - // } - // } - //} - - - Chainspace::Index handIndex = handInfo.getChains().begin(); - ParallelsTask((golem::Parallels*)context.getParallels(), [&] (ParallelsTask*) { - // evaluated all the joints of the hand - for (Chainspace::Index i = handInfo.getChains().begin();;) { - { - CriticalSectionWrapper csw(cs); - if (handIndex == handInfo.getChains().end()) - break; - i = handIndex++; - } - const ChainDesc* cdesc = getChainDesc()[i]; - // if enable obs is ON than we compute observations (first for the finger [chain] and then for the single joint in the finger) - if (cdesc->enabledObs) { - // compute the observation for the finger tip - const RBCoord c(w.wpos[i]); - // computes the likelihood of contacting the mean pose - const Real likelihood_p1 = norm*pBelief->density((*maxLhdPose)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); - // std::printf("chain %d pose<%f %f %f> density_sample=%f density_hyp=%f\n", *i, c.p.x, c.p.y, c.p.z, likelihood_pi, likelihood_p1); - // store the computed observations for the finger tip - // computes the likelihood of contacting the other hypotheses - for (Belief::Hypothesis::Seq::const_iterator p = ++pBelief->getHypotheses().begin(); p != pBelief->getHypotheses().end(); ++p) { - const Real likelihood_pi = norm*pBelief->density((*p)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); - // the gain is saved in a vector - CriticalSectionWrapper csw(cs); - y.push_back(likelihood_pi - likelihood_p1); - } - } - } - -}); // end parallels task -#endif - //U32 index = 0; - - //const Parallels* parallels = context.getParallels(); - //if (parallels != NULL) { - // const Job* job = parallels->getCurrentJob(); - // if (job != NULL) { - // index = job->getJobId(); - // } - //} - //auto discountedEstimation = [&](const Hypothesis::Seq::const_iterator& ptr, const Waypoint& w, const U32 prev) -> Real { - // const Real next = estimate(ptr, w); - // const Real delta = Math::exp(next - prev); - // return next * (delta > 1 ? 1 : delta); - //}; - //const Real estimate_p1 = estimate(pBelief->getHypotheses().cbegin(), w); - const Real likelihood_p1 = estimate(pBelief->getHypotheses().cbegin(), w); //Math::exp(estimate_p1 - estimates[0]);//pBelief->getHypotheses().front()->evaluate(ftDrivenDesc.evaluationDesc, manipulator->getPose(w.cpos), testCollision); - //hypIndex = 1; - for (auto p = ++pBelief->getHypotheses().cbegin(); p != pBelief->getHypotheses().cend(); ++p) { - //for (auto j = 1; j < pBelief->getHypotheses().size(); ++j) { - //const Real likelihood_pi = pBelief->getHypotheses()[j]->evaluate(ftDrivenDesc.evaluationDesc, manipulator->getPose(w.cpos), testCollision); - // y.push_back(likelihood_pi - likelihood_p1); - //y.push_back(Math::exp(estimates[hypIndex++] - estimate(p, w)) - likelihood_p1); - y.push_back(estimate(p, w) - likelihood_p1); - } - - //for (Belief::Hypothesis::Seq::const_iterator p = ++pBelief->getHypotheses().begin(); p != pBelief->getHypotheses().end(); ++p) { - // const Real likelihood_pi = collision->evaluate(waypoint, (*p)->getCloud(), golem::Rand(rand), manipulator->getPose(w.cpos), testCollision); - // y.push_back(likelihood_pi - likelihood_p1); - //} - } -} - -Real FTDrivenHeuristic::evaluate(const Hypothesis::Seq::const_iterator &hypothesis, const golem::Waypoint &w) const { - Real eval = REAL_ZERO; - golem::Controller::State state = manipulator->getController().createState(); - manipulator->getController().lookupState(golem::SEC_TM_REAL_MAX, state); - // position control - state.cpos = w.cpos; - grasp::Manipulator::Config config(w.cpos); - if (intersect(manipulator->getBounds(config.config, config.frame.toMat34()), (*hypothesis)->bounds(), false)) - eval = (*hypothesis)->evaluate(ftDrivenDesc.evaluationDesc, config.config); - else { - const Real dist = config.frame.toMat34().p.distance((*hypothesis)->toRBPoseSampleGF().p); - if (dist < ftDrivenDesc.ftModelDesc.distMax) - eval = dist; - } - return eval; -} - - -//void FTDrivenHeuristic::h(const golem::Waypoint &wi, const golem::Waypoint &wj, Belief::Hypothesis::Seq::const_iterator p, std::vector &y) const { -// Belief::Hypothesis::Seq::const_iterator maxLhdPose = pBelief->getHypotheses().begin(); -// const Real norm = (ftDrivenDesc.ftModelDesc.enabledLikelihood)?/*REAL_ONE/*/(REAL_ONE - kernel(ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.lambda)):REAL_ONE; -// y.clear(); -// const U32 meanIdx = 0; -// U32 steps = 1; -// -// if (false) { -// const Real dist = getDist(wi, wj); -// steps = (U32)Math::round(dist/(desc.collisionDesc.pathDistDelta*4)); -// } -// y.reserve(steps*(armInfo.getChains().size() + handInfo.getJoints().size())); -// -// Waypoint w; -// U32 i = (steps == 1) ? 0 : 1; -// for (; i < steps; ++i) { -// if (steps != 1) { -// for (Configspace::Index j = stateInfo.getJoints().begin(); j < stateInfo.getJoints().end(); ++j) -// w.cpos[j] = wj.cpos[j] - (wj.cpos[j] - wi.cpos[j])*Real(i)/Real(steps); -// -// // skip reference pose computation -// w.setup(controller, false, true); -// } -// else w = wj; -// -//// Mat34 closestShape_idx, closestShape_mean; -//// context.getMessageStream()->write(Message::LEVEL_DEBUG, "||h(wj, p^%d) - h(wj, p^0)||\n", idx); -// // evaluated only the end effector(s) of the arm (wrist) -// for (Chainspace::Index i = armInfo.getChains().begin(); i < armInfo.getChains().end(); ++i) { -// const ChainDesc* cdesc = getChainDesc()[i]; -// if (cdesc->enabledObs) { -// const RBCoord c(w.wpos[i]); -// const Real likelihood_pi = norm*density((*p)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); -// const Real likelihood_p1 = norm*density((*maxLhdPose)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); -// -// y.push_back(likelihood_pi - likelihood_p1); -// } -// } -// // evaluated all the joints of the hand -// for (Chainspace::Index i = handInfo.getChains().begin(); i < handInfo.getChains().end(); ++i) { -// const ChainDesc* cdesc = getChainDesc()[i]; -// // if enable obs is ON than we compute observations (first for the finger [chain] and then for the single joint in the finger) -// if (cdesc->enabledObs) { -// // compute the observation for the finger tip -// const RBCoord c(w.wpos[i]); -// const Real likelihood_pi = norm*density((*p)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); -// const Real likelihood_p1 = norm*density((*maxLhdPose)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces)); -//// std::printf("chain %d pose<%f %f %f> density_sample=%f density_hyp=%f\n", *i, c.p.x, c.p.y, c.p.z, likelihood_pi, likelihood_p1); -// // store the computed observations for the finger tip -// y.push_back(likelihood_pi - likelihood_p1); -// // for each enabled joint we compute the observations -//// if (false) { -//// for (Configspace::Index j = handInfo.getJoints(i).begin(); j < handInfo.getJoints(i).end(); ++j) { -//// const JointDesc* jdesc = getJointDesc()[j]; -////// if (jdesc->enabled) { -//// const RBCoord c(w.wposex[j]); -//// const Real distToHyp = dist2NearestKPoints(c, maxLhdPose); -//// Real distToSample = dist2NearestKPoints(c, p); -//// // check if the fingertip is inside the object -//// //if (j == handInfo.getJoints(i).end() - 1 && distance(c, p->second->sample) < ftDrivenDesc.ftModelDesc.distMax) -//// // distToSample = distToHyp; -//// const Real likelihood_pi = norm*density(distToSample); -//// const Real likelihood_p1 = norm*density(distToHyp); -//// //Real d_idx, d_mean; -//// //if (ftDrivenDesc.ftModelDesc.enabledLikelihood && !ftDrivenDesc.ftModelDesc.enabledExpLikelihood) { -//// // d_idx = density(RBCoord(w.wpos[i]), RBCoord(closestShape_idx), covariance, ftDrivenDesc.ftModelDesc.tactileRange); -//// // d_mean = density(RBCoord(w.wpos[i]), RBCoord(closestShape_mean), covariance, ftDrivenDesc.ftModelDesc.tactileRange); -//// //} -//// //else { -//// // const Real distToIdx = getLinearDist(w.wpos[i].p, closestShape_idx.p); -//// // const Real distToMean = getLinearDist(w.wpos[i].p, closestShape_mean.p); -//// // d_idx = (ftDrivenDesc.ftModelDesc.enabledLikelihood)?distToIdx:density(distToIdx, ftDrivenDesc.ftModelDesc.lambda, ftDrivenDesc.ftModelDesc.tactileRange); -//// // d_mean = (ftDrivenDesc.ftModelDesc.enabledLikelihood)?distToMean:density(distToMean, ftDrivenDesc.ftModelDesc.lambda, ftDrivenDesc.ftModelDesc.tactileRange); -//// //} -//// y.push_back(likelihood_pi - likelihood_p1); -////// } -//// // context.getMessageStream()->write(Message::LEVEL_DEBUG, " %.5f - %.5f = %.5f\n", d_idx, d_mean, d_idx - d_mean); -//// } -//// } -// } -// } -// } -//} -// -//golem::Real FTDrivenHeuristic::h(const Waypoint &wij, U32 idx) const { -// if (collides(wij)) -// return Node::COST_INF; -// -// Real cost = REAL_ZERO; -// Mat34 closestShape; -// for (Chainspace::Index i = stateInfo.getChains().begin(); i < stateInfo.getChains().end(); ++i) { -// closestShape = getClosestShapePose(wij.wpos[i], particleSet[idx].object); -// const Real d = getLinearDist(wij.wpos[i].p, closestShape.p);//density(RBCoord(wij.wpos[i]), RBCoord(closestShape), bsDesc.beliefDesc.covariance, bsDesc.ftModelDesc.tactileRange); -// cost += d; -// } -// cost /= stateInfo.getChains().size(); -//// printf("p.weigth %.6f, eta_hit %.4f, cost %.4f\n", particleSet[idx].weight, hitNormFacInv, cost); -// return particleSet[idx].weight*/*hitNormFacInv**/cost; -//} - -//------------------------------------------------------------------------------ - -//Real FTDrivenHeuristic::dist2NearestKPoints(const grasp::RBCoord &pose, Belief::Hypothesis::Seq::const_iterator p, const bool normal) const { -// if (pose.p.distance(p->second->sample.p) > (REAL_ONE + REAL_HALF)*ftDrivenDesc.ftModelDesc.distMax) -// return ftDrivenDesc.ftModelDesc.distMax + 1;; -// -// SecTmReal init = context.getTimer().elapsed(); -// if (!ftDrivenDesc.knearest) -// return dist2NearestPoint(pose, p, normal); -// if (ftDrivenDesc.trimesh) -// return dist2NearestTriangle(pose, p, normal); -// -// pcl::PointXYZ searchPoint; -// searchPoint.x = (float)pose.p.x; -// searchPoint.y = (float)pose.p.y; -// searchPoint.z = (float)pose.p.z; -// -// std::vector indeces; -// std::vector distances; -// pcl::KdTree::PointCloudConstPtr cloud = p->second->pTree->getInputCloud(); -// Real result = REAL_ZERO; -// Vec3 median; -// median.setZero(); -// if (p->second->pTree->nearestKSearch(searchPoint, ftDrivenDesc.ftModelDesc.k, indeces, distances) > 0) { -// const size_t size = indeces.size() < ftDrivenDesc.numIndeces ? indeces.size() : ftDrivenDesc.numIndeces; -// for (size_t i = 0; i < size; ++i) { -// idxdiff_t idx = size < indeces.size() ? indeces[size_t(rand.next())%indeces.size()] : indeces[i]; -// //Point point; -// //point.frame.setId(); -// //point.frame.p.set(cloud->points[idx].x, cloud->points[idx].y, cloud->points[idx].z); -// //result += pose.p.distance(point.frame.p); -// //median += Vec3(cloud->points[idx].x, cloud->points[idx].y, cloud->points[idx].z); -// result += pose.p.distance(Vec3(cloud->points[idx].x, cloud->points[idx].y, cloud->points[idx].z)); -// median += Vec3(cloud->points[idx].x, cloud->points[idx].y, cloud->points[idx].z); -// } -// result /= size; -// median /= size; -// } -// -// if (normal) { -// Vec3 v; -// Mat34 jointFrame(Mat33(pose.q), pose.p); -// Mat34 jointFrameInv; -// jointFrameInv.setInverse(jointFrame); -// jointFrameInv.multiply(v, median); -// v.normalise(); -//// const Real thx(Math::atan2(v.z, v.x)), thy(Math::atan2(v.z, v.y)); -//// if (v.z < 0 || Math::abs(thx) > ftDrivenDesc.ftModelDesc.coneTheta1 || Math::abs(thy) > ftDrivenDesc.ftModelDesc.coneTheta2) -// if (v.z < 0 || v.z < ftDrivenDesc.ftModelDesc.coneTheta1) -// result = ftDrivenDesc.ftModelDesc.distMax + 1; -// //else { -// // Mat33 rot; -// // Real roll, pitch, yaw; -// // Quat quat(pose.q); -// // quat.toMat33(rot); -// // rot.toEuler(roll, pitch, yaw); -// // std::printf("nearest(): pose [%.4f,%.4f,%.4f]<%.4f,%.4f,%.4f>; median <%.4f,%.4f,%.4f>, thx %.4f, thy %.4f\n", -// // roll, pitch, yaw, pose.p.x, pose.p.y, pose.p.z, median.x, median.y, median.z, thx, thy); -// //} -// } -// //if (result < ftDrivenDesc.ftModelDesc.distMax + 1) { -// // std::printf("NearestKNeigh (t %.7f) %.7f\n", context.getTimer().elapsed() - init, result); -// // dist2NearestPoint(pose, p, normal); -// //} -// return result; -//} -// -//Real FTDrivenHeuristic::dist2NearestTriangle(const grasp::RBCoord &pose, Belief::Hypothesis::Seq::const_iterator p, const bool normal) const { -// //Point::Seq samplePoints = p->second.points; -// // -// //pcl::PointCloud::Ptr cloud(new pcl::PointCloud); -// //cloud->width = samplePoints.size(); -// //cloud->height = 1; -// //cloud->points.resize (cloud->width * cloud->height); -// //int j = 0; -// //for (Point::Seq::const_iterator point = samplePoints.begin(); point != samplePoints.end(); ++point/*, ++j*/) { -// // cloud->points[j].x = (float)point->frame.p.x; -// // cloud->points[j].y = (float)point->frame.p.y; -// // cloud->points[j++].z = (float)point->frame.p.z; -// //} -// //for (size_t i = 0; i < cloud->points.size (); ++i) -// // std::printf("<%.4f, %.4f, %.4f>\n", cloud->points[i].x, cloud->points[i].y, cloud->points[i].z); -// -// //pcl::KdTreeFLANN kdtree; -// //kdtree.setInputCloud(cloud); -// SecTmReal init = context.getTimer().elapsed(); -// -// pcl::PointXYZ searchPoint; -// searchPoint.x = (float)pose.p.x; -// searchPoint.y = (float)pose.p.y; -// searchPoint.z = (float)pose.p.z; -// -// Real result = REAL_MAX; -// Vec3 median; -// median.setZero(); -// const size_t size = (*p)->pTriangles->polygons.size() < ftDrivenDesc.ftModelDesc.points ? (*p)->pTriangles->polygons.size() : ftDrivenDesc.ftModelDesc.points; -// for (size_t i = 0; i < size; ++i) { -// const pcl::Vertices vertex = size < (*p)->pTriangles->polygons.size() ? (*p)->pTriangles->polygons[size_t(rand.next())%(*p)->pTriangles->polygons.size()] : (*p)->pTriangles->polygons[i]; -// for (std::vector::const_iterator j = vertex.vertices.begin(); j != vertex.vertices.end(); ++j) { -// //const Real dist = pose.p.distance(p->second->points[*j].frame.p); -// //if (result > dist) { -// // result = dist; -// // median.set(p->second->points[*j].frame.p); -// //} -// const Real dist = pose.p.distance(Cloud::getPoint((*p)->points[*j])); -// if (result > dist) { -// result = dist; -// median.set(Cloud::getPoint((*p)->points[*j])); -// } -// } -// } -// -// if (normal) { -// Vec3 v; -// Mat34 jointFrame(Mat33(pose.q), pose.p); -// Mat34 jointFrameInv; -// jointFrameInv.setInverse(jointFrame); -// jointFrameInv.multiply(v, median); -// v.normalise(); -//// const Real thx(Math::atan2(v.z, v.x)), thy(Math::atan2(v.z, v.y)); -//// if (v.z < 0 || Math::abs(thx) > ftDrivenDesc.ftModelDesc.coneTheta1 || Math::abs(thy) > ftDrivenDesc.ftModelDesc.coneTheta2) -// if (v.z < 0 || v.z < ftDrivenDesc.ftModelDesc.coneTheta1) -// result = ftDrivenDesc.ftModelDesc.distMax + 1; -// //else { -// // Mat33 rot; -// // Real roll, pitch, yaw; -// // Quat quat(pose.q); -// // quat.toMat33(rot); -// // rot.toEuler(roll, pitch, yaw); -// // std::printf("nearest(): pose [%.4f,%.4f,%.4f]<%.4f,%.4f,%.4f>; median <%.4f,%.4f,%.4f>, thx %.4f, thy %.4f\n", -// // roll, pitch, yaw, pose.p.x, pose.p.y, pose.p.z, median.x, median.y, median.z, thx, thy); -// //} -// } -// std::printf("dist2NearestTriangle: preforming time %.7f\n", context.getTimer().elapsed() - init); -// return result; -//} -// -// -//Real FTDrivenHeuristic::dist2NearestPoint(const grasp::RBCoord &pose, Belief::Hypothesis::Seq::const_iterator p, const bool normal) const { -// SecTmReal init = context.getTimer().elapsed(); -// Real result = REAL_MAX; -// Vec3 median; -// median.setZero(); -// const size_t size = modelPoints.size() < ftDrivenDesc.ftModelDesc.points ? modelPoints.size() : ftDrivenDesc.ftModelDesc.points; -// for (size_t i = 0; i < size; ++i) { -// //const Point& point = size < p->second->points.size() ? p->second->points[size_t(rand.next())%p->second->points.size()] : p->second->points[i]; -// //const Real dist = pose.p.distance(point.frame.p); -// //if (result > dist) { -// // result = dist; -// // median.set(point.frame.p); -// //} -// const Vec3 point = Cloud::getPoint(size < (*p)->points.size() ? (*p)->points[size_t(rand.next())%(*p)->points.size()] : (*p)->points[i]); -// const Real dist = pose.p.distance(point); -// if (result > dist) { -// result = dist; -// median.set(point); -// } -// } -// -// if (normal) { -// Vec3 v; -// Mat34 jointFrame(Mat33(pose.q), pose.p); -// Mat34 jointFrameInv; -// jointFrameInv.setInverse(jointFrame); -// jointFrameInv.multiply(v, median); -// v.normalise(); -//// const Real thx(Math::atan2(v.z, v.x)), thy(Math::atan2(v.z, v.y)); -//// if (v.z < 0 || Math::abs(thx) > ftDrivenDesc.ftModelDesc.coneTheta1 || Math::abs(thy) > ftDrivenDesc.ftModelDesc.coneTheta2) -// if (v.z < 0 || v.z < ftDrivenDesc.ftModelDesc.coneTheta1) -// result = ftDrivenDesc.ftModelDesc.distMax + 1; -// //else { -// // Mat33 rot; -// // Real roll, pitch, yaw; -// // Quat quat(pose.q); -// // quat.toMat33(rot); -// // rot.toEuler(roll, pitch, yaw); -// // std::printf("nearest(): pose [%.4f,%.4f,%.4f]<%.4f,%.4f,%.4f>; median <%.4f,%.4f,%.4f>, thx %.4f, thy %.4f\n", -// // roll, pitch, yaw, pose.p.x, pose.p.y, pose.p.z, median.x, median.y, median.z, thx, thy); -// //} -// } -// std::printf("NearestPoint (t %.7f) %.7f\n", context.getTimer().elapsed() - init, result); -// return result; -//} -// -//Real FTDrivenHeuristic::distance(const RBCoord &a, const RBCoord &b, bool enableAng) const { -// Real dist = REAL_ZERO; -// -// dist += a.p.distance(b.p); -// -// if (enableAng) -// dist += golem::REAL_ONE - golem::Math::abs(a.q.dot(b.q)); -// -// return dist; -//} -// -//Real FTDrivenHeuristic::kernel(Real x, Real lambda) const { -// return /*lambda**/golem::Math::exp(-lambda*x); -//} -// -//Real FTDrivenHeuristic::density(const RBCoord &x, const RBCoord &mean) const { -// Real dist = x.p.distance(mean.p); -// dist = (dist > ftDrivenDesc.ftModelDesc.distMax) ? ftDrivenDesc.ftModelDesc.distMax : dist; -// if (ftDrivenDesc.ftModelDesc.enabledLikelihood) -// return kernel(dist, ftDrivenDesc.ftModelDesc.lambda); // esponential up to norm factor -// -// return dist; //linear distance -//} -// -//Real FTDrivenHeuristic::density(const Real dist) const { -// if (ftDrivenDesc.ftModelDesc.enabledLikelihood) -//// return kernel(ftDrivenDesc.ftModelDesc.lambda*dist); -// return (dist > ftDrivenDesc.ftModelDesc.distMax) ? REAL_ZERO : (dist < REAL_EPS) ? kernel(REAL_EPS, ftDrivenDesc.ftModelDesc.lambda) : kernel(dist, ftDrivenDesc.ftModelDesc.lambda); // esponential up to norm factor -// -// return dist; //linear distance -//} -// -//golem::Real FTDrivenHeuristic::evaluate(const golem::Bounds::Seq &bounds, const grasp::RBCoord &pose, const grasp::RBPose::Sample &sample, const Real &force, const golem::Mat34 &trn, bool &intersect) const { -// Real distMin = ftDrivenDesc.ftModelDesc.distMax + REAL_ONE; -// const Real norm = (ftDrivenDesc.ftModelDesc.enabledLikelihood)?/*REAL_ONE/*/(REAL_ONE - density(ftDrivenDesc.ftModelDesc.distMax)):REAL_ONE; -// Mat34 actionFrame(sample.q, sample.p), queryFrame(Mat34(sample.q, sample.p) * trn), queryFrameInv, fromQueryToJoint; -// intersect = false; -// -// Vec3 boundFrame, surfaceFrame; -// if (pose.p.distance(queryFrame.p) < distMin) { -// const size_t size = modelPoints.size() < ftDrivenDesc.ftModelDesc.points ? modelPoints.size() : ftDrivenDesc.ftModelDesc.points; -// for (size_t i = 0; i < size; ++i) { -// // CHECK: Do you assume that a point has a full frame with **orientation**? Where does the orientation come from? -// -// //const Point& point = size < modelPoints.size() ? modelPoints[size_t(rand.next())% -// // modelPoints.size()] : modelPoints[i]; -// //Mat34 pointFrame; -// //pointFrame.multiply(actionFrame, point.frame); -// const Vec3 point = Cloud::getPoint(size < modelPoints.size() ? modelPoints[size_t(rand.next())%modelPoints.size()] : modelPoints[i]); -// Mat34 pointFrame = actionFrame; -// pointFrame.p += point; // only position is updated -// const Real dist = [&] () -> Real { -// Real min = REAL_MAX; -// for (golem::Bounds::Seq::const_iterator b = bounds.begin(); b != bounds.end(); ++b) { -// const Real d = (*b)->getSurfaceDistance(pointFrame.p); -// if (d < min) { -// min = d; -// boundFrame = (*b)->getPose().p; -// surfaceFrame = pointFrame.p; -// } -// } -// return min; -// } (); -// if (dist <= REAL_ZERO) -// intersect = true; -// if (dist > -REAL_EPS && dist < distMin) -// distMin = dist; -// } -// -// } -// //context.write(" -> dist(to shape)=%5.7f, bound <%f %f %f>, point <%f %f %f>, intersection %s, norm=%5.7f, density=%5.7f, prob of touching=%5.7f\n", distMin, -// // boundFrame.x, boundFrame.y, boundFrame.z, surfaceFrame.x, surfaceFrame.y, surfaceFrame.z, -// // intersect ? "Y" : "N", norm, density(distMin), norm*density(distMin)); -// //return norm*density(distMin); // if uncomment here there is no notion of direction of contacts -// -// // compute the direction of contact (from torques) -// Vec3 v; -// Mat34 jointFrame(Mat33(pose.q), pose.p); -// Mat34 jointFrameInv; -// jointFrameInv.setInverse(jointFrame); -// jointFrameInv.multiply(v, queryFrame.p); -// v.normalise(); -//// const Real thx(Math::atan2(v.z, v.x)), thy(Math::atan2(v.z, v.y)); -//// if (v.z < 0 || Math::abs(thx) > ftDrivenDesc.ftModelDesc.coneTheta1 || Math::abs(thy) > ftDrivenDesc.ftModelDesc.coneTheta2) -// if ((v.z < 0 && force > 0) || (v.z > 0 && force < 0)) { -// //context.write("evaluation pose <%f %f %f> sample <%f %f %f> v.z=%f force=%f\n", -// // pose.p.x, pose.p.y, pose.p.z, queryFrame.p.x, queryFrame.p.y, queryFrame.p.z, v.z, force); -// return REAL_ZERO; -// } -// // return the likelihood of observing such a contact for the current joint -// return norm*density(distMin); -// -// -//// queryFrameInv.setInverse(queryFrame); -//// fromQueryToJoint.multiply(queryFrameInv, Mat34(pose.q, pose.p)); -//// //context.write("pose <%5.7f %5.7f %5.7f>\n sample <%5.7f %5.7f %5.7f>\n", -//// // pose.p.x, pose.p.y, pose.p.z, sample.p.x, sample.p.y, sample.p.z); -//// //context.write("query-to-joint frame <%5.7f %5.7f %5.7f>\n", -//// // fromQueryToJoint.p.x, fromQueryToJoint.p.y, fromQueryToJoint.p.z); -//// -//// //context.write(" -> magnitude=%5.7f\n", fromQueryToJoint.p.magnitude()); -//// //if (fromQueryToJoint.p.magnitude() > 2*ftDrivenDesc.ftModelDesc.distMax) -//// // return golem::REAL_ZERO; //norm*density(distMin); -//// -//// fromQueryToJoint.multiply(trn, fromQueryToJoint); -//// //context.write("new pose <%5.7f %5.7f %5.7f>, model <%5.7f %5.7f %5.7f>, dist=%5.7f\n", -//// // fromQueryToJoint.p.x, fromQueryToJoint.p.y, fromQueryToJoint.p.z, trn.p.x, trn.p.y, trn.p.z, trn.p.distance(fromQueryToJoint.p)); -//// if (fromQueryToJoint.p.magnitude() < distMin) { -//// const size_t size = modelPoints.size() < ftDrivenDesc.ftModelDesc.points ? modelPoints.size() : ftDrivenDesc.ftModelDesc.points; -//// for (size_t i = 0; i < size; ++i) { -//// const Point& point = size < modelPoints.size() ? modelPoints[size_t(rand.next())%modelPoints.size()] : modelPoints[i]; -//// const Real dist = fromQueryToJoint.p.distance(point.frame.p); -//// if (dist < distMin) -//// distMin = dist; -//// } -//// } -////// context.write(" -> dist(to shape)=%5.7f, norm=%5.7f, density=%5.7f, prob of touching=%5.7f\n", distMin, norm, density(distMin), norm*density(distMin)); -//// return /*norm**/density(distMin); -//} -// -//golem::Real FTDrivenHeuristic::evaluate(const grasp::Manipulator *manipulator, const golem::Waypoint &w, const grasp::RBPose::Sample &sample, const std::vector &triggeredGuards, const grasp::RealSeq &force, const golem::Mat34 &trn) const { -// Real weight = golem::REAL_ONE; -// bool intersect = false; -// // retrieve wrist's workspace pose and fingers configuration -// //golem::Mat34 poses[grasp::Manipulator::JOINTS]; -// //manipulator->getPoses(manipulator->getPose(w.cpos), poses); -// -// grasp::RealSeq forces(force); -// for (Chainspace::Index i = handInfo.getChains().begin(); i != handInfo.getChains().end(); ++i) { -// // check if any of the joint in the current chain (finger) has been triggered -// bool triggered = false; -//// context.write("Chain %d\n", i); -// for (Configspace::Index j = handInfo.getJoints(i).begin(); j != handInfo.getJoints(i).end(); ++j) { -// const size_t idx = j - handInfo.getJoints().begin(); -// triggered = triggered || [&] () -> bool { -// for (std::vector::const_iterator i = triggeredGuards.begin(); i < triggeredGuards.end(); ++i) -// if (j == i->jointIdx) { -// forces[idx] = i->type == FTGuard_ABS ? 0 : i->type == FTGuard_LESSTHAN ? -1 : 1; -// return true; -// } -// return false; -// } (); -// //const golem::U32 joint = manipulator->getArmJoints() + idx; -// //golem::Bounds::Seq bounds; -// //manipulator->getJointBounds(joint, poses[joint], bounds); -// golem::Bounds::Seq bounds; -// manipulator->getJointBounds(golem::U32(j - manipulator->getController()->getStateInfo().getJoints().begin()), w.wposex[j], bounds); -// //for (golem::Bounds::Seq::const_iterator b = bounds.begin(); b != bounds.end(); ++b) -// // context.write("finger bound frame <%f %f %f>\n", (*b)->getPose().p.x, (*b)->getPose().p.y, (*b)->getPose().p.z); -// const Real eval = evaluate(bounds, grasp::RBCoord(w.wposex[j]), sample, forces[idx], trn, intersect); -// weight *= (triggered ? ftDrivenDesc.contactFac*eval : intersect ? ftDrivenDesc.nonContactFac*(REAL_ONE - eval) : 1); -// //context.write("evaluate %f prob %f partial weight %f, intersect %s, triggered %s\n", eval, -// // /*jointFac[j - handInfo.getJoints().begin()]**/(triggered ? ftDrivenDesc.contactFac*eval : ftDrivenDesc.nonContactFac*(REAL_ONE - eval)), weight, -// // intersect ? "Y" : "N", triggered ? "Y" : "N"); -// } -//// const Real eval = evaluate(grasp::RBCoord(w.wpos[i]), sample, 0, trn); -//// weight *= triggered ? eval : REAL_ONE; -//// weight *= triggered ? ftDrivenDesc.contactFac*eval : ftDrivenDesc.nonContactFac*(REAL_ONE - eval); -//// context.write(" -> (triggered %d) weight=%f partial_weight=%f\n", triggered, eval, weight); -// -// } -// -// //for (Configspace::Index j = handInfo.getJoints().begin(); j != handInfo.getJoints().end(); ++j) { -// // context.write("Joint %d\n", j); -// // const size_t k = j - handInfo.getJoints().begin(); -// // bool triggered = [&] () -> bool { -// // for (std::vector::const_iterator i = triggeredGuards.begin(); i < triggeredGuards.end(); ++i) -// // if (j == *i) -// // return true; -// // return false; -// // } (); -// // const Real eval = evaluate(grasp::RBCoord(w.wposex[j]), sample, force[k], trn); -// // const Real tmp = triggered ? eval : 1 - eval; -// // weight *= jointFac[j - handInfo.getJoints().begin()]*tmp; -// // context.write(" -> (triggered %d) weight=%f partial_weight=%f\n", triggered, jointFac[j - handInfo.getJoints().begin()]*tmp, weight); -// //} -//// context.write("final weight=%f\n", weight); -// -// return weight; -//} -// -//golem::Real FTDrivenHeuristic::evaluate(const grasp::Manipulator *manipulator, const golem::Waypoint &w, const grasp::RBPose::Sample &sample, const std::vector &triggeredGuards, const grasp::RealSeq &force, const golem::Mat34 &trn) const { -// Real weight = golem::REAL_ONE; -// bool intersect = false; -// // retrieve wrist's workspace pose and fingers configuration -// //golem::Mat34 poses[grasp::Manipulator::JOINTS]; -// //manipulator->getPoses(manipulator->getPose(w.cpos), poses); -// -// for (Chainspace::Index i = handInfo.getChains().begin(); i != handInfo.getChains().end(); ++i) { -// // check if any of the joint in the current chain (finger) has been triggered -// bool triggered = false; -//// context.write("Chain %d\n", i); -// for (Configspace::Index j = handInfo.getJoints(i).begin(); j != handInfo.getJoints(i).end(); ++j) { -// const size_t idx = j - handInfo.getJoints().begin(); -// triggered = triggered || [&] () -> bool { -// for (std::vector::const_iterator i = triggeredGuards.begin(); i < triggeredGuards.end(); ++i) -// if (j == *i) -// return true; -// return false; -// } (); -// //const golem::U32 joint = manipulator->getArmJoints() + idx; -// //golem::Bounds::Seq bounds; -// //manipulator->getJointBounds(joint, poses[joint], bounds); -// golem::Bounds::Seq bounds; -// manipulator->getJointBounds(golem::U32(j - manipulator->getController()->getStateInfo().getJoints().begin()), w.wposex[j], bounds); -// //for (golem::Bounds::Seq::const_iterator b = bounds.begin(); b != bounds.end(); ++b) -// // context.write("finger bound frame <%f %f %f>\n", (*b)->getPose().p.x, (*b)->getPose().p.y, (*b)->getPose().p.z); -// const Real eval = evaluate(bounds, grasp::RBCoord(w.wposex[j]), sample, force[idx], trn, intersect); -// weight *= (triggered ? ftDrivenDesc.contactFac*eval : intersect ? ftDrivenDesc.nonContactFac*(REAL_ONE - eval) : 1); -// //context.write("evaluate %f prob %f partial weight %f, intersect %s, triggered %s\n", eval, -// // /*jointFac[j - handInfo.getJoints().begin()]**/(triggered ? ftDrivenDesc.contactFac*eval : ftDrivenDesc.nonContactFac*(REAL_ONE - eval)), weight, -// // intersect ? "Y" : "N", triggered ? "Y" : "N"); -// } -//// const Real eval = evaluate(grasp::RBCoord(w.wpos[i]), sample, 0, trn); -//// weight *= triggered ? eval : REAL_ONE; -//// weight *= triggered ? ftDrivenDesc.contactFac*eval : ftDrivenDesc.nonContactFac*(REAL_ONE - eval); -//// context.write(" -> (triggered %d) weight=%f partial_weight=%f\n", triggered, eval, weight); -// -// } -// -// //for (Configspace::Index j = handInfo.getJoints().begin(); j != handInfo.getJoints().end(); ++j) { -// // context.write("Joint %d\n", j); -// // const size_t k = j - handInfo.getJoints().begin(); -// // bool triggered = [&] () -> bool { -// // for (std::vector::const_iterator i = triggeredGuards.begin(); i < triggeredGuards.end(); ++i) -// // if (j == *i) -// // return true; -// // return false; -// // } (); -// // const Real eval = evaluate(grasp::RBCoord(w.wposex[j]), sample, force[k], trn); -// // const Real tmp = triggered ? eval : 1 - eval; -// // weight *= jointFac[j - handInfo.getJoints().begin()]*tmp; -// // context.write(" -> (triggered %d) weight=%f partial_weight=%f\n", triggered, jointFac[j - handInfo.getJoints().begin()]*tmp, weight); -// //} -//// context.write("final weight=%f\n", weight); -// -// return weight; -//} - -//------------------------------------------------------------------------------ - -Real FTDrivenHeuristic::directionApproach(const golem::Waypoint &w) const { - const Chainspace::Index armIndex = armInfo.getChains().begin(); - - Vec3 v; - Mat34 tcpFrameInv, hypothesis; - hypothesis = (*pBelief->getHypotheses().begin())->toRBPoseSampleGF().toMat34(); - tcpFrameInv.setInverse(w.wpos[armIndex]); - tcpFrameInv.multiply(v, hypothesis.p); - v.normalise(); - return v.z > 0 ? REAL_ONE - ftDrivenDesc.directionFac : REAL_ONE; -} - -//------------------------------------------------------------------------------ - -//bool FTDrivenHeuristic::collides(const Waypoint &w) const { -//#ifdef _HEURISTIC_PERFMON -// ++waypointCollisionCounter; -//#endif -// -// //if (!desc.collisionDesc.enabled) -// // return false; -// -// // find data pointer for the current thread -// ThreadData* data = getThreadData(); -// -// // all chains -// //for (Chainspace::Index i = stateInfo.getChains().begin(); i < stateInfo.getChains().end(); ++i) -// if (desc.collisionDesc.enabled) { -// for (Chainspace::Index i = stateInfo.getChains().end() - 1; i >= stateInfo.getChains().begin(); --i) -// { -// // all joints in a chain -// //for (Configspace::Index j = stateInfo.getJoints(i).begin(); j < stateInfo.getJoints(i).end(); ++j) -// for (Configspace::Index j = stateInfo.getJoints(i).end() - 1; j >= stateInfo.getJoints(i).begin(); --j) -// { -// const JointDesc* jdesc = getJointDesc()[j]; -// Bounds::Seq& jointBounds = data->jointBounds[j]; -// -// if (jointBounds.empty() || !jdesc->collisionBounds) -// continue; -// -// // reset to the current joint pose -// setPose(data->jointBoundsPoses[j], w.wposex[j], jointBounds); -// -// // check for collision between the joint bounds and the estimated pose of the object to grasp -// if (pBelief.get()) -// if (pBelief->intersect(jointBounds, w.wposex[j])) -// return true; -// -// // check for collisions between the current joint and the environment bounds, test bounds groups -// if (!collisionBounds.empty() && intersect(jointBounds, collisionBounds, true)) { -// // context.debug("FTDrivenHeuristic::collides(): collisions between the current joint and the environment bounds. collision bounds %s empty\n", collisionBounds.empty()?"are":"are not"); -// return true; -// } -// -// // check for collisions between the current joint and the collision joints, do not test bounds groups -// const U32Seq& collisionJoints = jdesc->collisionJoints; -// for (U32Seq::const_iterator k = collisionJoints.begin(); k != collisionJoints.end(); ++k) { -// const Configspace::Index collisionIndex(*k); -// -// golem::Bounds::Seq &collisionJointBounds = data->jointBounds[collisionIndex]; -// if (collisionJointBounds.empty()) -// continue; -// -// setPose(data->jointBoundsPoses[collisionIndex], w.wposex[collisionIndex], collisionJointBounds); -// if (intersect(jointBounds, collisionJointBounds, false)) { -// // context.debug("FTDrivenHeuristic::collides(): collisions between the current joint and the collision joints\n"); -// return true; -// } -// } -// -// -// //if (pointCloudCollision && pBelief.get() && i != stateInfo.getChains().begin()) -// // if (pBelief->intersect(jointBounds, w.wposex[j])) -// // return true; -// -// // TODO joint bounds - chain bounds collisions -// } -// -// // TODO bounds - chain bounds collisions -// } -// } -// //// check for collisions with the object to grasp. only the hand -// if (pointCloudCollision && pBelief.get()) { -// if (manipulator.get()) { -// if (Math::abs(collision->evaluate(waypoint, (*pBelief->getHypotheses().begin())->getCloud(), golem::Rand(rand), manipulator->getPose(w.cpos), testCollision)) > REAL_EPS) { -// //Controller::State state = controller.createState(); -// //state.cpos = w.cpos; -// //std::stringstream str; -// //for (golem::Configspace::Index i = state.getInfo().getJoints().begin(); i < state.getInfo().getJoints().end(); ++i) -// // str << " c" << (*i - *state.getInfo().getJoints().begin() + 1) << "=\"" << state.cpos[i] << "\""; -// //context.write("\n", state.getInfo().getJoints().size(), str.str().c_str()); -// return true; -// } -// } -// } -//// SecTmReal init = context.getTimer().elapsed(); -//// //CriticalSection cs; -//// //Chainspace::Index handIndex = handInfo.getChains().begin(); -////// grasp::Manipulator::Ptr manipulator(new grasp::Manipulator(&controller)); -//// //bool collision = false; -////// ParallelsTask((golem::Parallels*)context.getParallels(), [&] (ParallelsTask*) { -//// // evaluated all the joints of the hand -//// for (Chainspace::Index i = handInfo.getChains().begin(); i != handInfo.getChains().end(); ++i) { -//// //{ -//// // CriticalSectionWrapper csw(cs); -//// // if (handIndex == handInfo.getChains().end()) -//// // break; -//// // i = handIndex++; -//// //} -//// // bounds of the entire finger -////// context.write("chain\n"); -//// //golem::Bounds::Seq bounds; -//// for (Configspace::Index j = handInfo.getJoints(i).end()-1; j >= handInfo.getJoints(i).begin(); --j) { -////// context.write("joint\n"); -//// //manipulator->getJointBounds(golem::U32(j - manipulator->getController()->getStateInfo().getJoints().begin()), w.wposex[j], bounds); -//// const size_t k = j - armInfo.getJoints().begin(); // from the first joint -//// Bounds::Seq boundsSeq; -//// golem::Bounds::Desc::SeqPtr boundsDescSeq = controller.getJoints()[armInfo.getJoints().begin() + k]->getBoundsDescSeq(); -//// for (golem::Bounds::Desc::Seq::const_iterator b = boundsDescSeq->begin(); b != boundsDescSeq->end(); ++b) { -//// boundsSeq.push_back(b->get()->create()); -//// boundsSeq.back()->multiplyPose(w.wposex[j], boundsSeq.back()->getPose()); -//// } -//// if (pBelief->intersect(boundsSeq, w.wposex[j])) { -////// context.debug("FTDrivenHeuristic::collides(): collision between chain %d, joint %d and point cloud. (time %.5f)\n", i - handInfo.getChains().begin(), k, context.getTimer().elapsed() - init); -//// return true; -//// } -//// //{ -//// // CriticalSectionWrapper csw(cs); -//// // if (!collision) -//// // collision = pBelief->intersect(bounds, w.wposex[j]); -//// // else -//// // break; // collision is true. no need to keep looping -//// //} -//// } -//// } -//// }); // end parallels task -//// context.write("done(0). (time %.5f)\n", context.getTimer().elapsed() - init); -//// return collision; -//// } -// -// return false; -// -// //// check if the waypoint collides with objects in the scene -// //if (Heuristic::collides(w)) -// // return true; -// -// //// check if the trajectory collides with the point cloud of the ML sample -// //if (pBelief->getHypotheses().empty()) -// // return false; -// -// //Belief::Hypothesis::Seq::const_iterator maxLhdPose = pBelief->getHypotheses().begin(); -// //for (Chainspace::Index i = handInfo.getChains().begin(); i < handInfo.getChains().end(); ++i) { -// // const RBCoord c(w.wpos[i]); -// // if (c.p.distance((*maxLhdPose)->sample.p) < 0.045) //if (distance(maxLhdPose->second->sample, c) < 0.05) -// // return true; -// //} -// -// //return false; -//} -// -////bool FTDrivenHeuristic::collides2(const golem::Waypoint &w1, const golem::Waypoint &w2) const { -//// if (w1.index == Node::IDX_GOAL || w1.index == Node::IDX_GOAL) -//// return false; -//// -//// const Real dist = getDist(w1, w2); -//// U32 steps = (U32)Math::round(dist / (desc.collisionDesc.pathDistDelta*4)); -//// for (int i = 1; i < steps; ++i) { -//// Waypoint w; -//// for (Configspace::Index j = stateInfo.getJoints().begin(); j < stateInfo.getJoints().end(); ++j) -//// w.cpos[j] = w2.cpos[j] - (w2.cpos[j] - w1.cpos[j])*Real(i) / Real(steps); -//// -//// // skip reference pose computation -//// w.setup(controller, false, true); -//// if (collides(w)) -//// return true; -//// } -//// return false; -////} -// -//bool FTDrivenHeuristic::collides(const Waypoint &w0, const Waypoint &w1) const { -//#ifdef _HEURISTIC_PERFMON -// ++pathCollisionCounter; -//#endif -// -// //if (!desc.collisionDesc.enabled) -// // return false; -// -// // check the end waypoint first -// if (collides(w1)) -// return true; -// -// Waypoint w; -// -// const Real dist = getDist(w0, w1); -// -// const U32 steps = (U32)Math::round(dist / desc.collisionDesc.pathDistDelta); -// -// // the first waypoint does not collide by assumption -// // TODO binary collision detection -// for (U32 i = 1; i < steps; ++i) { -// // lineary interpolate coordinates -// for (Configspace::Index j = stateInfo.getJoints().begin(); j < stateInfo.getJoints().end(); ++j) -// w.cpos[j] = w1.cpos[j] - (w1.cpos[j] - w0.cpos[j])*Real(i) / Real(steps); -// -// // skip reference pose computation -// w.setup(controller, false, true); -// -// if (collides(w)) -// return true; -// } -// -// return false; -//} - -bool FTDrivenHeuristic::collides(const golem::Waypoint &w, ThreadData* data) const { -#ifdef _HEURISTIC_PERFMON - ++perfCollisionWaypoint; -#endif - // check for collisions with the object to grasp. only the hand - grasp::Manipulator::Config config(w.cpos); - if (pointCloudCollision && !pBelief->getHypotheses().empty()) { - Hypothesis::Seq::const_iterator maxLhdPose = pBelief->getHypotheses().begin(); - if (intersect(manipulator->getBounds(config.config, config.frame.toMat34()), (*maxLhdPose)->bounds(), false)) { -#ifdef _HEURISTIC_PERFMON - ++perfCollisionPointCloud; -#endif - //if ((*maxLhdPose)->check(waypoint, manipulator->getPose(w.cpos))) - if ((*maxLhdPose)->check(ftDrivenDesc.checkDesc, rand, config.config)) - return true; - } -// } - } - - return Heuristic::collides(w, data); -} - -bool FTDrivenHeuristic::collides(const golem::Waypoint &w0, const golem::Waypoint &w1, ThreadData* data) const { - const Real dist = getDist(w0, w1); - const U32 size = (U32)Math::round(dist / desc.collisionDesc.pathDistDelta) + 1; - Real p[2]; - golem::Waypoint w; - -#ifdef _HEURISTIC_PERFMON - ++perfCollisionPath; - perfCollisionSegs += size - 1; -#endif - - // test for collisions in the range (w0, w1) - excluding w0 and w1 - for (U32 i = 1; i < size; ++i) { - p[0] = Real(i) / Real(size); - p[1] = REAL_ONE - p[0]; - - // lineary interpolate coordinates - for (Configspace::Index j = stateInfo.getJoints().begin(); j < stateInfo.getJoints().end(); ++j) - w.cpos[j] = p[0] * w0.cpos[j] + p[1] * w1.cpos[j]; - - // skip reference pose computation - w.setup(controller, false, true); - - // test for collisions - if (collides(w, data)) - return true; - } - - return false; -} - -//------------------------------------------------------------------------------ - -bool FTDrivenHeuristic::collides(const golem::Waypoint &w) const { - if (!desc.collisionDesc.enabled && !pointCloudCollision) - return false; - - // find data pointer for the current thread - ThreadData* data = getThreadData(); - - return collides(w, data); -} - -bool FTDrivenHeuristic::collides(const golem::Waypoint &w0, const golem::Waypoint &w1) const { - if (!desc.collisionDesc.enabled && !pointCloudCollision) - return false; - - // find data pointer for the current thread - ThreadData* data = getThreadData(); - - return collides(w0, w1, data); -} - - -//------------------------------------------------------------------------------ - -golem::Real FTDrivenHeuristic::getCollisionCost(const golem::Waypoint &wi, const golem::Waypoint &wj, Hypothesis::Seq::const_iterator p) const { - // Create the normal estimation class, and pass the input dataset to it - //pcl::KdTree::PointCloudConstPtr cloud = p->second->pTree->getInputCloud(); - //pcl::NormalEstimation ne; - //ne.setInputCloud(cloud); - - //// Create an empty kdtree representation, and pass it to the normal estimation object. - //// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given). - //pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); - //ne.setSearchMethod(tree); - - //// Output datasets - //pcl::PointCloud::Ptr cloudNormals(new pcl::PointCloud); - - //// Use all neighbors in a sphere of radius 3cm - //ne.setRadiusSearch (0.01); - - //// Compute the features - //ne.compute(*cloudNormals); // compute the normal at the closest point of the surface - auto kernel = [&](Real x, Real lambda) -> Real { - return /*lambda**/golem::Math::exp(-lambda*x); - }; - - auto density = [&](const Real dist) -> Real { - return (dist > ftDrivenDesc.ftModelDesc.distMax) ? REAL_ZERO : (dist < REAL_EPS) ? kernel(REAL_EPS, ftDrivenDesc.ftModelDesc.lambda) : kernel(dist, ftDrivenDesc.ftModelDesc.lambda); // esponential up to norm factor - }; - - Hypothesis::Seq::const_iterator maxLhdPose = pBelief->getHypotheses().begin(); - const Real norm = (REAL_ONE - density(ftDrivenDesc.ftModelDesc.distMax/*pBelief->myDesc.sensory.sensoryRange*/));//const Real norm = //(ftDrivenDesc.ftModelDesc.enabledLikelihood)?/*REAL_ONE/*/(REAL_ONE - kernel(ftDrivenDesc.ftModelDesc.lambda*ftDrivenDesc.ftModelDesc.distMax)):REAL_ONE; - Real threshold(0.01), cost(REAL_ZERO); - for (Chainspace::Index i = handInfo.getChains().begin(); i < handInfo.getChains().end(); ++i) { - const RBCoord c(wj.wpos[i]); - - const Real d = 0;// (*maxLhdPose)->dist2NearestKPoints(c, ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.k, ftDrivenDesc.numIndeces); -// context.write("FTDrivenHeuristic::getCollisionCost(): distance=%4.5f, cost=%4.5f\n", d, d < threshold ? Node::COST_INF : REAL_ZERO); - // penalise collisions with the shape of the mean pose - if (d < threshold) - return golem::Node::COST_INF; - - //Real distMin(REAL_MAX), dist; - //size_t k = 0, kMin; - //for (; k != cloud->size(); ++k) { - // if (dist = c.p.distance(Vec3(cloud->points[k].x, cloud->points[k].y, cloud->points[k].z)) < distMin) { - // distMin = dist; - // kMin = k; - // } - //} - // compute the cost relative to the approaching direction - //Vec3 approachDir = wi.wpos[i].p - wj.wpos[i].p; - //Vec3 normal(cloudNormals->points[kMin].normal[0], cloudNormals->points[kMin].normal[1], cloudNormals->points[kMin].normal[2]); - //cost += normal.dot(approachDir); - } - - return cost; -} - -Real FTDrivenHeuristic::testObservations(const grasp::RBCoord &pose, const bool normal) const { -// const Real norm = (ftDrivenDesc.ftModelDesc.enabledLikelihood)?/*REAL_ONE/*/(REAL_ONE - density(ftDrivenDesc.ftModelDesc.distMax)):REAL_ONE; - auto kernel = [&](Real x, Real lambda) -> Real { - return /*lambda**/golem::Math::exp(-lambda*x); - }; - - auto density = [&](const Real dist) -> Real { - return (dist > ftDrivenDesc.ftModelDesc.distMax) ? REAL_ZERO : (dist < REAL_EPS) ? kernel(REAL_EPS, ftDrivenDesc.ftModelDesc.lambda) : kernel(dist, ftDrivenDesc.ftModelDesc.lambda); // esponential up to norm factor - }; - const Real norm = (REAL_ONE - density(ftDrivenDesc.ftModelDesc.distMax));//(ftDrivenDesc.ftModelDesc.enabledLikelihood)?/*REAL_ONE/*/(REAL_ONE - kernel(ftDrivenDesc.ftModelDesc.distMax, ftDrivenDesc.ftModelDesc.lambda)):REAL_ONE; - pcl::PointXYZ searchPoint; - searchPoint.x = (float)pose.p.x; - searchPoint.y = (float)pose.p.y; - searchPoint.z = (float)pose.p.z; - - std::vector indeces; - std::vector distances; -// pcl::KdTree::PointCloudConstPtr cloud = (*pBelief->getHypotheses().begin())->pTree->getInputCloud(); - Real result = REAL_ZERO; - Vec3 median; - median.setZero(); - //if ((*pBelief->getHypotheses().begin())->pTree->nearestKSearch(searchPoint, ftDrivenDesc.ftModelDesc.k, indeces, distances) > 0) { - // for (size_t i = 0; i < indeces.size(); ++i) { - // //Point point; - // //point.frame.setId(); - // //point.frame.p.set(cloud->points[indeces[i]].x, cloud->points[indeces[i]].y, cloud->points[indeces[i]].z); - // //result += pose.p.distance(point.frame.p); - // //median += Vec3(cloud->points[indeces[i]].x, cloud->points[indeces[i]].y, cloud->points[indeces[i]].z); - // result += pose.p.distance(Vec3(cloud->points[indeces[i]].x, cloud->points[indeces[i]].y, cloud->points[indeces[i]].z)); - // median += Vec3(cloud->points[indeces[i]].x, cloud->points[indeces[i]].y, cloud->points[indeces[i]].z); - // } - // result /= indeces.size(); - // median /= indeces.size(); - //} - - context.write("FTDrivenHeuristic:testObs(normal=%s):\n", normal ? "ON" : "OFF"); - if (normal) { - Vec3 v; - Mat34 jointFrame(Mat33(pose.q), pose.p); - Mat34 jointFrameInv; - jointFrameInv.setInverse(jointFrame); - jointFrameInv.multiply(v, (*pBelief->getHypotheses().begin())->sample.p); - v.normalise(); -// const Real thx(Math::atan2(v.z, v.x)), thy(Math::atan2(v.z, v.y)); -// if (v.z < 0 || Math::abs(thx) > ftDrivenDesc.ftModelDesc.coneTheta1 || Math::abs(thy) > ftDrivenDesc.ftModelDesc.coneTheta2) - if (v.z < 0 /*|| v.z < ftDrivenDesc.ftModelDesc.coneTheta1*/) - result = ftDrivenDesc.ftModelDesc.distMax + 1; - - context.write("joint pose <%f %f %f> mug frame <%f %f %f> v <%f %f %f> distance=%f result=%f density=%f\n", - pose.p.x, pose.p.y, pose.p.z, (*pBelief->getHypotheses().begin())->sample.p.x, (*pBelief->getHypotheses().begin())->sample.p.y, (*pBelief->getHypotheses().begin())->sample.p.z, - v.x, v.y, v.z, pose.p.distance(median), result, norm*density(result)); - //else { - // Mat33 rot; - // Real roll, pitch, yaw; - // Quat quat(pose.q); - // quat.toMat33(rot); - // rot.toEuler(roll, pitch, yaw); - // std::printf("nearest(): pose [%.4f,%.4f,%.4f]<%.4f,%.4f,%.4f>; median <%.4f,%.4f,%.4f>, thx %.4f, thy %.4f\n", - // roll, pitch, yaw, pose.p.x, pose.p.y, pose.p.z, median.x, median.y, median.z, thx, thy); - //} - } - else { - context.write("joint pose <%f %f %f> mug frame <%f %f %f> distance=%f result=%f density=%f\n", - pose.p.x, pose.p.y, pose.p.z, (*pBelief->getHypotheses().begin())->sample.p.x, (*pBelief->getHypotheses().begin())->sample.p.y, (*pBelief->getHypotheses().begin())->sample.p.z, - pose.p.distance(median), result, norm*density(result)); - } - return result; -} - - -//------------------------------------------------------------------------------ - -void spam::XMLData(FTDrivenHeuristic::FTModelDesc& val, golem::XMLContext* context, bool create) { - golem::XMLData("dist_max", val.distMax, context, create); - golem::XMLData("cone_theta_orizontal_axis", val.coneTheta1, context, create); - golem::XMLData("cone_theta_vertical_axis", val.coneTheta2, context, create); - golem::XMLData("num_nearest_points", val.k, context, create); - golem::XMLData("num_points", val.points, context, create); - golem::XMLData("mahalanobis_fac", val.mahalanobisDistFac, context, create); - //golem::XMLData("enable_arm_chain", val.enabledArmChain, context, create); - //golem::XMLData("enable_hand_joints", val.enabledHandJoints, context, create); - // golem::XMLData("enabled_steps", val.enabledSteps, context, create); - golem::XMLData("enabled_likelihood", val.enabledLikelihood, context, create); - golem::XMLData("intrinsic_exp_parameter", val.lambda, context, create); - golem::XMLData(val.jointFac, context->getContextFirst("joint_fac"), create); -} - -void spam::XMLData(FTDrivenHeuristic::Desc& val, golem::XMLContext* context, bool create) { - // golem::XMLData((golem::Heuristic::Desc)val, context, create); - // spam::XMLData(val.beliefDesc, context->getContextFirst("belief_space"), create); - golem::XMLData("contact_fac", val.contactFac, context, create); - golem::XMLData("non_contact_fac", val.nonContactFac, context, create); - golem::XMLData("max_surface_points_inkd", val.maxSurfacePoints, context, create); - spam::XMLData(val.ftModelDesc, context->getContextFirst("ftmodel"), create); - golem::XMLData(&val.covariance[0], &val.covariance[grasp::RBCoord::N], "c", context->getContextFirst("covariance"), create); - XMLData(val.evaluationDesc, context->getContextFirst("evaluation_model"), create); - XMLData(val.checkDesc, context->getContextFirst("check_model"), create); -} diff --git a/src/Spam/Spam/Robot.cpp b/src/Spam/Spam/Robot.cpp deleted file mode 100644 index e63a09e..0000000 --- a/src/Spam/Spam/Robot.cpp +++ /dev/null @@ -1,761 +0,0 @@ -/** @file Robot.cpp - * - * @author Claudio Zito (The University Of Birmingham) - * - * @version 1.0 - * - */ - -#include -#include -//#include -//#include -//#include -//#include -#include -#include -#include - -#ifdef WIN32 - #pragma warning (push) - #pragma warning (disable : 4244) - #pragma warning (disable : 4996) -#endif -#include -#include -#ifdef WIN32 - #pragma warning (pop) -#endif - -#ifdef _GRASP_HITHANDII_DEBUG -#include -#endif // _GRASP_HITHANDII_DEBUG - -using namespace golem; -using namespace spam; - -//------------------------------------------------------------------------------ - -std::string spam::controllerDebug(grasp::ActiveCtrl &ctrl) { - std::stringstream str; - - //const Heuristic& heuristic = planner.getHeuristic(); - //FTDrivenHeuristic *pHeuristic = dynamic_cast(&planner.getHeuristic()); - //const Controller& controller = planner.getController(); - //const Heuristic::JointDesc::JointSeq& jointDesc = heuristic.getJointDesc(); - //const Chainspace::Range chains = controller.getStateInfo().getChains(); - //golem::U32 enabled = 0; - //for (Chainspace::Index i = chains.begin(); i < chains.end(); ++i) { - // const Configspace::Range joints = controller.getStateInfo().getJoints(i); - // for (Configspace::Index j = joints.begin(); j < joints.end(); ++j) - // if (jointDesc[j]->enabled) ++enabled; - //} - - //str << controller.getName() << ": chains=" << controller.getStateInfo().getChains().size() << ", joints=" << controller.getStateInfo().getJoints().size() << "(enabled=" << enabled << "), collisions=" << (heuristic.getDesc().collisionDesc.enabled ? "ENABLED" : "DISABLED") << ", non-Euclidian metrics=" << (pHeuristic && pHeuristic->enableUnc ? "ENABLE" : "DISABLE") << ", point cloud collisions=" << (pHeuristic && pHeuristic->getPointCloudCollision() ? "ENABLE" : "DISABLE"); - - return str.str(); -} - -//------------------------------------------------------------------------------ - -Robot::Robot(Scene &scene) : grasp::Robot(scene) { -} - -bool Robot::create(const Desc& desc) { - grasp::Robot::create(desc); // throws - - this->desc = desc; - // overwrite the heuristic - pFTDrivenHeuristic = dynamic_cast(&planner->getHeuristic()); - - steps = 0; - windowSize = 40; - // compute guassian pdf over x=[-2:.1:2] - // vector index - grasp::RealSeq v; - v.assign(windowSize + 1, REAL_ZERO); - const Real delta(.1); - Real i = -2; - for (I32 j = 0; j < windowSize + 1; j++) { - v[j] = N(i, REAL_ONE); - i += delta; - } - - // compute the derivative mask=diff(v) - mask.assign(windowSize, REAL_ZERO); - for (I32 i = 0; i < windowSize; ++i) - mask[i] = v[i + 1] - v[i]; - - forceInpSensorSeq.resize(dimensions()); - for (std::vector::iterator i = forceInpSensorSeq.begin(); i != forceInpSensorSeq.end(); ++i) - i->assign(windowSize, REAL_ZERO); - handFilteredForce.assign(dimensions(), REAL_ZERO); - - triggeredGuards.clear(); - triggeredGuards.reserve(fLimit.size()); - - //objectPointCloudPtr.reset(new grasp::Cloud::PointSeq()); - //manipulator.reset(); - //w.setToDefault(); - //w.points = 5000;// desc.maxModelPoints; - //collision.reset(); - - //desc.handCtrlDesc->forceReader = [=](const golem::Controller::State& state, grasp::RealSeq& force) { - // // no force by default - // force.assign(state.getInfo().getJoints().size(), REAL_ZERO); - // // use simulated force - // if (simHandForceMode != FORCE_MODE_DISABLED) { - // const Vec3 simForce(simModeVec.z*simForceGain.v.z, simModeVec.x*simForceGain.v.x, simModeVec.y*simForceGain.v.y); - - // Chainspace::Index i = state.getInfo().getChains().begin() + simHandForceMode - 1; // simHandForceMode corresponds to chain - // for (Configspace::Index j = state.getInfo().getJoints(i).begin(); j < state.getInfo().getJoints(i).end(); ++j) { - // const size_t k = j - state.getInfo().getJoints().begin(); // from the first joint - // const size_t l = j - state.getInfo().getJoints(i).begin(); // from the first joint in the chain - // //if (!objectPointCloudPtr->empty()) - // // context.write("Chainspace::Index i=%d, j=%d, from first joint k=%d, from first joint in chain l=%d, triggerred guard=%d (%d)\n", i, j, k, l, joints.front(), joints.size()); - // force[k] = simForce[std::min(size_t(2), l)]; - // } - // } - // // read from the state variable (if supported) - // else { - // try { - // const ptrdiff_t forceOffset = hand->getReservedOffset(Controller::RESERVED_INDEX_FORCE_TORQUE); - // std::vector joints; - // Real value = !objectPointCloudPtr->empty() ? collision->evaluate(this, w, *objectPointCloudPtr.get(), rand, manipulator->getPose(recvState().command), joints)*5. : REAL_ZERO; - // if (forceOffset != Controller::ReservedOffset::UNAVAILABLE) { - // for (Configspace::Index j = state.getInfo().getJoints().begin(); j < state.getInfo().getJoints().end(); ++j) { - // const size_t k = j - state.getInfo().getJoints().begin(); - // force[k] = state.get(forceOffset)[j]; - // } - // for (auto j = joints.begin(); j != joints.end(); ++j) { - // const size_t k = (*j) - state.getInfo().getJoints().begin(); - // if (k > force.size()) { - // context.write("k=%d > force.size()=%d\n", k, force.size()); - // continue; - // } - // force[k] = value; - // } - // if (!joints.empty()) { - // const size_t k = joints.front() - state.getInfo().getJoints().begin(); - // //context.write("-----------COLLISSION JOINT=%d k=%d force=%3.3f limit=%3.3f (size=%d)--------------\n", joints.front(), k, - // // force[k], fLimit[k], joints.size()); - // } - // } - // } - // catch (const Message &msg) { context.write("%s\n", msg.str()); } - // } - - // bool emergency = false; - // try { - // golem::CriticalSectionWrapper csw(csData); - // // use customised force reader if available, otherwise the default one - // handForceReader ? handForceReader(state, force) : handForceReaderDflt(state, force); - // } - // catch (const std::exception& ex) { - // // if force threshold limit is reached - // if (handCtrl->getMode() != grasp::ActiveCtrl::MODE_EMERGENCY) { - // emergency = true; - // // print exception - // context.write("%s\n", ex.what()); - // // set emergency mode - // armCtrl->setMode(grasp::ActiveCtrl::MODE_EMERGENCY); - // handCtrl->setMode(grasp::ActiveCtrl::MODE_EMERGENCY); - // // stop controller and cleanup the command queue - // controller->stop(); - // } - // } - - // { - // golem::CriticalSectionWrapper csw(csData); - // handForce = force; - // // emergency mode handler - // if (emergency && emergencyModeHandler) emergencyModeHandlerThread.start(emergencyModeHandler); - // } - //}; - //handCtrl = desc.handCtrlDesc->create(*hand); // create and install callback (throws) - - collectFTInp = [=](const Controller::State&, grasp::RealSeq& force) { - golem::CriticalSectionWrapper csw(csHandForce); - for (I32 i = 0; i < dimensions(); ++i) - forceInpSensorSeq[i][steps] = force[i]; - steps = (I32)(++steps % windowSize); - }; - - ftFilter = [=](const Controller::State&, grasp::RealSeq& force) { - // find index as for circular buffer - auto findIndex = [&](const I32 idx) -> I32 { - return (I32)((steps + idx) % windowSize); - }; - // high pass filter implementation - auto hpFilter = [&]() { - Real b = 1 / windowSize; - - }; - // gaussian filter - auto conv = [&]() -> grasp::RealSeq { - grasp::RealSeq output; - output.assign(dimensions(), REAL_ZERO); - { - golem::CriticalSectionWrapper csw(csHandForce); - for (I32 i = 0; i < dimensions(); ++i) { - Real tmp = REAL_ZERO; - grasp::RealSeq& seq = forceInpSensorSeq[i]; - // conv y[k] = x[k]h[0] + x[k-1]h[1] + ... + x[0]h[k] - for (I32 j = 0; j < windowSize; ++j) { - tmp += seq[findIndex(windowSize - j - 1)] * mask[j]; - } - output[i] = tmp; - } - } - return output; - }; - - force = conv(); - }; - - handContactDetector = nullptr; - - guardsReader = [=](const Controller::State &state, grasp::RealSeq& force, std::vector &joints) { - joints.clear(); - joints.reserve(getStateHandInfo().getJoints().size()); - ftFilter(state, handFilteredForce); - // the loop skips the last joint because it's coupled with the 3rd joint. - for (Configspace::Index i = getStateHandInfo().getJoints().begin(); i != getStateHandInfo().getJoints().end(); ++i) { - const size_t k = i - getStateHandInfo().getJoints().begin(); - if (Math::abs(handFilteredForce[k]) > fLimit[k]) - joints.push_back(i); - } - //for (size_t i = 0; i < size; ++i) - // if (Math::abs(force[i]) > fLimit[i]) - // throw Message(Message::LEVEL_NOTICE, "Robot::handForceReaderDflt(): F[%u/%u] = (%3.3lf)", i, size, force[i]); - }; - - return true; -} - -//------------------------------------------------------------------------------ - -void Robot::render() { - grasp::Robot::render(); -} - -//------------------------------------------------------------------------------ - -void Robot::findTarget(const golem::Mat34 &trn, const golem::Controller::State &target, golem::Controller::State &cend, const bool lifting) { - // arm chain and joints pointers - const golem::Chainspace::Index armChain = armInfo.getChains().begin(); - const golem::Configspace::Range armJoints = armInfo.getJoints(); - // Compute a sequence of targets corresponding to the transformed arm end-effector - GenWorkspaceChainState gwcs; - controller->chainForwardTransform(target.cpos, gwcs.wpos); - gwcs.wpos[armChain].multiply(gwcs.wpos[armChain], controller->getChains()[armChain]->getReferencePose()); // 1:1 - gwcs.wpos[armChain].multiply(trn, gwcs.wpos[armChain]); // new waypoint frame - gwcs.t = target.t; -// gwcs.wpos[armChain].p.x += 0.045; - if (lifting) gwcs.wpos[armChain].p.z += 0.07; - - cend = target; - { - // lock controller - golem::CriticalSectionWrapper csw(csController); - // Find initial target position - if (!planner->findTarget(target, gwcs, cend)) - throw Message(Message::LEVEL_ERROR, "spam::Robot::findTarget(): Unable to find initial target configuration"); - } -// context.write(">\n"); //std::cout << ">\n"; //context.write(">\n"); - // set fingers pose - for (auto i = handInfo.getJoints().begin(); i != handInfo.getJoints().end(); ++i) - cend.cpos[i] = target.cpos[i]; - - // update arm configurations and compute average error - grasp::RBDist err; - WorkspaceChainCoord wcc; - controller->chainForwardTransform(cend.cpos, wcc); - wcc[armChain].multiply(wcc[armChain], controller->getChains()[armChain]->getReferencePose()); - err.add(err, grasp::RBDist(grasp::RBCoord(wcc[armChain]), grasp::RBCoord(gwcs.wpos[armChain]))); - context.write("Robot::findTarget(): Pose error: lin=%.9f, ang=%.9f\n", err.lin, err.ang); -} - -//------------------------------------------------------------------------------ - -//int Robot::checkGuards(std::vector &triggeredGuards, golem::Controller::State &state) { -// //golem::RobotJustin *justin = getRobotJustin(); -// //if (justin) -// // return justin->getTriggeredGuards(triggeredGuards, state); -// //else { -// //const ptrdiff_t forceOffset = hand->getReservedOffset(Controller::RESERVED_INDEX_FORCE_TORQUE); -// -// //context.write("spam::Robot::getTriggeredGuards(): retrieving forces from state\n"); -// //grasp::RealSeq force; -// //force.assign(handInfo.getJoints().size(), REAL_ZERO); -// //if (forceOffset != Controller::ReservedOffset::UNAVAILABLE) { -// // for (Configspace::Index j = handInfo.getJoints().begin(); j < handInfo.getJoints().end(); ++j) { -// // const size_t k = j - handInfo.getJoints().begin(); -// // const size_t z = j - armInfo.getJoints().begin(); -// // force[k] = state.get(forceOffset)[j]; -// // if (Math::abs(state.get(forceOffset)[j]) > ftHandGuards[k]) { -// // context.write("hand joint %d force=%f guard=%f\n", k, force[k], ftHandGuards[k]); -// // triggeredGuards.push_back(z); -// // } -// // } -// //} -// //return triggeredGuards.size(); -// //} -// return triggeredGuards.empty() ? -1 : triggeredGuards.size(); -// -//} -// -//int Robot::getTriggeredGuards(FTGuard::Seq &triggeredJoints, golem::Controller::State &state) { -// triggeredJoints.clear(); -// triggeredJoints.reserve(ftGuards.size()); -// -// const int NUM_JOINTS_CTRL = handInfo.getJoints(handInfo.getChains().begin()).size() - 1; -// int ret = 0; -// // in case justin moves with enable guards -// golem::RobotJustin *justin = getRobotJustin(); -// if (justin) { -// std::vector triggeredIndeces; -// triggeredIndeces.reserve(ftGuards.size()); -// if ((ret = justin->getTriggeredGuards(triggeredIndeces, state)) > 0 /*|| !staticObject*/) { -// context.write("spam::Robot::getTriggeredGuards(): num=%d triggered joint(s):", triggeredIndeces.size()); -// for (std::vector::const_iterator i = triggeredIndeces.begin(); i != triggeredIndeces.end(); ++i) { -// triggeredJoints.push_back(ftGuards[*i]); -// context.write(" ", ftGuards[*i].chainIdx, ftGuards[*i].jointIdx); -// } -// context.write("\n"); -//// return triggeredJoints.size(); // return ret; -// } -// } -// else { -// // in case robot bham -// for (FTGuard::Seq::const_iterator i = triggeredGuards.begin(); i != triggeredGuards.end(); ++i) -// triggeredJoints.push_back(*i); -// -// //const ptrdiff_t forceOffset = hand->getReservedOffset(Controller::RESERVED_INDEX_FORCE_TORQUE); -// -// //context.write("spam::Robot::getTriggeredGuards(): retrieving forces from state. triggered joint(s): joints=%d guards=%d\n", handInfo.getJoints().size(), ftGuards.size()); -// //grasp::RealSeq force; -// //force.assign(handInfo.getJoints().size(), REAL_ZERO); -// //if (forceOffset != Controller::ReservedOffset::UNAVAILABLE) { -// // for (Configspace::Index j = handInfo.getJoints().begin(); j < handInfo.getJoints().end(); ++j) { -// // const size_t k = j - handInfo.getJoints().begin(); -// // force[k] = state.get(forceOffset)[j]; -// // if (state.get(forceOffset)[j] < ftGuards[2*k].value) { -// // context.write("hand chain %d joint %d (k=%d) force=%f guard=%f\n", ftGuards[2*k].chainIdx, ftGuards[2*k].joint, k, force[k], ftGuards[2*k].value); -// // triggeredJoints.push_back(ftGuards[2*k]); -// // } -// // else if (state.get(forceOffset)[j] > ftGuards[2*k+1].value) { -// // context.write("hand chain %d joint %d (k=%d) force=%f guard=%f\n", ftGuards[2*k+1].chainIdx, ftGuards[2*k+1].joint, k, force[k], ftGuards[2*k+1].value); -// // triggeredJoints.push_back(ftGuards[2*k+1]); -// // } -// // } -// //} -// ret = triggeredJoints.size(); -// } -// // if (!triggeredJoints.empty()) -// // return true; -// //} -// context.write("spam::Robot::getTriggeredGuards(): %d triggered guard(s).\n", ret); -// return ret; -//} -// -void Robot::readFT(const Controller::State &state, grasp::RealSeq &force) const { - // use simulated force - if (simHandForceMode != FORCE_MODE_DISABLED) { - const Vec3 simForce(simModeVec.z*simForceGain.v.z, simModeVec.x*simForceGain.v.x, simModeVec.y*simForceGain.v.y); - - Chainspace::Index i = handInfo.getChains().begin(); // simHandForceMode corresponds to chain - for (Configspace::Index j = handInfo.getJoints(i).begin(); j < handInfo.getJoints(i).end(); ++j) { - const size_t k = j - handInfo.getJoints().begin(); // from the first joint - const size_t l = j - handInfo.getJoints(i).begin(); // from the first joint in the chain - force[k] = simForce[std::min(size_t(2), l)]; - } - } - // read from the state variable (if supported) - else { - const ptrdiff_t forceOffset = hand->getReservedOffset(Controller::RESERVED_INDEX_FORCE_TORQUE); - if (forceOffset != Controller::ReservedOffset::UNAVAILABLE) { - Real max = REAL_ZERO; - Real min = REAL_ZERO; - static int jj = 0; - for (Configspace::Index j = handInfo.getJoints().begin(); j < handInfo.getJoints().end(); ++j) { - const size_t k = j - handInfo.getJoints().begin(); - force[k] = state.get(forceOffset)[j]; - if (force[k] > max) - max = force[k]; - if (force[k] < min) - min = force[k]; - } - //if (++jj % 100 == 0) - // printf("[%f/%f]\n", max, min); - } - } -} -// -//} -// -////size_t Robot::isGrasping(std::vector &triggeredJoints, golem::Controller::State &state) { -//// // in case justin tried to grasp with no guards enabled -//// triggeredJoints.clear(); -//// const ptrdiff_t forceOffset = hand->getReservedOffset(Controller::RESERVED_INDEX_FORCE_TORQUE); -//// state = recvState().config; -//// -//// context.write("spam::Robot::isGrasping(): retrieving forces from state (hand joints size %d, guards size %d)\n", handInfo.getJoints().size(), ftHandGuards.size()); -//// grasp::RealSeq force; -//// readFT(state, force); -//// force.assign(handInfo.getJoints().size(), REAL_ZERO); -//// if (forceOffset != Controller::ReservedOffset::UNAVAILABLE) { -//// for (Configspace::Index j = handInfo.getJoints().begin(); j < handInfo.getJoints().end(); ++j) { -//// const size_t k = j - handInfo.getJoints().begin(); -//// //force[k] = state.get(forceOffset)[j]; -//// //if (Math::abs(state.get(forceOffset)[j]) > ftHandGuards[k]) { -//// if (Math::abs(force[k]) > ftHandGuards[k]) { -//// context.write("hand joint %d force=%f guard=%f\n", k, force[k], ftHandGuards[k]); -//// triggeredJoints.push_back(j); -//// } -//// } -//// } -//// context.write("robot:isGrasping: done./n"); -//// return triggeredJoints.size(); -//// -//// // Read forces at the hand -//// //const ptrdiff_t forceOffset = hand->getReservedOffset(Controller::RESERVED_INDEX_FORCE_TORQUE); -//// //const Controller::State s = recvState().config; -//// // -//// //grasp::RealSeq force; -//// //force.assign(s.getInfo().getJoints().size(), REAL_ZERO); -//// //if (forceOffset != Controller::ReservedOffset::UNAVAILABLE) { -//// // for (Configspace::Index j = s.getInfo().getJoints().begin(); j < s.getInfo().getJoints().end(); ++j) { -//// // const size_t k = j - s.getInfo().getJoints().begin(); -//// // force[k] = s.get(forceOffset)[j]; -//// // } -//// //} -//// //for (grasp::RealSeq::const_iterator i = force.begin(), j = ftHandGuards.begin(); i != force.end() && j != ftHandGuards.end(); ++i, ++j) -//// // if (Math::abs(*i) >= *j ) -//// // return true; -//// // -//// //return false; -////} -// -//size_t Robot::isGrasping(FTGuard::Seq &triggeredJoints, golem::Controller::State &state) { -// // in case justin tried to grasp with no guards enabled -// triggeredJoints.clear(); -// const ptrdiff_t forceOffset = hand->getReservedOffset(Controller::RESERVED_INDEX_FORCE_TORQUE); -// state = recvState().config; -// -// context.write("spam::Robot::isGrasping(): retrieving forces from state (hand joints size %d, guards size %d)\n", handInfo.getJoints().size(), ftHandGuards.size()); -// grasp::RealSeq force; -// force.assign(handInfo.getJoints().size(), REAL_ZERO); -// size_t res = 0; // counts the triggered guards. but leaves triggeredjoint empty to avoid the belief update. -// if (forceOffset != Controller::ReservedOffset::UNAVAILABLE) { -// for (Configspace::Index j = handInfo.getJoints().begin(); j < handInfo.getJoints().end(); ++j) { -// const size_t k = j - handInfo.getJoints().begin(); -// force[k] = state.get(forceOffset)[j]; -// if (Math::abs(state.get(forceOffset)[j]) > ftHandGuards[k]) { -// context.write("hand joint %d force=%f guard=%f\n", k, force[k], ftHandGuards[k]); -// res++; -//// triggeredJoints.push_back(j); -// // TODO generate ftguards for each contact -// } -// } -// } -// -// return res; //triggeredJoints.size(); -//} - -void Robot::stopActiveController() { - context.write("Disabled active controller\n"); - if (armCtrl->getMode() != grasp::ActiveCtrl::MODE_DISABLED) armCtrl.get()->setMode(grasp::ActiveCtrl::MODE_DISABLED); - if (handCtrl->getMode() != grasp::ActiveCtrl::MODE_DISABLED) handCtrl.get()->setMode(grasp::ActiveCtrl::MODE_DISABLED); - //simHandForceMode = FORCE_MODE_DISABLED; - //simArmForceMode = FORCE_MODE_DISABLED; - //workspaceMode = WORKSPACE_MODE_DISABLED; -} - -void Robot::startActiveController(const golem::I32 steps) { - context.write("Enabled active controller\n"); - if (armCtrl->getMode() != grasp::ActiveCtrl::MODE_ENABLED) armCtrl.get()->setMode(grasp::ActiveCtrl::MODE_ENABLED); - if (handCtrl->getMode() != grasp::ActiveCtrl::MODE_ENABLED) handCtrl.get()->setMode(grasp::ActiveCtrl::MODE_ENABLED, steps == grasp::ActiveCtrl::STEP_DEFAULT ? grasp::ActiveCtrl::STEP_DEFAULT : steps); - //simHandForceMode = FORCE_MODE_DISABLED; - //simArmForceMode = FORCE_MODE_DISABLED; - //workspaceMode = WORKSPACE_MODE_DISABLED; -} - -void Robot::emergencyActiveController() { - if (handCtrl->getMode() != grasp::ActiveCtrl::MODE_EMERGENCY) { - // set emergency mode - armCtrl->setMode(grasp::ActiveCtrl::MODE_EMERGENCY); - handCtrl->setMode(grasp::ActiveCtrl::MODE_EMERGENCY); - // stop controller and cleanup the command queue - controller->stop(); - } -} - - -//------------------------------------------------------------------------------ - -grasp::RBDist Robot::trnTrajectory(const golem::Mat34& actionFrame, const golem::Mat34& modelFrame, const golem::Mat34& trn, golem::Controller::State::Seq::const_iterator begin, golem::Controller::State::Seq::const_iterator end, golem::Controller::State::Seq& trajectory) { - if (begin == end) - throw Message(Message::LEVEL_ERROR, "Robot::createTrajectory(2): Empty input trajectory"); - // arm chain and joints pointers - const golem::Chainspace::Index armChain = armInfo.getChains().begin(); - const golem::Configspace::Range armJoints = armInfo.getJoints(); - // Compute a sequence of targets corresponding to the transformed arm end-effector - GenWorkspaceChainState::Seq seq; - //for (Controller::State::Seq::iterator i = trajectory.begin(); i != trajectory.end(); ++i) { - // GenWorkspaceChainState gwcs; - // controller->chainForwardTransform(i->cpos, gwcs.wpos); - // gwcs.wpos[armChain].multiply(gwcs.wpos[armChain], controller->getChains()[armChain]->getReferencePose()); // 1:1 - // gwcs.t = i->t; - // seq.push_back(gwcs); - //} - //trajectory.clear(); - - for (Controller::State::Seq::const_iterator i = begin; i != end; ++i) { - GenWorkspaceChainState gwcs; - controller->chainForwardTransform(i->cpos, gwcs.wpos); - gwcs.wpos[armChain].multiply(gwcs.wpos[armChain], controller->getChains()[armChain]->getReferencePose()); // 1:1 - // define the grasp frame - Mat34 poseFrameInv, graspFrame, graspFrameInv; - poseFrameInv.setInverse(gwcs.wpos[armChain]); - graspFrame.multiply(poseFrameInv, actionFrame * modelFrame); - graspFrameInv.setInverse(graspFrame); - gwcs.wpos[armChain].multiply(modelFrame, graspFrameInv); -// context.write("trnTrajectory(): grasp frame at model <%f %f %f>\n", gwcs.wpos[armChain].p.x, gwcs.wpos[armChain].p.y, gwcs.wpos[armChain].p.z); - gwcs.wpos[armChain].multiply(trn, gwcs.wpos[armChain]); // new waypoint frame -// context.write("trnTrajectory(): grasp frame at new query <%f %f %f>\n", gwcs.wpos[armChain].p.x, gwcs.wpos[armChain].p.y, gwcs.wpos[armChain].p.z); - gwcs.t = i->t; - seq.push_back(gwcs); - } - // planner debug - //context.verbose("%s\n", plannerDebug(*planner).c_str()); - Controller::State::Seq ctrajectory; - { - // lock controller - golem::CriticalSectionWrapper csw(csController); - // Find initial target position - Controller::State cend = *begin; - // planner debug - //context.verbose("%s\n", plannerWorkspaceDebug(*planner, &seq[0].wpos).c_str()); - if (!planner->findTarget(*begin, seq[0], cend)) - throw Message(Message::LEVEL_ERROR, "Robot::createTrajectory(2): Unable to find initial target configuration"); - // Find remaining position sequence - if (seq.size() == 1) - ctrajectory.push_back(cend); - else if (seq.size() > 1 && !planner->findLocalTrajectory(cend, ++seq.begin(), seq.end(), ctrajectory, ctrajectory.end())) - throw Message(Message::LEVEL_ERROR, "Robot::createTrajectory(2): Unable to find trajectory"); - } - // update arm configurations and compute average error - grasp::RBDist err; - Controller::State::Seq::const_iterator j = begin; - for (size_t i = 0; i < ctrajectory.size(); ++i, ++j) { - // copy config - trajectory.push_back(*j); - trajectory.back().set(armInfo.getJoints().begin(), armInfo.getJoints().end(), ctrajectory[i]); - // error - WorkspaceChainCoord wcc; - controller->chainForwardTransform(trajectory.back().cpos, wcc); - wcc[armChain].multiply(wcc[armChain], controller->getChains()[armChain]->getReferencePose()); - err.add(err, grasp::RBDist(grasp::RBCoord(wcc[armChain]), grasp::RBCoord(seq[i].wpos[armChain]))); - } - context.debug("Robot::createTrajectory(2): Pose error: lin=%.9f, ang=%.9f\n", err.lin, err.ang); - - return err; -} - -grasp::RBDist Robot::findTrnTrajectory(const golem::Mat34& trn, const golem::Controller::State& startPose, golem::Controller::State::Seq::const_iterator begin, golem::Controller::State::Seq::const_iterator end, golem::Controller::State::Seq& trajectory) { - if (begin == end) - throw Message(Message::LEVEL_ERROR, "Robot::transformTrajectory(): Empty input trajectory"); - // arm chain and joints pointers - const golem::Chainspace::Index armChain = armInfo.getChains().begin(); - const golem::Configspace::Range armJoints = armInfo.getJoints(); - // Compute a sequence of targets corresponding to the transformed arm end-effector - GenWorkspaceChainState::Seq seq; - //// the starting pose of the robot does not need a tranformation - //GenWorkspaceChainState gStart; - //controller->chainForwardTransform(startPose.cpos, gStart.wpos); - //gStart.wpos[armChain].multiply(gStart.wpos[armChain], controller->getChains()[armChain]->getReferencePose()); // 1:1 - //gStart.t = startPose.t; - //seq.push_back(gStart); - for (Controller::State::Seq::const_iterator i = begin; i != end; ++i) { - GenWorkspaceChainState gwcs; - controller->chainForwardTransform(i->cpos, gwcs.wpos); - gwcs.wpos[armChain].multiply(gwcs.wpos[armChain], controller->getChains()[armChain]->getReferencePose()); // 1:1 - gwcs.wpos[armChain].multiply(trn, gwcs.wpos[armChain]); // new waypoint frame - gwcs.t = i->t; - seq.push_back(gwcs); - } - // planner debug - //context.verbose("%s\n", plannerDebug(*planner).c_str()); - Controller::State::Seq initTrajectory, ctrajectory; - { - // lock controller - golem::CriticalSectionWrapper csw(csController); - // Find initial target position - Controller::State cend = *begin; - // planner debug - //context.debug("Seq[0]: %s\n", grasp::plannerWorkspaceDebug(*planner, &seq[0].wpos).c_str()); - //context.debug("Seq[2]: %s\n", grasp::plannerWorkspaceDebug(*planner, &seq[2].wpos).c_str()); - if (!planner->findTarget(*begin, seq[0], cend)) - throw Message(Message::LEVEL_ERROR, "Robot::transformTrajectory(): Unable to find initial target configuration"); - // compute the initial trajectory to move thee robot from current pose to the beginning of the approach trajectory - if (!planner->findGlobalTrajectory(startPose, cend, initTrajectory, initTrajectory.end())) - throw Message(Message::LEVEL_ERROR, "Robot::transformTrajectory(): Unable to find initial trajectory"); - // Find remaining position sequence - if (seq.size() == 1) - ctrajectory.push_back(cend); - else if (seq.size() > 1 && !planner->findLocalTrajectory(cend, ++seq.begin(), seq.end(), ctrajectory, ctrajectory.end())) - throw Message(Message::LEVEL_ERROR, "Robot::transformTrajectory(): Unable to find trajectory"); - } - // update arm configurations and compute average error - trajectory.insert(trajectory.end(), initTrajectory.begin(), initTrajectory.end()); - grasp::RBDist err; - Controller::State::Seq::const_iterator j = begin; - for (size_t i = 0; i < ctrajectory.size(); ++i, ++j) { - // copy config - trajectory.push_back(*j); - trajectory.back().set(armInfo.getJoints().begin(), armInfo.getJoints().end(), ctrajectory[i]); - // error - WorkspaceChainCoord wcc; - controller->chainForwardTransform(trajectory.back().cpos, wcc); - wcc[armChain].multiply(wcc[armChain], controller->getChains()[armChain]->getReferencePose()); - err.add(err, grasp::RBDist(grasp::RBCoord(wcc[armChain]), grasp::RBCoord(seq[i].wpos[armChain]))); - } - context.debug("Robot::transformTrajectory(): Pose error: lin=%.9f, ang=%.9f\n", err.lin, err.ang); - return err; -} - - -void Robot::createTrajectory(const golem::Controller::State& begin, const golem::Controller::State* pcend, const golem::Mat34* pwend, golem::SecTmReal t, const golem::Controller::State::Seq& waypoints, golem::Controller::State::Seq& trajectory) { - if (!pcend && !pwend) - throw Message(Message::LEVEL_ERROR, "Robot::findTrajectory(): no target specified"); - - // Trajectory from initial position to end position - for (golem::U32 i = 0; i < trajectoryTrials; ++i) { - if (universe.interrupted()) - throw grasp::Interrupted(); -// context.debug("Robot::findTrajectory(): Planning movement...\n"); - // lock controller - golem::CriticalSectionWrapper csw(csController); - // All bounds are treated as obstacles - physPlanner->setCollisionBoundsGroup(Bounds::GROUP_ALL); - // planner debug - //context.verbose("%s\n", plannerDebug(*planner).c_str()); - - // Setup configspace target - Controller::State cend = pcend ? *pcend : begin; - // Workspace target - if (pwend) { - // Setup workspace target - GenWorkspaceChainState wend; - wend.setToDefault(info.getChains().begin(), info.getChains().end()); // all used chains - wend.wpos[armInfo.getChains().begin()] = *pwend; - // planner debug - //context.verbose("%s\n", plannerWorkspaceDebug(*planner, &wend.wpos).c_str()); - if (!planner->findTarget(begin, wend, cend)) - continue; - // update configspace coords of the hand - if (pcend) cend.cpos.set(handInfo.getJoints(), pcend->cpos); - // error - WorkspaceChainCoord wcc; - controller->chainForwardTransform(cend.cpos, wcc); - wcc[armInfo.getChains().begin()].multiply(wcc[armInfo.getChains().begin()], controller->getChains()[armInfo.getChains().begin()]->getReferencePose()); - grasp::RBDist err; - err.set(grasp::RBCoord(*pwend), grasp::RBCoord(wcc[armInfo.getChains().begin()])); - context.debug("Robot::findTrajectory(): Pose error: lin=%.9f, ang=%.9f\n", err.lin, err.ang); - } - - // planner debug - //context.verbose("%s\n", plannerConfigspaceDebug(*planner, &cend.cpos).c_str()); - // Find collision-free trajectory and wait until the device is ready for new commands - cend.t = begin.t + (t > SEC_TM_REAL_ZERO ? t : trajectoryDuration); - if (planner->findGlobalTrajectory(begin, cend, trajectory, trajectory.begin())) - return;// success - } - - throw Message(Message::LEVEL_ERROR, "Robot::findTrajectory(): unable to find trajectory"); -} - - -//------------------------------------------------------------------------------ - -void spam::XMLData(Robot::Desc &val, golem::Context* context, golem::XMLContext* xmlcontext, bool create) { - // grasp::XMLData((grasp::Robot::Desc&)val, context, xmlcontext, create); - RagGraphPlanner::Desc* pRagGraphPlanner(new RagGraphPlanner::Desc); - (golem::Planner::Desc&)*pRagGraphPlanner = *val.physPlannerDesc->pPlannerDesc; - val.physPlannerDesc->pPlannerDesc.reset(pRagGraphPlanner); - val.physPlannerDesc->pPlannerDesc = RagGraphPlanner::Desc::load(context, xmlcontext->getContextFirst("planner")); - - FTDrivenHeuristic::Desc* pFTDrivenHeuristic(new FTDrivenHeuristic::Desc); - (golem::Heuristic::Desc&)*pFTDrivenHeuristic = *val.physPlannerDesc->pPlannerDesc->pHeuristicDesc; - val.physPlannerDesc->pPlannerDesc->pHeuristicDesc.reset(pFTDrivenHeuristic); - spam::XMLData((FTDrivenHeuristic::Desc&)*val.physPlannerDesc->pPlannerDesc->pHeuristicDesc, xmlcontext->getContextFirst("rag_planner heuristic"), create); -} - - - -//grasp::RBDist Robot::trnTrajectory(const golem::Mat34& actionFrame, const golem::Mat34& modelFrame, const golem::Mat34& trn, golem::Controller::State::Seq::const_iterator begin, golem::Controller::State::Seq::const_iterator end, golem::Controller::State::Seq& trajectory) { -// if (begin == end) -// throw Message(Message::LEVEL_ERROR, "Robot::createTrajectory(2): Empty input trajectory"); -// // arm chain and joints pointers -// const golem::Chainspace::Index armChain = armInfo.getChains().begin(); -// const golem::Configspace::Range armJoints = armInfo.getJoints(); -// // Compute a sequence of targets corresponding to the transformed arm end-effector -// GenWorkspaceChainState::Seq seq; -// for (Controller::State::Seq::iterator i = trajectory.begin(); i != trajectory.end(); ++i) { -// GenWorkspaceChainState gwcs; -// controller->chainForwardTransform(i->cpos, gwcs.wpos); -// gwcs.wpos[armChain].multiply(gwcs.wpos[armChain], controller->getChains()[armChain]->getReferencePose()); // 1:1 -// gwcs.t = i->t; -// seq.push_back(gwcs); -// } -// trajectory.clear(); -// -// for (Controller::State::Seq::const_iterator i = begin; i != end; ++i) { -// GenWorkspaceChainState gwcs; -// controller->chainForwardTransform(i->cpos, gwcs.wpos); -// gwcs.wpos[armChain].multiply(gwcs.wpos[armChain], controller->getChains()[armChain]->getReferencePose()); // 1:1 -// // define the grasp frame -// Mat34 poseFrameInv, graspFrame, graspFrameInv; -// poseFrameInv.setInverse(gwcs.wpos[armChain]); -// graspFrame.multiply(poseFrameInv, actionFrame * modelFrame); -// graspFrameInv.setInverse(graspFrame); -// gwcs.wpos[armChain].multiply(modelFrame, graspFrameInv); -// // context.write("trnTrajectory(): grasp frame at model <%f %f %f>\n", gwcs.wpos[armChain].p.x, gwcs.wpos[armChain].p.y, gwcs.wpos[armChain].p.z); -// gwcs.wpos[armChain].multiply(trn, gwcs.wpos[armChain]); // new waypoint frame -// // context.write("trnTrajectory(): grasp frame at new query <%f %f %f>\n", gwcs.wpos[armChain].p.x, gwcs.wpos[armChain].p.y, gwcs.wpos[armChain].p.z); -// } -// // planner debug -// //context.verbose("%s\n", plannerDebug(*planner).c_str()); -// Controller::State::Seq ctrajectory; -// { -// // lock controller -// golem::CriticalSectionWrapper csw(csController); -// // Find initial target position -// Controller::State cend = *begin; -// // planner debug -// //context.verbose("%s\n", plannerWorkspaceDebug(*planner, &seq[0].wpos).c_str()); -// if (!planner->findTarget(*begin, seq[0], cend)) -// throw Message(Message::LEVEL_ERROR, "Robot::createTrajectory(2): Unable to find initial target configuration"); -// // Find remaining position sequence -// if (seq.size() == 1) -// ctrajectory.push_back(cend); -// else if (seq.size() > 1 && !planner->findLocalTrajectory(cend, ++seq.begin(), seq.end(), ctrajectory, ctrajectory.end())) -// throw Message(Message::LEVEL_ERROR, "Robot::createTrajectory(2): Unable to find trajectory"); -// } -// // update arm configurations and compute average error -// grasp::RBDist err; -// Controller::State::Seq::const_iterator j = begin; -// for (size_t i = 0; i < ctrajectory.size(); ++i, ++j) { -// // copy config -// trajectory.push_back(*j); -// trajectory.back().set(armInfo.getJoints().begin(), armInfo.getJoints().end(), ctrajectory[i]); -// // error -// WorkspaceChainCoord wcc; -// controller->chainForwardTransform(trajectory.back().cpos, wcc); -// wcc[armChain].multiply(wcc[armChain], controller->getChains()[armChain]->getReferencePose()); -// err.add(err, grasp::RBDist(grasp::RBCoord(wcc[armChain]), grasp::RBCoord(seq[i].wpos[armChain]))); -// } -// context.write("Robot::createTrajectory(2): Pose error: lin=%.9f, ang=%.9f\n", err.lin, err.ang); -// return err; -//}