From 5937f2101490794a8e3a751e23be4f056cde9dc2 Mon Sep 17 00:00:00 2001 From: Yuya Nagamatsu Date: Sat, 4 Sep 2021 22:12:17 +0900 Subject: [PATCH 01/13] [ImpedanceController] fix sensor localPos rotation --- rtc/ImpedanceController/ImpedanceController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtc/ImpedanceController/ImpedanceController.cpp b/rtc/ImpedanceController/ImpedanceController.cpp index 49c750a6027..492af67a3b8 100644 --- a/rtc/ImpedanceController/ImpedanceController.cpp +++ b/rtc/ImpedanceController/ImpedanceController.cpp @@ -535,7 +535,7 @@ void ImpedanceController::calcForceMoment () if ( sensor ) { // real force sensor sensorR = sensor->link->R * sensor->localR; - sensorPos = sensor->link->p + sensorR * sensor->localPos; + sensorPos = sensor->link->p + sensor->link->R * sensor->localPos; } else if ( m_vfs.find(sensor_name) != m_vfs.end()) { // virtual force sensor if ( DEBUGP ) { From c4453f545ef3b7e21cd5f2362b7429a0581c4d10 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Tue, 16 Nov 2021 17:47:44 +0800 Subject: [PATCH 02/13] Fix util build on Ubuntu 20.04 - Look for opencv4 via pkg-config - Migrate some OpenCV 1 code to OpenCV >= 2 --- CMakeLists.txt | 5 ++- lib/util/GLsceneBase.h | 9 ++++-- rtc/CameraImageLoader/CameraImageLoader.cpp | 34 ++++++++++--------- rtc/CameraImageSaver/CameraImageSaver.cpp | 15 +++++++-- rtc/CameraImageViewer/CameraImageViewer.h | 4 +-- rtc/ColorExtractor/ColorExtractor.cpp | 6 +++- rtc/ColorExtractor/ColorExtractor.h | 2 +- rtc/JpegDecoder/JpegDecoder.cpp | 6 ++++ rtc/JpegEncoder/JpegEncoder.cpp | 6 ++++ rtc/RGB2Gray/RGB2Gray.cpp | 1 + rtc/RangeDataViewer/RangeDataViewer.h | 4 +-- rtc/ResizeImage/ResizeImage.cpp | 3 +- rtc/ResizeImage/ResizeImage.h | 2 +- rtc/RotateImage/RotateImage.cpp | 3 +- rtc/RotateImage/RotateImage.h | 2 +- rtc/UndistortImage/UndistortImage.cpp | 36 ++++++++++----------- rtc/UndistortImage/UndistortImage.h | 7 ++-- rtc/VideoCapture/camera.h | 3 +- rtc/VirtualCamera/VirtualCamera.h | 4 +-- util/simulator/CMakeLists.txt | 19 ++++------- 20 files changed, 101 insertions(+), 70 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cb21d372433..52d103bbaf8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -152,7 +152,10 @@ pkg_check_modules(OpenCV opencv) if (NOT OpenCV_FOUND) pkg_check_modules(OpenCV opencv-2.3.1) if (NOT OpenCV_FOUND) - message(WARNING "opencv not found") + pkg_check_modules(OpenCV opencv4) + if (NOT OpenCV_FOUND) + message(WARNING "opencv not found") + endif() endif() endif() pkg_check_modules(GLEW glew) diff --git a/lib/util/GLsceneBase.h b/lib/util/GLsceneBase.h index 34e5e086368..76913a388b7 100644 --- a/lib/util/GLsceneBase.h +++ b/lib/util/GLsceneBase.h @@ -6,8 +6,13 @@ #include #include //Open CV header -#include -#include +#include +#include +#ifndef CV_VERSION_EPOCH + #if CV_VERSION_MAJOR > 3 + #include + #endif +#endif #include #include #include diff --git a/rtc/CameraImageLoader/CameraImageLoader.cpp b/rtc/CameraImageLoader/CameraImageLoader.cpp index ee3aed4b058..4b82726dca4 100644 --- a/rtc/CameraImageLoader/CameraImageLoader.cpp +++ b/rtc/CameraImageLoader/CameraImageLoader.cpp @@ -7,8 +7,14 @@ * $Id$ */ -#include -#include +#ifndef CV_VERSION_EPOCH + #define CV_VERSION_EPOCH CV_VERSION_MAJOR +#endif +#if CV_VERSION_EPOCH > 3 +#include +#else +#include +#endif #include "CameraImageLoader.h" // Module specification @@ -116,26 +122,26 @@ RTC::ReturnCode_t CameraImageLoader::onExecute(RTC::UniqueId ec_id) std::string filename; std::cin >> filename; - IplImage *image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR); - if (!image) { + cv::Mat image = cv::imread(filename.c_str(), cv::IMREAD_COLOR); + if (image.empty()) { std::cerr << m_profile.instance_name << ": failed to load(" << filename << ")" << std::endl; return RTC::RTC_OK; } - m_image.data.image.width = image->width; - m_image.data.image.height = image->height; - m_image.data.image.raw_data.length(image->imageSize); - switch(image->nChannels){ + m_image.data.image.width = image.size().width; + m_image.data.image.height = image.size().height; + m_image.data.image.raw_data.length(image.size().area()); + switch(image.channels()){ case 3: m_image.data.image.format = Img::CF_RGB; { // BGR -> RGB - char *src; + unsigned char *src; unsigned char *dst = m_image.data.image.raw_data.get_buffer(); - for (int i=0; iheight; i++){ - for (int j=0; jwidth; j++){ - src = image->imageData + image->widthStep*i + j*3; + for (int i=0; iimageData, + image.data, m_image.data.image.raw_data.length()); break; default: break; } - cvReleaseImage (&image); - m_imageOut.write(); return RTC::RTC_OK; diff --git a/rtc/CameraImageSaver/CameraImageSaver.cpp b/rtc/CameraImageSaver/CameraImageSaver.cpp index c7a8ca0d3ad..e246a5348d3 100644 --- a/rtc/CameraImageSaver/CameraImageSaver.cpp +++ b/rtc/CameraImageSaver/CameraImageSaver.cpp @@ -7,8 +7,16 @@ * $Id$ */ -#include -#include +#include +#include +#ifndef CV_VERSION_EPOCH + #define CV_VERSION_EPOCH CV_VERSION_MAJOR +#endif +#if CV_VERSION_EPOCH > 3 +#include +#else +#include +#endif #include "CameraImageSaver.h" // Module specification @@ -158,7 +166,8 @@ RTC::ReturnCode_t CameraImageSaver::onExecute(RTC::UniqueId ec_id) char fname[256]; sprintf(fname, "%s%04d.png", m_basename.c_str(), m_count++); - cvSaveImage(fname, cvImage); + cv::Mat image = cv::cvarrToMat(cvImage); + cv::imwrite(fname, image); cvReleaseImage(&cvImage); } diff --git a/rtc/CameraImageViewer/CameraImageViewer.h b/rtc/CameraImageViewer/CameraImageViewer.h index 229d816410f..ff05ae16a5c 100644 --- a/rtc/CameraImageViewer/CameraImageViewer.h +++ b/rtc/CameraImageViewer/CameraImageViewer.h @@ -21,8 +21,8 @@ #include #include #include -#include -#include +#include +#include // Service implementation headers // diff --git a/rtc/ColorExtractor/ColorExtractor.cpp b/rtc/ColorExtractor/ColorExtractor.cpp index 3b66d19adba..90fa858141d 100644 --- a/rtc/ColorExtractor/ColorExtractor.cpp +++ b/rtc/ColorExtractor/ColorExtractor.cpp @@ -7,7 +7,11 @@ * $Id$ */ -#include +#include +#include +#ifndef CV_RGB +#define CV_RGB( r, g, b ) cvScalar( (b), (g), (r), 0 ) +#endif #include "ColorExtractor.h" #include "hrpsys/util/VectorConvert.h" diff --git a/rtc/ColorExtractor/ColorExtractor.h b/rtc/ColorExtractor/ColorExtractor.h index a0dfb4e1c91..a858e5d89b1 100644 --- a/rtc/ColorExtractor/ColorExtractor.h +++ b/rtc/ColorExtractor/ColorExtractor.h @@ -19,7 +19,7 @@ #include #include #include -#include +#include // Service implementation headers // diff --git a/rtc/JpegDecoder/JpegDecoder.cpp b/rtc/JpegDecoder/JpegDecoder.cpp index c67b0a4ee18..5ce4ac9e30a 100644 --- a/rtc/JpegDecoder/JpegDecoder.cpp +++ b/rtc/JpegDecoder/JpegDecoder.cpp @@ -9,6 +9,12 @@ #include #include +#ifndef CV_VERSION_EPOCH +#define CV_VERSION_EPOCH CV_VERSION_MAJOR +#endif +#if CV_VERSION_EPOCH > 3 +#include +#endif #include "JpegDecoder.h" // Module specification diff --git a/rtc/JpegEncoder/JpegEncoder.cpp b/rtc/JpegEncoder/JpegEncoder.cpp index cf9fd0a69ee..42b6b194213 100644 --- a/rtc/JpegEncoder/JpegEncoder.cpp +++ b/rtc/JpegEncoder/JpegEncoder.cpp @@ -10,6 +10,12 @@ #include #include #include +#ifndef CV_VERSION_EPOCH +#define CV_VERSION_EPOCH CV_VERSION_MAJOR +#endif +#if CV_VERSION_EPOCH > 3 +#include +#endif #include "JpegEncoder.h" // Module specification diff --git a/rtc/RGB2Gray/RGB2Gray.cpp b/rtc/RGB2Gray/RGB2Gray.cpp index d01b1df4ac7..f7f49cb6e11 100644 --- a/rtc/RGB2Gray/RGB2Gray.cpp +++ b/rtc/RGB2Gray/RGB2Gray.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include "RGB2Gray.h" // Module specification diff --git a/rtc/RangeDataViewer/RangeDataViewer.h b/rtc/RangeDataViewer/RangeDataViewer.h index fbc25998442..6e830404c87 100644 --- a/rtc/RangeDataViewer/RangeDataViewer.h +++ b/rtc/RangeDataViewer/RangeDataViewer.h @@ -19,8 +19,8 @@ #include #include #include -#include -#include +#include +#include // Service implementation headers // diff --git a/rtc/ResizeImage/ResizeImage.cpp b/rtc/ResizeImage/ResizeImage.cpp index 6eb73ea013f..a6685d5d9ea 100644 --- a/rtc/ResizeImage/ResizeImage.cpp +++ b/rtc/ResizeImage/ResizeImage.cpp @@ -7,7 +7,8 @@ * $Id$ */ -#include +#include +#include #include "ResizeImage.h" // Module specification diff --git a/rtc/ResizeImage/ResizeImage.h b/rtc/ResizeImage/ResizeImage.h index 1251382aece..b895a162a88 100644 --- a/rtc/ResizeImage/ResizeImage.h +++ b/rtc/ResizeImage/ResizeImage.h @@ -18,7 +18,7 @@ #include #include #include -#include +#include // Service implementation headers // diff --git a/rtc/RotateImage/RotateImage.cpp b/rtc/RotateImage/RotateImage.cpp index e3328a6c96d..23ebdccd05a 100644 --- a/rtc/RotateImage/RotateImage.cpp +++ b/rtc/RotateImage/RotateImage.cpp @@ -7,7 +7,8 @@ * $Id$ */ -#include +#include +#include #include "RotateImage.h" // Module specification diff --git a/rtc/RotateImage/RotateImage.h b/rtc/RotateImage/RotateImage.h index 288fae83179..ba4ec3654f0 100644 --- a/rtc/RotateImage/RotateImage.h +++ b/rtc/RotateImage/RotateImage.h @@ -18,7 +18,7 @@ #include #include #include -#include +#include // Service implementation headers // diff --git a/rtc/UndistortImage/UndistortImage.cpp b/rtc/UndistortImage/UndistortImage.cpp index abedbe6adcb..e2bf0e50405 100644 --- a/rtc/UndistortImage/UndistortImage.cpp +++ b/rtc/UndistortImage/UndistortImage.cpp @@ -9,6 +9,10 @@ #include "UndistortImage.h" +#include +#include +#include + // Module specification // static const char* cameraimageviewercomponent_spec[] = @@ -37,8 +41,6 @@ UndistortImage::UndistortImage(RTC::Manager* manager) m_imageOut("imageOut", m_image), // m_cvImage(NULL), - m_intrinsic(NULL), - m_distortion(NULL), dummy(0) { } @@ -106,18 +108,16 @@ RTC::ReturnCode_t UndistortImage::onActivated(RTC::UniqueId ec_id) { std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl; - CvFileStorage *fs - = cvOpenFileStorage (m_calibFile.c_str(), 0, CV_STORAGE_READ); - if (!fs){ + cv::FileStorage fs(m_calibFile.c_str(), cv::FileStorage::READ); + if (!fs.isOpened()){ std::cerr << m_profile.instance_name << ": can't open " << m_calibFile << std::endl; return RTC::RTC_ERROR; } - CvFileNode *param = cvGetFileNodeByName (fs, NULL, "intrinsic"); - m_intrinsic = (CvMat *) cvRead (fs, param); - param = cvGetFileNodeByName (fs, NULL, "distortion"); - m_distortion = (CvMat *) cvRead (fs, param); - cvReleaseFileStorage (&fs); + cv::FileNode param = fs["intrinsic"]; + param >> m_intrinsic; + param = fs["distortion"]; + param >> m_distortion; return RTC::RTC_OK; } @@ -129,9 +129,7 @@ RTC::ReturnCode_t UndistortImage::onDeactivated(RTC::UniqueId ec_id) cvReleaseImage(&m_cvImage); m_cvImage = NULL; } - if (m_intrinsic) cvReleaseMat (&m_intrinsic); - if (m_distortion) cvReleaseMat (&m_distortion); - + return RTC::RTC_OK; } @@ -189,14 +187,16 @@ RTC::ReturnCode_t UndistortImage::onExecute(RTC::UniqueId ec_id) } - IplImage *dst_img = cvCloneImage (m_cvImage); - cvUndistort2 (m_cvImage, dst_img, m_intrinsic, m_distortion); + cv::Mat src_img = cv::cvarrToMat(m_cvImage); + cv::Mat dst_img; + src_img.copyTo(dst_img); + cv::undistort (src_img, dst_img, m_intrinsic, m_distortion); switch(m_image.data.image.format){ case Img::CF_RGB: { // BGR -> RGB - char *src = dst_img->imageData; + unsigned char *src = dst_img.data; for (unsigned int i=0; iimageData, + dst_img.data, m_image.data.image.raw_data.length()); break; default: break; } - cvReleaseImage (&dst_img); - m_imageOut.write(); return RTC::RTC_OK; diff --git a/rtc/UndistortImage/UndistortImage.h b/rtc/UndistortImage/UndistortImage.h index 03c6e88b24e..edac8e53045 100644 --- a/rtc/UndistortImage/UndistortImage.h +++ b/rtc/UndistortImage/UndistortImage.h @@ -18,8 +18,9 @@ #include #include #include -#include -#include +#include +#include +#include // Service implementation headers // @@ -137,7 +138,7 @@ class UndistortImage private: IplImage* m_cvImage; std::string m_calibFile; - CvMat *m_intrinsic, *m_distortion; + cv::Mat m_intrinsic, m_distortion; int dummy; }; diff --git a/rtc/VideoCapture/camera.h b/rtc/VideoCapture/camera.h index 4ea2b8d7682..68aab3ea786 100644 --- a/rtc/VideoCapture/camera.h +++ b/rtc/VideoCapture/camera.h @@ -1,7 +1,6 @@ /* Most of capture.cpp and capture.h are copied from http://jsk-enshu.svn.sourceforge.net/viewvc/jsk-enshu/trunk/keisanki/2009/ */ -#include -#include +#include #include #include #include diff --git a/rtc/VirtualCamera/VirtualCamera.h b/rtc/VirtualCamera/VirtualCamera.h index a78eeb17956..01e895f1b07 100644 --- a/rtc/VirtualCamera/VirtualCamera.h +++ b/rtc/VirtualCamera/VirtualCamera.h @@ -22,8 +22,8 @@ #include #include //Open CV headder -#include -#include +#include +#include // #include "hrpsys/util/LogManager.h" #include "hrpsys/util/SDLUtil.h" diff --git a/util/simulator/CMakeLists.txt b/util/simulator/CMakeLists.txt index 1f6c0c607b7..beb53389d08 100644 --- a/util/simulator/CMakeLists.txt +++ b/util/simulator/CMakeLists.txt @@ -32,19 +32,12 @@ add_library(hrpsysext SHARED PyShape.cpp ) -if (NOT APPLE) - target_link_libraries(hrpsysext - boost_python - hrpsysUtil - ${PYTHON_LIBRARIES} - ) -else() - target_link_libraries(hrpsysext - boost_python27-mt - hrpsysUtil - ${PYTHON_LIBRARIES} - ) -endif() +find_package(Boost REQUIRED COMPONENTS python) +target_link_libraries(hrpsysext + Boost::python + hrpsysUtil + ${PYTHON_LIBRARIES} + ) set_target_properties(hrpsysext PROPERTIES PREFIX "") set_target_properties(hrpsysext PROPERTIES SUFFIX ".so") From 26dbe875c18528373d9df1888cf1efcb85eda99f Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Mon, 13 Dec 2021 00:24:22 +0900 Subject: [PATCH 03/13] [hrpEC/hroEC-common] fix memory leak --- ec/hrpEC/hrpEC-common.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ec/hrpEC/hrpEC-common.cpp b/ec/hrpEC/hrpEC-common.cpp index db16eaccc63..2657c50c37c 100644 --- a/ec/hrpEC/hrpEC-common.cpp +++ b/ec/hrpEC/hrpEC-common.cpp @@ -146,7 +146,7 @@ namespace RTC #else RTC::RTObject_var rtc = list[i]; #endif - rtc_names.push_back(std::string(rtc->get_component_profile()->instance_name)); + rtc_names.push_back(std::string(RTC::ComponentProfile_var(rtc->get_component_profile())->instance_name)); } } printRTCProcessingTime(processes); From 5fb77b0b7c5eb8d8f539afba59c11e58dfb3049c Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Mon, 14 Feb 2022 20:51:24 +0800 Subject: [PATCH 04/13] [cmake] Remove unnecessary add_subdirectory --- CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 52d103bbaf8..9c484c987d8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -226,7 +226,6 @@ if (NOT QNXNTO) add_subdirectory(util) endif() add_subdirectory(sample) -add_subdirectory(test) #if catkin environment string(REGEX MATCH "catkin" need_catkin "$ENV{_}") From bf592f337343d85d1a059c427da5b480a3c0f531 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Mon, 14 Feb 2022 20:57:40 +0800 Subject: [PATCH 05/13] [cmake] Correctly set file-level dependencies --- sample/SampleRobot/rtc/CMakeLists.txt | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/sample/SampleRobot/rtc/CMakeLists.txt b/sample/SampleRobot/rtc/CMakeLists.txt index 555417c8e0d..8f680a634b7 100644 --- a/sample/SampleRobot/rtc/CMakeLists.txt +++ b/sample/SampleRobot/rtc/CMakeLists.txt @@ -31,19 +31,16 @@ add_custom_target(SampleComponentService.idl.compiled ALL DEPENDS SampleComponen # plugins comps set(comp_sources SampleComponent.cpp SampleComponent_impl.cpp SampleComponentServiceSk.cpp SampleComponentServiceDynSk.cpp) -add_library(SampleComponent SHARED ${comp_sources}) +add_library(SampleComponent SHARED ${comp_sources} SampleComponentService.hh) target_link_libraries(SampleComponent ${libs}) set_target_properties(SampleComponent PROPERTIES PREFIX "") include_directories(${CMAKE_CURRENT_BINARY_DIR}) -add_executable(SampleComponentComp SampleComponentComp.cpp ${comp_sources}) +add_executable(SampleComponentComp SampleComponentComp.cpp ${comp_sources} SampleComponentService.hh) target_link_libraries(SampleComponentComp ${libs}) set(target SampleComponent SampleComponentComp) -add_dependencies(SampleComponent SampleComponentService.hh) -add_dependencies(SampleComponentComp SampleComponentService.hh) - configure_file(samplerobot_sample_component.py.in ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_sample_component.py) install(PROGRAMS From 89d8ac44ebc8a814c78b04e7f4fd1ac8feeeb0d7 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Mon, 14 Feb 2022 21:03:16 +0800 Subject: [PATCH 06/13] [cmake] Change USE_HRPSYSUTIL into an option --- CMakeLists.txt | 57 +++++++++++++++++++------------------------------- 1 file changed, 22 insertions(+), 35 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9c484c987d8..38af72e50ea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -135,50 +135,37 @@ include(CPack) find_package(LibXml2 REQUIRED) find_package(QuickHull REQUIRED) -find_package(SDL) -if ( NOT SDL_FOUND ) - message(WARNING "Package SDL is not found!") -endif( NOT SDL_FOUND ) -find_package(OpenGL) -if ( NOT OPENGL_FOUND ) - message(WARNING "Package OpenGL is not found!") -endif( NOT OPENGL_FOUND ) -find_package(GLUT) -if ( NOT GLUT_FOUND ) - message(WARNING "Package GLUT is not found!") -endif( NOT GLUT_FOUND ) + +option(USE_HRPSYSUTIL "Build hrpsysUtil" ON) + include(FindPkgConfig) -pkg_check_modules(OpenCV opencv) -if (NOT OpenCV_FOUND) - pkg_check_modules(OpenCV opencv-2.3.1) + +if(USE_HRPSYSUTIL) + find_package(SDL REQUIRED) + find_package(OpenGL REQUIRED) + find_package(GLUT REQUIRED) + pkg_check_modules(OpenCV opencv) if (NOT OpenCV_FOUND) - pkg_check_modules(OpenCV opencv4) + pkg_check_modules(OpenCV opencv-2.3.1) if (NOT OpenCV_FOUND) - message(WARNING "opencv not found") + pkg_check_modules(OpenCV opencv4) + if (NOT OpenCV_FOUND) + message(FATAL_ERROR "OpenCV is required to build hrpsysUtil") + endif() endif() endif() -endif() -pkg_check_modules(GLEW glew) -if (NOT GLEW_FOUND) - message(WARNING "Package GLEW is not found") + include_directories(${OpenCV_INCLUDE_DIRS}) + link_directories(${OpenCV_LIBRARY_DIRS}) + pkg_check_modules(GLEW glew) + if (NOT GLEW_FOUND) + message(FATAL_ERROR "GLEW is required to build hrpsysUtil") + endif() endif() find_package(PCL) -if(OpenCV_FOUND AND OPENGL_FOUND AND GLEW_FOUND AND GLUT_FOUND AND SDL_FOUND) - if(APPLE) - if(PCL_FOUND) # qhull is included in pcl - set(USE_HRPSYSUTIL true) - else() - set(USE_HRPSYSUTIL false) - endif() - else() - set(USE_HRPSYSUTIL true) - endif() -else() - set(USE_HRPSYSUTIL false) +if(USE_HRPSYSUTIL AND APPLE AND NOT PCL_FOUND) + message(FATAL_ERROR "PCL is required to build hrpsysUtil on Apple platform") endif() -include_directories(${OpenCV_INCLUDE_DIRS}) -link_directories(${OpenCV_LIBRARY_DIRS}) if(NOT QNXNTO) find_package(Eigen REQUIRED) include_directories(${Eigen_INCLUDE_DIRS}) From e78f5338d01340eafa6cd87dd484e2c920d04709 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Mon, 14 Feb 2022 21:24:33 +0800 Subject: [PATCH 07/13] [cmake] Remove the FindEigen module - The variable the module set is not used by hrpsys-base - This interferes with PCL's module on Ubuntu 20.04 --- CMakeLists.txt | 7 ------- cmake_modules/FindEigen.cmake | 26 -------------------------- 2 files changed, 33 deletions(-) delete mode 100644 cmake_modules/FindEigen.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 38af72e50ea..c8c46d79b6f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -138,8 +138,6 @@ find_package(QuickHull REQUIRED) option(USE_HRPSYSUTIL "Build hrpsysUtil" ON) -include(FindPkgConfig) - if(USE_HRPSYSUTIL) find_package(SDL REQUIRED) find_package(OpenGL REQUIRED) @@ -166,11 +164,6 @@ if(USE_HRPSYSUTIL AND APPLE AND NOT PCL_FOUND) message(FATAL_ERROR "PCL is required to build hrpsysUtil on Apple platform") endif() -if(NOT QNXNTO) - find_package(Eigen REQUIRED) - include_directories(${Eigen_INCLUDE_DIRS}) -endif() - execute_process( COMMAND python -c "from distutils import sysconfig; print sysconfig.get_config_var(\"VERSION\")" OUTPUT_VARIABLE PYTHON_VERSION diff --git a/cmake_modules/FindEigen.cmake b/cmake_modules/FindEigen.cmake deleted file mode 100644 index 6f6503d08ee..00000000000 --- a/cmake_modules/FindEigen.cmake +++ /dev/null @@ -1,26 +0,0 @@ -IF (UNIX) - FIND_PATH( - EIGEN_INCLUDE_DIR - Eigen/Core - PATHS /opt/local/include/eigen3 /usr/include/eigen3 /usr/local/include/eigen3 /usr/include/eigen /usr/include/eigen2 - DOC "the top directory of Eigen/Core in Eigen") -ELSE() - IF (WIN32) - INCLUDE(${CMAKE_MODULE_PATH}/GetWinSystemPrefixPaths.cmake) - foreach(localPath ${WIN_SYSTEM_PREFIX_PATHS}) - list(APPEND EnumPaths ${localPath}/Eigen/include/eigen3) - endforeach() - FIND_PATH( - EIGEN_INCLUDE_DIR - Eigen/Core - PATHS ${EnumPaths} - DOC "the top directory of Eigen/Core in Eigen") - ENDIF(WIN32) -ENDIF(UNIX) - -IF ( EIGEN_INCLUDE_DIR ) - MESSAGE(STATUS "Looking for Eigen - found") - SET(KDL_CFLAGS "${KDL_CFLAGS} -I${EIGEN_INCLUDE_DIR}" CACHE INTERNAL "") -ELSE ( EIGEN_INCLUDE_DIR ) - MESSAGE(FATAL_ERROR "Looking for Eigen - not found") -ENDIF ( EIGEN_INCLUDE_DIR ) From 611baf6508dacff3f260652513a07144b6cff2d7 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Wed, 23 Feb 2022 07:58:16 +0800 Subject: [PATCH 08/13] Introduce an option to skip hrpsysext build hrpsysext relies on Boost.Python and it can be difficult to: 1. Get it to work with the desired Python version 2. Let CMake find it Since those bindings are not always required, introducing this option allows to simplify the installation process --- util/CMakeLists.txt | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/util/CMakeLists.txt b/util/CMakeLists.txt index 8a533d0c282..bf839368143 100644 --- a/util/CMakeLists.txt +++ b/util/CMakeLists.txt @@ -1,7 +1,10 @@ add_subdirectory(ProjectGenerator) add_subdirectory(SelfCollisionChecker) if(USE_HRPSYSUTIL) - add_subdirectory(simulator) + option(USE_HRPSYSEXT "Build hrpsys Python bindings" ON) + if(USE_HRPSYSEXT) + add_subdirectory(simulator) + endif() add_subdirectory(viewer) add_subdirectory(monitor) endif() From 1a8121a5d98ef07979edfb71ac09f124b50144af Mon Sep 17 00:00:00 2001 From: Arnaud Tanguy Date: Thu, 7 Apr 2022 17:20:24 +0200 Subject: [PATCH 09/13] Add an option to disable Irrlicht --- CMakeLists.txt | 12 ++++++------ rtc/CMakeLists.txt | 8 +++++--- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c8c46d79b6f..fb40757a659 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,12 @@ include(FindPkgConfig) include(CheckIncludeFiles) option(ENABLE_INSTALL_RPATH "Enable RPATH setting for installed binary files" OFF) +option(USE_HRPSYSUTIL "Build hrpsysUtil" ON) +option(USE_IRRLICHT "Build Irrlicht components" OFF) +option(NO_DIGITAL_INPUT "Disable readDigitalInput and lengthDigitalInput" OFF) +option(USE_QPOASES "Build qpOASES" OFF) +option(ENABLE_DOXYGEN "Use Doxygen" ON) + set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) @@ -136,8 +142,6 @@ include(CPack) find_package(LibXml2 REQUIRED) find_package(QuickHull REQUIRED) -option(USE_HRPSYSUTIL "Build hrpsysUtil" ON) - if(USE_HRPSYSUTIL) find_package(SDL REQUIRED) find_package(OpenGL REQUIRED) @@ -177,7 +181,6 @@ install(FILES add_definitions(-DHRPSYS_PACKAGE_VERSION=\"\\"${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}\\"\") -option(NO_DIGITAL_INPUT "Disable readDigitalInput and lengthDigitalInput" OFF) if(NO_DIGITAL_INPUT) add_definitions(-DNO_DIGITAL_INPUT) message(STATUS "Disablng readDigitalInput and lengthDigitalInput") @@ -194,11 +197,8 @@ add_subdirectory(python) add_subdirectory(idl) add_subdirectory(lib) add_subdirectory(ec) -# option for 3rdparty -option(USE_QPOASES "Build qpOASES" OFF) add_subdirectory(3rdparty) add_subdirectory(rtc) -option(ENABLE_DOXYGEN "Use Doxygen" ON) if(ENABLE_DOXYGEN) add_subdirectory(doc) endif() diff --git a/rtc/CMakeLists.txt b/rtc/CMakeLists.txt index a292b3849f6..1ab754e5e0b 100644 --- a/rtc/CMakeLists.txt +++ b/rtc/CMakeLists.txt @@ -63,9 +63,11 @@ if (NOT APPLE AND USE_HRPSYSUTIL) add_subdirectory(VideoCapture) endif() -find_package(Irrlicht) -if (IRRLICHT_FOUND AND USE_HRPSYSUTIL) - add_subdirectory(OGMap3DViewer) +if (USE_IRRLICHT AND USE_HRPSYSUTIL) + find_package(Irrlicht) + if(IRRLICHT_FOUND) + add_subdirectory(OGMap3DViewer) + endif() endif() # Octomap From d48a0eb0cc0a1f09247c5535207fd8f737a29350 Mon Sep 17 00:00:00 2001 From: "Rohan P. Singh" Date: Tue, 17 May 2022 17:45:10 +0900 Subject: [PATCH 10/13] [ModifiedServo] Create InPorts for pdgains Ports 'pgains' and 'dgains' will allow modifying the servo PD gains (of all joints) at each timestep. --- rtc/ModifiedServo/ModifiedServo.cpp | 20 ++++++++++++++++++++ rtc/ModifiedServo/ModifiedServo.h | 4 ++++ 2 files changed, 24 insertions(+) diff --git a/rtc/ModifiedServo/ModifiedServo.cpp b/rtc/ModifiedServo/ModifiedServo.cpp index e317615375f..095c58b3ec6 100644 --- a/rtc/ModifiedServo/ModifiedServo.cpp +++ b/rtc/ModifiedServo/ModifiedServo.cpp @@ -37,6 +37,8 @@ ModifiedServo::ModifiedServo(RTC::Manager* manager) m_qIn("q", m_q), m_torqueModeIn("torqueMode", m_torqueMode), m_tauOut("tau", m_tau), + m_pgainsIn("pgains", m_pgains), + m_dgainsIn("dgains", m_dgains), // gain_fname(""), dt(0.005), @@ -58,6 +60,8 @@ RTC::ReturnCode_t ModifiedServo::onInitialize() addInPort("qRef", m_qRefIn); addInPort("q", m_qIn); addInPort("torqueMode", m_torqueModeIn); + addInPort("pgains", m_pgainsIn); + addInPort("dgains", m_dgainsIn); // Set OutPort buffer addOutPort("tau", m_tauOut); @@ -174,11 +178,19 @@ RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) m_qRefIn.read(); step = nstep; } + if(m_pgainsIn.isNew()){ + m_pgainsIn.read(); + } + if(m_dgainsIn.isNew()){ + m_dgainsIn.read(); + } if (m_torqueModeIn.isNew()) m_torqueModeIn.read(); for (size_t i = 0; i < dof; i++) { + Pgain[i] = m_pgains.data[i]; + Dgain[i] = m_dgains.data[i]; double q = m_q.data[i]; double qRef = step > 0 ? qRef_old[i] + (m_qRef.data[i] - qRef_old[i]) / step : qRef_old[i]; @@ -250,6 +262,8 @@ void ModifiedServo::readGainFile() Pgain.resize(dof); Dgain.resize(dof); + m_pgains.data.length(dof); + m_dgains.data.length(dof); for (unsigned int i = 0; i < dof; i++) { @@ -266,6 +280,12 @@ void ModifiedServo::readGainFile() gain.close(); + // initialize pgains, dgains with with values read from file + for(unsigned int i=0; i < dof; ++i){ + m_pgains.data[i] = Pgain[i]; + m_dgains.data[i] = Dgain[i]; + } + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] successfully read" << std::endl; } else diff --git a/rtc/ModifiedServo/ModifiedServo.h b/rtc/ModifiedServo/ModifiedServo.h index a58ed0863a0..4ae02d75344 100644 --- a/rtc/ModifiedServo/ModifiedServo.h +++ b/rtc/ModifiedServo/ModifiedServo.h @@ -106,6 +106,10 @@ class ModifiedServo : public RTC::DataFlowComponentBase InPort m_qIn; TimedBooleanSeq m_torqueMode; InPort m_torqueModeIn; + TimedDoubleSeq m_pgains; + InPort m_pgainsIn; + TimedDoubleSeq m_dgains; + InPort m_dgainsIn; // From 3322be67190657787b3b855811610816fbc28fa6 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Tue, 17 May 2022 18:52:03 +0900 Subject: [PATCH 11/13] [RobotHardware/robot.cpp] fix for robots which have a relay for each joint --- rtc/RobotHardware/robot.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index 2027dd405db..79c48bd51f5 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -440,12 +440,19 @@ bool robot::power(int jid, bool turnon) int com = OFF; if (turnon) { +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 + int s; + read_servo_state(jid, &s); + if (s == ON) + return false; +#else for(unsigned int i=0; i Date: Wed, 18 May 2022 14:02:49 +0900 Subject: [PATCH 12/13] [ModifiedServo] Intialize m_pgains, m_dgains in onActivated --- rtc/ModifiedServo/ModifiedServo.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/rtc/ModifiedServo/ModifiedServo.cpp b/rtc/ModifiedServo/ModifiedServo.cpp index 095c58b3ec6..c35a991974b 100644 --- a/rtc/ModifiedServo/ModifiedServo.cpp +++ b/rtc/ModifiedServo/ModifiedServo.cpp @@ -150,10 +150,15 @@ RTC::ReturnCode_t ModifiedServo::onActivated(RTC::UniqueId ec_id) m_tau.data.length(dof); + m_pgains.data.length(dof); + m_dgains.data.length(dof); + for (size_t i = 0; i < dof; i++) { m_tauRef.data[i] = 0.0; m_qRef.data[i] = qRef_old[i] = q_old[i] = m_q.data[i]; m_torqueMode.data[i] = false; + m_pgains.data[i] = Pgain[i]; + m_dgains.data[i] = Dgain[i]; } return RTC::RTC_OK; @@ -262,8 +267,6 @@ void ModifiedServo::readGainFile() Pgain.resize(dof); Dgain.resize(dof); - m_pgains.data.length(dof); - m_dgains.data.length(dof); for (unsigned int i = 0; i < dof; i++) { @@ -280,12 +283,6 @@ void ModifiedServo::readGainFile() gain.close(); - // initialize pgains, dgains with with values read from file - for(unsigned int i=0; i < dof; ++i){ - m_pgains.data[i] = Pgain[i]; - m_dgains.data[i] = Dgain[i]; - } - std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] successfully read" << std::endl; } else From cbad549f848038d112f1f398e51dc9c1718a5121 Mon Sep 17 00:00:00 2001 From: "Rohan P. Singh" Date: Wed, 18 May 2022 14:42:44 +0900 Subject: [PATCH 13/13] [ModifiedServo] Create OutPorts to Get + rename register name --- rtc/ModifiedServo/ModifiedServo.cpp | 14 ++++++++++---- rtc/ModifiedServo/ModifiedServo.h | 4 ++++ 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/rtc/ModifiedServo/ModifiedServo.cpp b/rtc/ModifiedServo/ModifiedServo.cpp index c35a991974b..544dd1653e6 100644 --- a/rtc/ModifiedServo/ModifiedServo.cpp +++ b/rtc/ModifiedServo/ModifiedServo.cpp @@ -37,8 +37,10 @@ ModifiedServo::ModifiedServo(RTC::Manager* manager) m_qIn("q", m_q), m_torqueModeIn("torqueMode", m_torqueMode), m_tauOut("tau", m_tau), - m_pgainsIn("pgains", m_pgains), - m_dgainsIn("dgains", m_dgains), + m_pgainsIn("pgainsSet", m_pgains), + m_dgainsIn("dgainsSet", m_dgains), + m_pgainsOut("pgainsGet", m_pgains), + m_dgainsOut("dgainsGet", m_dgains), // gain_fname(""), dt(0.005), @@ -60,11 +62,13 @@ RTC::ReturnCode_t ModifiedServo::onInitialize() addInPort("qRef", m_qRefIn); addInPort("q", m_qIn); addInPort("torqueMode", m_torqueModeIn); - addInPort("pgains", m_pgainsIn); - addInPort("dgains", m_dgainsIn); + addInPort("pgainsSet", m_pgainsIn); + addInPort("dgainsSet", m_dgainsIn); // Set OutPort buffer addOutPort("tau", m_tauOut); + addOutPort("pgainsGet", m_pgainsOut); + addOutPort("dgainsGet", m_dgainsOut); // Set service provider to Ports @@ -217,6 +221,8 @@ RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) m_tau.tm = m_q.tm; m_tauOut.write(); + m_pgainsOut.write(); + m_dgainsOut.write(); return RTC::RTC_OK; } diff --git a/rtc/ModifiedServo/ModifiedServo.h b/rtc/ModifiedServo/ModifiedServo.h index 4ae02d75344..287af997312 100644 --- a/rtc/ModifiedServo/ModifiedServo.h +++ b/rtc/ModifiedServo/ModifiedServo.h @@ -117,6 +117,10 @@ class ModifiedServo : public RTC::DataFlowComponentBase // TimedDoubleSeq m_tau; OutPort m_tauOut; + // Will bind to to m_pgains + OutPort m_pgainsOut; + // Will bind to m_dgains + OutPort m_dgainsOut; //