├── third_party ├── mvcam │ └── __init__.py ├── mvsdk │ ├── .gitignore │ ├── mvsdk │ │ ├── __init__.py │ │ ├── lib │ │ │ ├── arm │ │ │ │ └── libMVSDK.so │ │ │ ├── x64 │ │ │ │ └── libMVSDK.so │ │ │ ├── x86 │ │ │ │ └── libMVSDK.so │ │ │ ├── arm64 │ │ │ │ └── libMVSDK.so │ │ │ └── arm_softfp │ │ │ │ └── libMVSDK.so │ │ └── utils.py │ ├── 88-mvusb.rules │ ├── uninstall-msvdk.sh │ ├── setup.py │ ├── README.md │ └── install-msvdk.sh └── flexiv_rdk-main │ ├── thirdparty │ └── eigen3 │ │ ├── Eigen │ │ ├── Eigen │ │ ├── src │ │ │ ├── Core │ │ │ │ ├── util │ │ │ │ │ ├── NonMPL2.h │ │ │ │ │ └── ReenableStupidWarnings.h │ │ │ │ ├── functors │ │ │ │ │ └── TernaryFunctors.h │ │ │ │ ├── DiagonalProduct.h │ │ │ │ ├── arch │ │ │ │ │ ├── AVX │ │ │ │ │ │ └── TypeCasting.h │ │ │ │ │ ├── Default │ │ │ │ │ │ ├── Settings.h │ │ │ │ │ │ └── ConjHelper.h │ │ │ │ │ └── SSE │ │ │ │ │ │ └── TypeCasting.h │ │ │ │ └── SelfCwiseBinaryOp.h │ │ │ ├── misc │ │ │ │ ├── lapacke_mangling.h │ │ │ │ └── RealSvd2x2.h │ │ │ ├── SparseCore │ │ │ │ ├── SparseFuzzy.h │ │ │ │ ├── SparseRedux.h │ │ │ │ └── MappedSparseMatrix.h │ │ │ └── SparseLU │ │ │ │ └── SparseLU_Utils.h │ │ ├── Dense │ │ ├── StdList │ │ ├── StdDeque │ │ ├── StdVector │ │ ├── Householder │ │ ├── Sparse │ │ ├── Jacobi │ │ ├── QtAlignedMalloc │ │ ├── MetisSupport │ │ ├── PardisoSupport │ │ ├── SPQRSupport │ │ ├── SparseQR │ │ ├── Cholesky │ │ ├── QR │ │ ├── UmfPackSupport │ │ ├── SparseCholesky │ │ ├── LU │ │ ├── SVD │ │ ├── PaStiXSupport │ │ ├── SparseLU │ │ ├── Eigenvalues │ │ ├── CholmodSupport │ │ ├── IterativeLinearSolvers │ │ ├── Geometry │ │ ├── SuperLUSupport │ │ └── SparseCore │ │ ├── README.md │ │ ├── signature_of_eigen3_matrix_library │ │ └── unsupported │ │ └── Eigen │ │ ├── MoreVectorization │ │ ├── CXX11 │ │ ├── src │ │ │ ├── ThreadPool │ │ │ │ ├── ThreadYield.h │ │ │ │ ├── ThreadLocal.h │ │ │ │ ├── ThreadPoolInterface.h │ │ │ │ └── ThreadEnvironment.h │ │ │ └── Tensor │ │ │ │ ├── TensorGlobalFunctions.h │ │ │ │ ├── TensorMacros.h │ │ │ │ └── TensorContractionBlocking.h │ │ ├── TensorSymmetry │ │ └── ThreadPool │ │ ├── Splines │ │ ├── ArpackSupport │ │ ├── Skyline │ │ ├── KroneckerProduct │ │ ├── AutoDiff │ │ ├── src │ │ ├── NonLinearOptimization │ │ │ ├── r1mpyq.h │ │ │ ├── rwupdt.h │ │ │ ├── chkder.h │ │ │ └── covar.h │ │ └── SpecialFunctions │ │ │ └── SpecialFunctionsHalf.h │ │ ├── EulerAngles │ │ ├── LevenbergMarquardt │ │ ├── IterativeSolvers │ │ ├── SparseExtra │ │ ├── SpecialFunctions │ │ └── NumericalDiff │ ├── .gitignore │ ├── doc │ └── flexiv_logo.png │ ├── resources │ └── meshes │ │ ├── rizon10 │ │ └── collision │ │ │ ├── link0.stl │ │ │ ├── link1.stl │ │ │ ├── link2.stl │ │ │ ├── link3.stl │ │ │ ├── link4.stl │ │ │ ├── link5.stl │ │ │ ├── link6.stl │ │ │ └── link7.stl │ │ ├── rizon4 │ │ └── collision │ │ │ ├── link0.stl │ │ │ ├── link1.stl │ │ │ ├── link2.stl │ │ │ ├── link3.stl │ │ │ ├── link4.stl │ │ │ ├── link5.stl │ │ │ ├── link6.stl │ │ │ └── link7.stl │ │ └── rizon4s │ │ └── collision │ │ ├── link0.stl │ │ ├── link1.stl │ │ ├── link2.stl │ │ ├── link3.stl │ │ ├── link4.stl │ │ ├── link5.stl │ │ ├── link6.stl │ │ └── link7.stl │ ├── cmake │ └── flexiv_rdk-config.cmake.in │ ├── .github │ └── ISSUE_TEMPLATE │ │ ├── feature_request.md │ │ └── bug_report.md │ ├── test │ ├── CMakeLists.txt │ └── test_log.cpp │ ├── example │ └── CMakeLists.txt │ ├── include │ └── flexiv │ │ ├── Log.hpp │ │ └── Mode.hpp │ ├── .clang-format │ └── example_py │ ├── utility.py │ └── basics2_clear_fault.py ├── reactive_diffusion_policy ├── __init__.py ├── model │ ├── __init__.py │ ├── common │ │ ├── module_attr_mixin.py │ │ ├── shape_util.py │ │ ├── dict_of_tensor_mixin.py │ │ └── lr_scheduler.py │ ├── diffusion │ │ ├── positional_embedding.py │ │ └── conv1d_components.py │ ├── vision │ │ ├── choice_randomizer.py │ │ └── model_getter.py │ └── vae │ │ └── utils.py ├── real_world │ ├── publisher │ │ ├── __init__.py │ │ ├── marker_track │ │ │ ├── src │ │ │ │ └── __init__.py │ │ │ ├── makefile │ │ │ └── README.md │ │ ├── lib │ │ │ └── find_marker.so │ │ └── setting.py │ ├── robot │ │ └── __init__.py │ ├── device_mapping │ │ ├── __init__.py │ │ └── device_mapping_utils.py │ ├── simple_camera │ │ ├── __init__.py │ │ └── simple_usb_camera.py │ ├── teleoperation │ │ └── __init__.py │ └── real_inference_util.py ├── config │ ├── real_world_env.yaml │ ├── at │ │ ├── at_peel.yaml │ │ └── at_wipe_lift.yaml │ ├── task │ │ ├── real_franka_env.yaml │ │ ├── real_robot_env.yaml │ │ ├── real_peel_image_wrench_at_24fps.yaml │ │ ├── real_wipe_image_wrench_at_24fps.yaml │ │ ├── real_peel_image_mctac_emb_at_24fps.yaml │ │ ├── real_peel_image_gelsight_emb_at_24fps.yaml │ │ ├── real_wipe_image_gelsight_emb_at_24fps.yaml │ │ ├── real_peel_image_mctac_emb_at_ablation_absolute_24fps.yaml │ │ ├── real_lift_image_wrench_at_absolute_24fps.yaml │ │ ├── real_lift_image_tactile_emb_at_absolute_24fps.yaml │ │ └── real_lift_three_realsense_one_mctac_one_gelsight_24fps.yaml │ └── robot │ │ └── charmander.yaml ├── common │ ├── time_utils.py │ ├── precise_sleep.py │ ├── visualization_utils.py │ ├── pcd_utils.py │ └── checkpoint_util.py ├── policy │ └── base_image_policy.py ├── scripts │ └── reencode_videos.py └── dataset │ ├── base_dataset.py │ └── real_image_tactile_latent_diffusion_dataset.py ├── assets ├── teaser.png ├── marker_motion.png └── tactile_dataset.jpg ├── data ├── PCA_Transform_GelSight │ ├── pca_mean_matrix.npy │ └── pca_transform_matrix.npy ├── PCA_Transform_McTAC_v1 │ ├── pca_mean_matrix.npy │ └── pca_transform_matrix.npy └── calibration │ └── v6 │ ├── external_camera_to_left_robot_base_transform.json │ ├── external_camera_to_right_robot_base_transform.json │ ├── left_wrist_camera_to_left_robot_tcp_transform.json │ ├── world_to_left_robot_base_transform.json │ └── world_to_right_robot_base_transform.json ├── train_dp.sh ├── tests ├── test_ring_buffer_simple.py ├── test_cv2_util.py ├── test_precise_sleep.py ├── test_replay_buffer.py ├── test_pca_explainality.py └── test_camera_res_and_fps.py ├── eval.sh ├── requirements.txt ├── docs ├── data_collection_tips.md ├── Q&A.md └── tactile_embedding_guide.md ├── train.py └── train_rdp.sh /third_party/mvcam/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/model/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /third_party/mvsdk/.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | 3 | .idea -------------------------------------------------------------------------------- /reactive_diffusion_policy/real_world/publisher/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/real_world/robot/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/real_world/device_mapping/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/real_world/simple_camera/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/real_world/teleoperation/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/real_world/publisher/marker_track/src/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/Eigen: -------------------------------------------------------------------------------- 1 | #include "Dense" 2 | #include "Sparse" 3 | -------------------------------------------------------------------------------- /assets/teaser.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/assets/teaser.png -------------------------------------------------------------------------------- /third_party/mvsdk/mvsdk/__init__.py: -------------------------------------------------------------------------------- 1 | from .mvsdk import * 2 | from .utils import install_driver, uninstall_driver 3 | -------------------------------------------------------------------------------- /assets/marker_motion.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/assets/marker_motion.png -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | .vs 3 | .DS_Store 4 | config.h 5 | build*/ 6 | html/ 7 | __pycache__ 8 | -------------------------------------------------------------------------------- /assets/tactile_dataset.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/assets/tactile_dataset.jpg -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/real_world_env.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - task: real_peel_two_realsense_one_gelsight_one_mctac_24fps 3 | -------------------------------------------------------------------------------- /third_party/mvsdk/mvsdk/lib/arm/libMVSDK.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/mvsdk/mvsdk/lib/arm/libMVSDK.so -------------------------------------------------------------------------------- /third_party/mvsdk/mvsdk/lib/x64/libMVSDK.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/mvsdk/mvsdk/lib/x64/libMVSDK.so -------------------------------------------------------------------------------- /third_party/mvsdk/mvsdk/lib/x86/libMVSDK.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/mvsdk/mvsdk/lib/x86/libMVSDK.so -------------------------------------------------------------------------------- /third_party/mvsdk/mvsdk/lib/arm64/libMVSDK.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/mvsdk/mvsdk/lib/arm64/libMVSDK.so -------------------------------------------------------------------------------- /data/PCA_Transform_GelSight/pca_mean_matrix.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/data/PCA_Transform_GelSight/pca_mean_matrix.npy -------------------------------------------------------------------------------- /data/PCA_Transform_McTAC_v1/pca_mean_matrix.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/data/PCA_Transform_McTAC_v1/pca_mean_matrix.npy -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/doc/flexiv_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/doc/flexiv_logo.png -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/Core/util/NonMPL2.h: -------------------------------------------------------------------------------- 1 | #ifdef EIGEN_MPL2_ONLY 2 | #error Including non-MPL2 code in EIGEN_MPL2_ONLY mode 3 | #endif 4 | -------------------------------------------------------------------------------- /third_party/mvsdk/mvsdk/lib/arm_softfp/libMVSDK.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/mvsdk/mvsdk/lib/arm_softfp/libMVSDK.so -------------------------------------------------------------------------------- /data/PCA_Transform_GelSight/pca_transform_matrix.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/data/PCA_Transform_GelSight/pca_transform_matrix.npy -------------------------------------------------------------------------------- /data/PCA_Transform_McTAC_v1/pca_transform_matrix.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/data/PCA_Transform_McTAC_v1/pca_transform_matrix.npy -------------------------------------------------------------------------------- /reactive_diffusion_policy/real_world/publisher/lib/find_marker.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/reactive_diffusion_policy/real_world/publisher/lib/find_marker.so -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/Dense: -------------------------------------------------------------------------------- 1 | #include "Core" 2 | #include "LU" 3 | #include "Cholesky" 4 | #include "QR" 5 | #include "SVD" 6 | #include "Geometry" 7 | #include "Eigenvalues" 8 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link0.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link1.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link2.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link3.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link4.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link5.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link6.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon10/collision/link7.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link0.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link1.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link2.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link3.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link4.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link5.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link6.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4/collision/link7.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link0.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link1.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link2.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link3.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link4.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link5.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link6.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoxiaoxh/reactive_diffusion_policy/HEAD/third_party/flexiv_rdk-main/resources/meshes/rizon4s/collision/link7.stl -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/README.md: -------------------------------------------------------------------------------- 1 | **Eigen is a C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms.** 2 | 3 | For more information go to http://eigen.tuxfamily.org/. 4 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/cmake/flexiv_rdk-config.cmake.in: -------------------------------------------------------------------------------- 1 | include(CMakeFindDependencyMacro) 2 | 3 | # Find dependency 4 | set(THREADS_PREFER_PTHREAD_FLAG ON) 5 | find_dependency(Threads REQUIRED) 6 | 7 | # Add targets file 8 | include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake") 9 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/signature_of_eigen3_matrix_library: -------------------------------------------------------------------------------- 1 | This file is just there as a signature to help identify directories containing Eigen3. When writing a script looking for Eigen3, just look for this file. This is especially useful to help disambiguate with Eigen2... 2 | -------------------------------------------------------------------------------- /data/calibration/v6/external_camera_to_left_robot_base_transform.json: -------------------------------------------------------------------------------- 1 | [[-0.0462889067298, 0.5364400861539, -0.8426680076287, 1.1250191164408], [0.9985458083608, 0.0015125604616, -0.0538885958803, -0.2362831637806], [-0.0276334167061, -0.8439370510459, -0.5357300142358, 0.3949630824746], [0.0, 0.0, 0.0, 1.0]] -------------------------------------------------------------------------------- /data/calibration/v6/external_camera_to_right_robot_base_transform.json: -------------------------------------------------------------------------------- 1 | [[-0.0437188780646, 0.5343955917255, -0.8441030809357, 1.1205368559598], [0.9989636249251, 0.0126748376596, -0.0437152669755, 0.3550075155752], [-0.012662376444, -0.8451394559685, -0.5343958871362, 0.3945764849582], [0.0, 0.0, 0.0, 1.0]] -------------------------------------------------------------------------------- /data/calibration/v6/left_wrist_camera_to_left_robot_tcp_transform.json: -------------------------------------------------------------------------------- 1 | [[0.0177712322928, 0.8689294572827, 0.4946168027567, -0.1337746238729], [-0.9998420304905, 0.0152899977066, 0.0090625622622, 0.0297229419765], [0.0003120375284, -0.4946997212821, 0.8690639150235, -0.1789731841573], [0.0, 0.0, 0.0, 1.0]] -------------------------------------------------------------------------------- /data/calibration/v6/world_to_left_robot_base_transform.json: -------------------------------------------------------------------------------- 1 | [[0.9998725765127163, -0.0155034388645585, -0.0038306069688288555, 0.0070683045402865075], [0.015497448232742834, 0.9998787047162749, 0.005535240769101149, -0.3014053772350251], [0.0037443902860645477, 0.0015691629479678323, 0.9999775807491013, -0.03662462135872824], [0.0, 0.0, 0.0, 1.0]] -------------------------------------------------------------------------------- /data/calibration/v6/world_to_right_robot_base_transform.json: -------------------------------------------------------------------------------- 1 | [[0.9999906632170189, -0.0037600693552194524, 0.002129128845334851, -0.002331655782699671], [0.0037675095129413158, 0.9999870088066106, 0.003705015842041202, 0.30212868181439084], [-0.0021430990583483954, 0.0034660786780153128, 0.9999911172941717, -0.03321199511431261], [0.0, 0.0, 0.0, 1.0]] -------------------------------------------------------------------------------- /train_dp.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | CUDA_VISIBLE_DEVICES=0 accelerate launch train.py \ 4 | --config-name=train_diffusion_unet_real_image_workspace \ 5 | task=real_peel_image_gelsight_emb_dp_absolute_12fps \ 6 | task.dataset_path=/home/wendi/Desktop/record_data/peel_v3_downsample2_zarr \ 7 | task.name=real_peel_image_gelsight_emb_dp_absolute_12fps \ 8 | logging.mode=online -------------------------------------------------------------------------------- /third_party/mvsdk/88-mvusb.rules: -------------------------------------------------------------------------------- 1 | KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="add", ATTR{idVendor}=="f622", MODE="666", TAG="mvusb_dev", A" 2 | KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="add", ATTR{idVendor}=="080b", MODE="666", TAG="mvusb_dev", A" 3 | KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="remove", TAG=="mvusb_dev", R" 4 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/model/common/module_attr_mixin.py: -------------------------------------------------------------------------------- 1 | import torch.nn as nn 2 | 3 | class ModuleAttrMixin(nn.Module): 4 | def __init__(self): 5 | super().__init__() 6 | self._dummy_variable = nn.Parameter() 7 | 8 | @property 9 | def device(self): 10 | return next(iter(self.parameters())).device 11 | 12 | @property 13 | def dtype(self): 14 | return next(iter(self.parameters())).dtype 15 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/at/at_peel.yaml: -------------------------------------------------------------------------------- 1 | horizon: 32 2 | n_obs_steps: 2 3 | dataset_obs_temporal_downsample_ratio: 2 4 | 5 | policy: 6 | horizon: ${horizon} 7 | n_latent_dims: 4 # for peeling 8 | mlp_layer_num: 1 9 | use_conv_encoder: True 10 | conv_latent_dims: 32 11 | conv_layer_num: 1 12 | use_rnn_decoder: True 13 | rnn_latent_dims: 32 14 | rnn_layer_num: 1 15 | kl_multiplier: 1e-6 # keep it small for non-strict regularization 16 | n_embed: 16 17 | # ============== 18 | use_vq: False 19 | vqvae_groups: 4 20 | eval: false 21 | act_scale: 1 -------------------------------------------------------------------------------- /tests/test_ring_buffer_simple.py: -------------------------------------------------------------------------------- 1 | from reactive_diffusion_policy.common.ring_buffer import RingBuffer 2 | 3 | def test_ring_buffer(): 4 | # Example usage 5 | buffer = RingBuffer(size=5) 6 | 7 | # Push some items 8 | for i in range(10): 9 | buffer.push(f"item_{i}") 10 | 11 | # Get last 3 items 12 | items, err = buffer.peek_last_n(3) 13 | if err is None: 14 | print("Last 3 items:", items) # Will print newest to oldest 15 | # Expected: ['item_9', 'item_8', 'item_7'] 16 | 17 | if __name__ == "__main__": 18 | test_ring_buffer() 19 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/at/at_wipe_lift.yaml: -------------------------------------------------------------------------------- 1 | horizon: 32 2 | n_obs_steps: 2 3 | dataset_obs_temporal_downsample_ratio: 2 4 | 5 | policy: 6 | horizon: ${horizon} 7 | n_latent_dims: 8 # for wiping, lifting 8 | mlp_layer_num: 1 9 | use_conv_encoder: True 10 | conv_latent_dims: 32 11 | conv_layer_num: 1 12 | use_rnn_decoder: True 13 | rnn_latent_dims: 32 14 | rnn_layer_num: 1 15 | kl_multiplier: 1e-6 # keep it small for non-strict regularization 16 | n_embed: 16 17 | # ============== 18 | use_vq: False 19 | vqvae_groups: 4 20 | eval: false 21 | act_scale: 1 -------------------------------------------------------------------------------- /third_party/mvsdk/uninstall-msvdk.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | CURDIR=`pwd` 4 | echo "Your current directory is $CURDIR. This is where the MVSDK software will be installed..." 5 | CURR_USER=`whoami` 6 | CURR_ARCH=`arch` 7 | 8 | if [ $CURR_USER != 'root' ]; then 9 | echo "You have to be root to run this script" 10 | echo "Fail !!!" 11 | exit 1; 12 | fi 13 | 14 | rm /etc/udev/rules.d/88-mvusb.rules 15 | echo "Remove /etc/udev/rules.d/88-mvusb.rules" 16 | 17 | rm /lib/libMVSDK.so 18 | echo "Remove lib/libMVSDK.so" 19 | 20 | echo "Successful" 21 | echo "Please restart system now!!!" 22 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/model/diffusion/positional_embedding.py: -------------------------------------------------------------------------------- 1 | import math 2 | import torch 3 | import torch.nn as nn 4 | 5 | class SinusoidalPosEmb(nn.Module): 6 | def __init__(self, dim): 7 | super().__init__() 8 | self.dim = dim 9 | 10 | def forward(self, x): 11 | device = x.device 12 | half_dim = self.dim // 2 13 | emb = math.log(10000) / (half_dim - 1) 14 | emb = torch.exp(torch.arange(half_dim, device=device) * -emb) 15 | emb = x[:, None] * emb[None, :] 16 | emb = torch.cat((emb.sin(), emb.cos()), dim=-1) 17 | return emb 18 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/misc/lapacke_mangling.h: -------------------------------------------------------------------------------- 1 | #ifndef LAPACK_HEADER_INCLUDED 2 | #define LAPACK_HEADER_INCLUDED 3 | 4 | #ifndef LAPACK_GLOBAL 5 | #if defined(LAPACK_GLOBAL_PATTERN_LC) || defined(ADD_) 6 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname##_ 7 | #elif defined(LAPACK_GLOBAL_PATTERN_UC) || defined(UPPER) 8 | #define LAPACK_GLOBAL(lcname,UCNAME) UCNAME 9 | #elif defined(LAPACK_GLOBAL_PATTERN_MC) || defined(NOCHANGE) 10 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname 11 | #else 12 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname##_ 13 | #endif 14 | #endif 15 | 16 | #endif 17 | 18 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/common/time_utils.py: -------------------------------------------------------------------------------- 1 | from rclpy.time import Time 2 | from rclpy.clock import ClockType 3 | 4 | 5 | def convert_float_to_ros_time(timestamp: float): 6 | """ 7 | Convert a float timestamp (in seconds) to a Time object 8 | """ 9 | seconds = int(timestamp) 10 | nanoseconds = int((timestamp - seconds) * 1e9) 11 | return Time(seconds=seconds, nanoseconds=nanoseconds, clock_type=ClockType.ROS_TIME) 12 | 13 | def convert_ros_time_to_float(time: Time) -> float: 14 | """ 15 | Convert a Time object to a float timestamp (in seconds) 16 | """ 17 | return time.nanoseconds * 1e-9 -------------------------------------------------------------------------------- /tests/test_cv2_util.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | 4 | ROOT_DIR = os.path.dirname(os.path.dirname(__file__)) 5 | sys.path.append(ROOT_DIR) 6 | os.chdir(ROOT_DIR) 7 | 8 | import numpy as np 9 | from reactive_diffusion_policy.common.cv2_util import get_image_transform 10 | 11 | 12 | def test(): 13 | tf = get_image_transform((1280,720), (640,480), bgr_to_rgb=False) 14 | in_img = np.zeros((720,1280,3), dtype=np.uint8) 15 | out_img = tf(in_img) 16 | # print(out_img.shape) 17 | assert out_img.shape == (480,640,3) 18 | 19 | # import pdb; pdb.set_trace() 20 | 21 | if __name__ == '__main__': 22 | test() 23 | -------------------------------------------------------------------------------- /third_party/mvsdk/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | setup( 4 | name="mvsdk", 5 | version="1.0", 6 | keywords=("mvsdk"), 7 | description="Mindvision Python SDK", 8 | long_description="Mindvision SDK for Python", 9 | license="MIT Licence", 10 | 11 | # packages = find_packages(), 12 | packages=['mvsdk'], 13 | package_data={ 14 | 'mvsdk': ['lib/*/*.so'] 15 | }, 16 | # platforms="linux", 17 | install_requires=[], 18 | # 19 | # scripts = [], 20 | # entry_points = { 21 | # 'console_scripts': [ 22 | # 'test = test.help:main' 23 | # ] 24 | # } 25 | ) 26 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/real_world/publisher/marker_track/makefile: -------------------------------------------------------------------------------- 1 | CC = g++ 2 | SRCS = src/tracking_class.cpp 3 | 4 | UNAME_S := $(shell uname -s) 5 | ifeq ($(UNAME_S),Linux) 6 | CFLAGS = -g -Wall -std=c++11 -O3 -Wall -fPIC -shared -std=c++11 `python3 -m pybind11 --includes` -I ./ 7 | PROG = src/lib/find_marker.so 8 | endif 9 | 10 | ifeq ($(UNAME_S),Darwin) 11 | CFLAGS = -g -Wall -std=c++11 -O3 -Wall -shared -std=c++11 -undefined dynamic_lookup `python3 -m pybind11 --includes` -I ./ 12 | PROG = src/lib/find_marker`python3-config --extension-suffix` 13 | endif 14 | 15 | $(PROG):$(SRCS) 16 | mkdir -p src/lib 17 | $(CC) $(CFLAGS) -o $(PROG) $(SRCS) 18 | 19 | clean: 20 | rm -rf src/lib 21 | -------------------------------------------------------------------------------- /eval.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # DP w. GelSight Emb. (Peeling) 4 | #python eval_real_robot_flexiv.py \ 5 | # --config-name train_diffusion_unet_real_image_workspace \ 6 | # task=real_peel_image_gelsight_emb_absolute_12fps \ 7 | # +task.env_runner.output_dir=/path/for/saving/videos \ 8 | # +ckpt_path=/path/to/dp/checkpoint 9 | 10 | # RDP w. Force (Peeling) 11 | python eval_real_robot_flexiv.py \ 12 | --config-name train_latent_diffusion_unet_real_image_workspace \ 13 | task=real_peel_image_wrench_ldp_24fps \ 14 | at=at_peel \ 15 | at_load_dir=/path/to/at/checkpoint \ 16 | +task.env_runner.output_dir=/path/for/saving/videos \ 17 | +ckpt_path=/path/to/ldp/checkpoint -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy==1.26.4 2 | numba 3 | scipy 4 | opencv-python 5 | cffi 6 | ipykernel 7 | matplotlib 8 | zarr 9 | numcodecs 10 | h5py 11 | hydra-core 12 | einops 13 | tqdm 14 | dill 15 | scikit-video 16 | scikit-image 17 | pymunk 18 | wandb 19 | threadpoolctl 20 | shapely 21 | cython 22 | imageio 23 | imageio-ffmpeg 24 | termcolor 25 | tensorboard 26 | tensorboardx 27 | psutil 28 | click 29 | boto3 30 | accelerate 31 | datasets 32 | diffusers 33 | av 34 | cmake 35 | timm 36 | peft 37 | # for teleop 38 | fastapi 39 | uvicorn 40 | pyrealsense2 41 | pydantic 42 | loguru 43 | bson 44 | open3d 45 | transforms3d 46 | pyinstrument 47 | py_cli_interaction 48 | pygame 49 | # install this by yourself 50 | # torch==1.13.1 51 | # torchvision==0.14.1 52 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/MoreVectorization: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // 5 | // This Source Code Form is subject to the terms of the Mozilla 6 | // Public License v. 2.0. If a copy of the MPL was not distributed 7 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 8 | 9 | #ifndef EIGEN_MOREVECTORIZATION_MODULE_H 10 | #define EIGEN_MOREVECTORIZATION_MODULE_H 11 | 12 | #include 13 | 14 | namespace Eigen { 15 | 16 | /** 17 | * \defgroup MoreVectorization More vectorization module 18 | */ 19 | 20 | } 21 | 22 | #include "src/MoreVectorization/MathFunctions.h" 23 | 24 | #endif // EIGEN_MOREVECTORIZATION_MODULE_H 25 | -------------------------------------------------------------------------------- /third_party/mvsdk/README.md: -------------------------------------------------------------------------------- 1 | # MVSDK Python Library 2 | 3 | ## Get started 4 | 5 | ### Windows 6 | 7 | - Install the mvsdk via the official installer 8 | - `python setup.py install` 9 | 10 | ### Linux 11 | 12 | - `python setup.py install` 13 | - Launch a Python terminal, run 14 | 15 | ```python 16 | from mvsdk.utils import install_driver 17 | install_driver() 18 | ``` 19 | 20 | ## How to remove 21 | 22 | ### Windows 23 | 24 | - Remove with the official uninstaller 25 | 26 | ### Linux 27 | 28 | - Launch a Python terminal, run 29 | 30 | ```python 31 | from mvsdk.utils import uninstall_driver 32 | uninstall_driver() 33 | ``` 34 | 35 | - `pip uninstall mvsdk` 36 | 37 | ## How to use 38 | 39 | Refer to the official guide. 40 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest a new feature for RDK 4 | title: "[FEATURE]" 5 | labels: enhancement 6 | assignees: pzhu-flexiv 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | A clear and concise description of what the problem is. E.g. I'm always frustrated when [...] 12 | 13 | **Describe the solution you'd like** 14 | A clear and concise description of what you want to happen. 15 | 16 | **Describe alternatives you've considered** 17 | A clear and concise description of any alternative solutions or features you've considered. 18 | 19 | **Additional context** 20 | Add any other context or screenshots about the feature request here. 21 | -------------------------------------------------------------------------------- /docs/data_collection_tips.md: -------------------------------------------------------------------------------- 1 | ## (Important) Data Collection Tips 2 | ### 1. Pay Attention to Tactile / Force Feedback during Teleoperation 3 | Since RDP relies on tactile / force information for real-time responses, 4 | the collected data have to include such reactive behavior based on tactile / force feedback. 5 | It is important to note that when data collectors become more proficient, 6 | they may tend to rely only on visual cues and muscle memory to perform the task, 7 | which has to be avoided. 8 | 9 | ### 2. Align the Settings of Data Collection and Inference 10 | As RDP is an imitation learning algorithm, 11 | if you want to test the model with human perturbations, 12 | it is essential to include such perturbations in the data collection process 13 | to enhance the reactive capability. 14 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/Core/functors/TernaryFunctors.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 Eugene Brevdo 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_TERNARY_FUNCTORS_H 11 | #define EIGEN_TERNARY_FUNCTORS_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | //---------- associative ternary functors ---------- 18 | 19 | 20 | 21 | } // end namespace internal 22 | 23 | } // end namespace Eigen 24 | 25 | #endif // EIGEN_TERNARY_FUNCTORS_H 26 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/model/common/shape_util.py: -------------------------------------------------------------------------------- 1 | from typing import Dict, List, Tuple, Callable 2 | import torch 3 | import torch.nn as nn 4 | 5 | def get_module_device(m: nn.Module): 6 | device = torch.device('cpu') 7 | try: 8 | param = next(iter(m.parameters())) 9 | device = param.device 10 | except StopIteration: 11 | pass 12 | return device 13 | 14 | @torch.no_grad() 15 | def get_output_shape( 16 | input_shape: Tuple[int], 17 | net: Callable[[torch.Tensor], torch.Tensor] 18 | ): 19 | device = get_module_device(net) 20 | test_input = torch.zeros((1,)+tuple(input_shape), device=device) 21 | test_output = net(test_input) 22 | output_shape = tuple(test_output.shape[1:]) 23 | return output_shape 24 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: "[BUG]" 5 | labels: bug 6 | assignees: pzhu-flexiv 7 | 8 | --- 9 | 10 | **Version information** 11 | - RDK: [e.g. v0.8] 12 | - Robot software: [e.g. v2.10.6] 13 | - OS: [e.g. Ubuntu 20.04 - x86_64] 14 | 15 | **Describe the bug** 16 | A clear and concise description of what the bug is. 17 | 18 | **Steps to reproduce** 19 | 1. '....' 20 | 2. '....' 21 | 3. '....' 22 | 4. See error 23 | 24 | **Expected behavior** 25 | A clear and concise description of what you expected to happen. 26 | 27 | **Screenshots** 28 | If applicable, add screenshots to help explain your problem. 29 | 30 | **Additional context** 31 | Add any other context about the problem here. 32 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/model/vision/choice_randomizer.py: -------------------------------------------------------------------------------- 1 | import random 2 | from collections.abc import Sequence 3 | import torch.nn as nn 4 | 5 | 6 | class RandomChoice(nn.Module): 7 | """Apply a single transformation randomly picked from a list. This transform does not support torchscript.""" 8 | 9 | def __init__(self, transforms, p=None): 10 | super().__init__() 11 | self.transforms = transforms 12 | if p is not None and not isinstance(p, Sequence): 13 | raise TypeError("Argument p should be a sequence") 14 | self.p = p 15 | 16 | def forward(self, *args): 17 | t = random.choices(self.transforms, weights=self.p)[0] 18 | return t(*args) 19 | 20 | def __repr__(self) -> str: 21 | return f"{self.__class__.__name__}(transforms={self.transforms}, p={self.p})" -------------------------------------------------------------------------------- /reactive_diffusion_policy/common/precise_sleep.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | def precise_sleep(dt: float, slack_time: float=0.001, time_func=time.monotonic): 4 | """ 5 | Use hybrid of time.sleep and spinning to minimize jitter. 6 | Sleep dt - slack_time seconds first, then spin for the rest. 7 | """ 8 | t_start = time_func() 9 | if dt > slack_time: 10 | time.sleep(dt - slack_time) 11 | t_end = t_start + dt 12 | while time_func() < t_end: 13 | pass 14 | return 15 | 16 | def precise_wait(t_end: float, slack_time: float=0.001, time_func=time.monotonic): 17 | t_start = time_func() 18 | t_wait = t_end - t_start 19 | if t_wait > 0: 20 | t_sleep = t_wait - slack_time 21 | if t_sleep > 0: 22 | time.sleep(t_sleep) 23 | while time_func() < t_end: 24 | pass 25 | return 26 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/model/vision/model_getter.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torchvision 3 | 4 | def get_resnet(name, weights=None, **kwargs): 5 | """ 6 | name: resnet18, resnet34, resnet50 7 | weights: "IMAGENET1K_V1", "r3m" 8 | """ 9 | # load r3m weights 10 | if (weights == "r3m") or (weights == "R3M"): 11 | return get_r3m(name=name, **kwargs) 12 | 13 | func = getattr(torchvision.models, name) 14 | resnet = func(weights=weights, **kwargs) 15 | resnet.fc = torch.nn.Identity() 16 | return resnet 17 | 18 | def get_r3m(name, **kwargs): 19 | """ 20 | name: resnet18, resnet34, resnet50 21 | """ 22 | import r3m 23 | r3m.device = 'cpu' 24 | model = r3m.load_r3m(name) 25 | r3m_model = model.module 26 | resnet_model = r3m_model.convnet 27 | resnet_model = resnet_model.to('cpu') 28 | return resnet_model 29 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/StdList: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Hauke Heibel 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_STDLIST_MODULE_H 11 | #define EIGEN_STDLIST_MODULE_H 12 | 13 | #include "Core" 14 | #include 15 | 16 | #if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ 17 | 18 | #define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) 19 | 20 | #else 21 | 22 | #include "src/StlSupport/StdList.h" 23 | 24 | #endif 25 | 26 | #endif // EIGEN_STDLIST_MODULE_H 27 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/CXX11/src/ThreadPool/ThreadYield.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_CXX11_THREADPOOL_THREAD_YIELD_H 11 | #define EIGEN_CXX11_THREADPOOL_THREAD_YIELD_H 12 | 13 | // Try to come up with a portable way to yield 14 | #if EIGEN_COMP_GNUC && EIGEN_GNUC_AT_MOST(4, 7) 15 | #define EIGEN_THREAD_YIELD() sched_yield() 16 | #else 17 | #define EIGEN_THREAD_YIELD() std::this_thread::yield() 18 | #endif 19 | 20 | #endif // EIGEN_CXX11_THREADPOOL_THREAD_YIELD_H 21 | -------------------------------------------------------------------------------- /third_party/mvsdk/install-msvdk.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | CURDIR=`pwd` 4 | echo "Your current directory is $CURDIR. This is where the MVSDK software will be installed..." 5 | CURR_USER=`whoami` 6 | CURR_ARCH=`arch` 7 | 8 | if [ $CURR_USER != 'root' ]; then 9 | echo "You have to be root to run this script" 10 | echo "Fail !!!" 11 | exit 1; 12 | fi 13 | 14 | 15 | cp 88-mvusb.rules /etc/udev/rules.d/ 16 | 17 | if [ $CURR_ARCH == 'x86_64' ]; then 18 | cp lib/x64/libMVSDK.so /lib 19 | echo "Copy x64/libMVSDK.so to /lib" 20 | elif [ $CURR_ARCH == 'aarch64' ]; then 21 | cp lib/arm64/libMVSDK.so /lib 22 | echo "Copy arm64/libMVSDK.so to /lib" 23 | elif [[ ${CURR_ARCH:2} == '86' ]]; then 24 | cp lib/x86/libMVSDK.so /lib 25 | echo "Copy x86/libMVSDK.so to /lib" 26 | else 27 | cp lib/arm/libMVSDK.so /lib 28 | echo "Copy arm/libMVSDK.so to /lib" 29 | fi 30 | 31 | echo "Successful" 32 | echo "Please restart system now!!!" 33 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/model/vae/utils.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import numpy as np 3 | import torch.nn as nn 4 | 5 | def weights_init_encoder(m): 6 | if isinstance(m, nn.Linear): 7 | nn.init.orthogonal_(m.weight.data) 8 | m.bias.data.fill_(0.0) 9 | elif isinstance(m, nn.Conv2d) or isinstance(m, nn.ConvTranspose2d): 10 | assert m.weight.size(2) == m.weight.size(3) 11 | m.weight.data.fill_(0.0) 12 | m.bias.data.fill_(0.0) 13 | mid = m.weight.size(2) // 2 14 | gain = nn.init.calculate_gain("relu") 15 | nn.init.orthogonal_(m.weight.data[:, :, mid, mid], gain) 16 | 17 | def get_tensor(z, device): 18 | if z is None: 19 | return None 20 | if z[0].dtype == np.dtype("O"): 21 | return None 22 | if len(z.shape) == 1: 23 | return torch.FloatTensor(z.copy()).to(device).unsqueeze(0) 24 | else: 25 | return torch.FloatTensor(z.copy()).to(device) 26 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/policy/base_image_policy.py: -------------------------------------------------------------------------------- 1 | from typing import Dict 2 | import torch 3 | from reactive_diffusion_policy.model.common.module_attr_mixin import ModuleAttrMixin 4 | from reactive_diffusion_policy.model.common.normalizer import LinearNormalizer 5 | 6 | class BaseImagePolicy(ModuleAttrMixin): 7 | # init accepts keyword argument shape_meta, see config/task/*_image.yaml 8 | 9 | def predict_action(self, obs_dict: Dict[str, torch.Tensor]) -> Dict[str, torch.Tensor]: 10 | """ 11 | obs_dict: 12 | str: B,To,* 13 | return: B,Ta,Da 14 | """ 15 | raise NotImplementedError() 16 | 17 | # reset state for stateful policies 18 | def reset(self): 19 | pass 20 | 21 | # ========== training =========== 22 | # no standard training interface except setting normalizer 23 | def set_normalizer(self, normalizer: LinearNormalizer): 24 | raise NotImplementedError() 25 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/StdDeque: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // Copyright (C) 2009 Hauke Heibel 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_STDDEQUE_MODULE_H 12 | #define EIGEN_STDDEQUE_MODULE_H 13 | 14 | #include "Core" 15 | #include 16 | 17 | #if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ 18 | 19 | #define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) 20 | 21 | #else 22 | 23 | #include "src/StlSupport/StdDeque.h" 24 | 25 | #endif 26 | 27 | #endif // EIGEN_STDDEQUE_MODULE_H 28 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/StdVector: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // Copyright (C) 2009 Hauke Heibel 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_STDVECTOR_MODULE_H 12 | #define EIGEN_STDVECTOR_MODULE_H 13 | 14 | #include "Core" 15 | #include 16 | 17 | #if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ 18 | 19 | #define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...) 20 | 21 | #else 22 | 23 | #include "src/StlSupport/StdVector.h" 24 | 25 | #endif 26 | 27 | #endif // EIGEN_STDVECTOR_MODULE_H 28 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/CXX11/src/ThreadPool/ThreadLocal.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_CXX11_THREADPOOL_THREAD_LOCAL_H 11 | #define EIGEN_CXX11_THREADPOOL_THREAD_LOCAL_H 12 | 13 | // Try to come up with a portable implementation of thread local variables 14 | #if EIGEN_COMP_GNUC && EIGEN_GNUC_AT_MOST(4, 7) 15 | #define EIGEN_THREAD_LOCAL static __thread 16 | #elif EIGEN_COMP_CLANG 17 | #define EIGEN_THREAD_LOCAL static __thread 18 | #else 19 | #define EIGEN_THREAD_LOCAL static thread_local 20 | #endif 21 | 22 | #endif // EIGEN_CXX11_THREADPOOL_THREAD_LOCAL_H 23 | -------------------------------------------------------------------------------- /docs/Q&A.md: -------------------------------------------------------------------------------- 1 | ## Q&A 2 | ### 1. Why can not my GelSight Mini get 24 FPS? 3 | We recommend using a high-performance CPU (e.g., Core i9-13900K) and 4 | enabling explicit CPU core binding to ensure 24 FPS for GelSight Mini. 5 | That is because GelSight Mini has a high resolution 6 | which requires high CPU resources and 7 | the OS scheduler may cause delays when launching multiple processes simultaneously. 8 | Therefore, if you are using GelSight Mini, we recommend executing the following steps to perform CPU core binding. 9 | 1. Add config into `/etc/security/limits.conf` to ensure the user has the permission to set realtime priority. 10 | ``` 11 | username - rtprio 99 12 | ``` 13 | 2. Edit `/etc/default/grub` and add `isolcpus=xxx` to the `GRUB_CMDLINE_LINUX_DEFAULT` line 14 | for isolating certain CPU cores. 15 | 3. Modify the task configuration file and 16 | the beginning several lines of all entry-point Python files (e.g. teleop.py) 17 | to adjust the corresponding core binding. 18 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/Core/util/ReenableStupidWarnings.h: -------------------------------------------------------------------------------- 1 | #ifdef EIGEN_WARNINGS_DISABLED 2 | #undef EIGEN_WARNINGS_DISABLED 3 | 4 | #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS 5 | #ifdef _MSC_VER 6 | #pragma warning( pop ) 7 | #elif defined __INTEL_COMPILER 8 | #pragma warning pop 9 | #elif defined __clang__ 10 | #pragma clang diagnostic pop 11 | #elif defined __GNUC__ && (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) 12 | #pragma GCC diagnostic pop 13 | #endif 14 | 15 | #if defined __NVCC__ 16 | // Don't reenable the diagnostic messages, as it turns out these messages need 17 | // to be disabled at the point of the template instantiation (i.e the user code) 18 | // otherwise they'll be triggered by nvcc. 19 | // #pragma diag_default code_is_unreachable 20 | // #pragma diag_default initialization_not_reachable 21 | // #pragma diag_default 2651 22 | // #pragma diag_default 2653 23 | #endif 24 | 25 | #endif 26 | 27 | #endif // EIGEN_WARNINGS_DISABLED 28 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/Householder: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_HOUSEHOLDER_MODULE_H 9 | #define EIGEN_HOUSEHOLDER_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup Householder_Module Householder module 16 | * This module provides Householder transformations. 17 | * 18 | * \code 19 | * #include 20 | * \endcode 21 | */ 22 | 23 | #include "src/Householder/Householder.h" 24 | #include "src/Householder/HouseholderSequence.h" 25 | #include "src/Householder/BlockHouseholder.h" 26 | 27 | #include "src/Core/util/ReenableStupidWarnings.h" 28 | 29 | #endif // EIGEN_HOUSEHOLDER_MODULE_H 30 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 31 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/Splines: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 20010-2011 Hauke Heibel 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPLINES_MODULE_H 11 | #define EIGEN_SPLINES_MODULE_H 12 | 13 | namespace Eigen 14 | { 15 | /** 16 | * \defgroup Splines_Module Spline and spline fitting module 17 | * 18 | * This module provides a simple multi-dimensional spline class while 19 | * offering most basic functionality to fit a spline to point sets. 20 | * 21 | * \code 22 | * #include 23 | * \endcode 24 | */ 25 | } 26 | 27 | #include "src/Splines/SplineFwd.h" 28 | #include "src/Splines/Spline.h" 29 | #include "src/Splines/SplineFitting.h" 30 | 31 | #endif // EIGEN_SPLINES_MODULE_H 32 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/ArpackSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // 5 | // This Source Code Form is subject to the terms of the Mozilla 6 | // Public License v. 2.0. If a copy of the MPL was not distributed 7 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 8 | 9 | #ifndef EIGEN_ARPACKSUPPORT_MODULE_H 10 | #define EIGEN_ARPACKSUPPORT_MODULE_H 11 | 12 | #include 13 | 14 | #include 15 | 16 | /** \defgroup ArpackSupport_Module Arpack support module 17 | * 18 | * This module provides a wrapper to Arpack, a library for sparse eigenvalue decomposition. 19 | * 20 | * \code 21 | * #include 22 | * \endcode 23 | */ 24 | 25 | #include 26 | #include "src/Eigenvalues/ArpackSelfAdjointEigenSolver.h" 27 | 28 | #include 29 | 30 | #endif // EIGEN_ARPACKSUPPORT_MODULE_H 31 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 32 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/Sparse: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSE_MODULE_H 9 | #define EIGEN_SPARSE_MODULE_H 10 | 11 | /** \defgroup Sparse_Module Sparse meta-module 12 | * 13 | * Meta-module including all related modules: 14 | * - \ref SparseCore_Module 15 | * - \ref OrderingMethods_Module 16 | * - \ref SparseCholesky_Module 17 | * - \ref SparseLU_Module 18 | * - \ref SparseQR_Module 19 | * - \ref IterativeLinearSolvers_Module 20 | * 21 | \code 22 | #include 23 | \endcode 24 | */ 25 | 26 | #include "SparseCore" 27 | #include "OrderingMethods" 28 | #ifndef EIGEN_MPL2_ONLY 29 | #include "SparseCholesky" 30 | #endif 31 | #include "SparseLU" 32 | #include "SparseQR" 33 | #include "IterativeLinearSolvers" 34 | 35 | #endif // EIGEN_SPARSE_MODULE_H 36 | 37 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/Jacobi: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_JACOBI_MODULE_H 9 | #define EIGEN_JACOBI_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup Jacobi_Module Jacobi module 16 | * This module provides Jacobi and Givens rotations. 17 | * 18 | * \code 19 | * #include 20 | * \endcode 21 | * 22 | * In addition to listed classes, it defines the two following MatrixBase methods to apply a Jacobi or Givens rotation: 23 | * - MatrixBase::applyOnTheLeft() 24 | * - MatrixBase::applyOnTheRight(). 25 | */ 26 | 27 | #include "src/Jacobi/Jacobi.h" 28 | 29 | #include "src/Core/util/ReenableStupidWarnings.h" 30 | 31 | #endif // EIGEN_JACOBI_MODULE_H 32 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 33 | 34 | -------------------------------------------------------------------------------- /train.py: -------------------------------------------------------------------------------- 1 | """ 2 | Usage: 3 | Training: 4 | python train.py --config-name=train_diffusion_lowdim_workspace 5 | """ 6 | 7 | import sys 8 | # use line-buffering for both stdout and stderr 9 | sys.stdout = open(sys.stdout.fileno(), mode='w', buffering=1) 10 | sys.stderr = open(sys.stderr.fileno(), mode='w', buffering=1) 11 | 12 | import hydra 13 | from omegaconf import OmegaConf 14 | import pathlib 15 | from reactive_diffusion_policy.workspace.base_workspace import BaseWorkspace 16 | 17 | # allows arbitrary python code execution in configs using the ${eval:''} resolver 18 | OmegaConf.register_new_resolver("eval", eval, replace=True) 19 | 20 | @hydra.main( 21 | version_base=None, 22 | config_path=str(pathlib.Path(__file__).parent.joinpath( 23 | 'reactive_diffusion_policy','config')) 24 | ) 25 | def main(cfg: OmegaConf): 26 | # resolve immediately so all the ${now:} resolvers 27 | # will use the same time. 28 | OmegaConf.resolve(cfg) 29 | 30 | cls = hydra.utils.get_class(cfg._target_) 31 | workspace: BaseWorkspace = cls(cfg) 32 | workspace.run() 33 | 34 | if __name__ == "__main__": 35 | main() 36 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/Skyline: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // 5 | // This Source Code Form is subject to the terms of the Mozilla 6 | // Public License v. 2.0. If a copy of the MPL was not distributed 7 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 8 | 9 | #ifndef EIGEN_SKYLINE_MODULE_H 10 | #define EIGEN_SKYLINE_MODULE_H 11 | 12 | 13 | #include "Eigen/Core" 14 | 15 | #include "Eigen/src/Core/util/DisableStupidWarnings.h" 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | /** 23 | * \defgroup Skyline_Module Skyline module 24 | * 25 | * 26 | * 27 | * 28 | */ 29 | 30 | #include "src/Skyline/SkylineUtil.h" 31 | #include "src/Skyline/SkylineMatrixBase.h" 32 | #include "src/Skyline/SkylineStorage.h" 33 | #include "src/Skyline/SkylineMatrix.h" 34 | #include "src/Skyline/SkylineInplaceLU.h" 35 | #include "src/Skyline/SkylineProduct.h" 36 | 37 | #include "Eigen/src/Core/util/ReenableStupidWarnings.h" 38 | 39 | #endif // EIGEN_SKYLINE_MODULE_H 40 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/Core/DiagonalProduct.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Gael Guennebaud 5 | // Copyright (C) 2007-2009 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_DIAGONALPRODUCT_H 12 | #define EIGEN_DIAGONALPRODUCT_H 13 | 14 | namespace Eigen { 15 | 16 | /** \returns the diagonal matrix product of \c *this by the diagonal matrix \a diagonal. 17 | */ 18 | template 19 | template 20 | inline const Product 21 | MatrixBase::operator*(const DiagonalBase &a_diagonal) const 22 | { 23 | return Product(derived(),a_diagonal.derived()); 24 | } 25 | 26 | } // end namespace Eigen 27 | 28 | #endif // EIGEN_DIAGONALPRODUCT_H 29 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/KroneckerProduct: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // 5 | // This Source Code Form is subject to the terms of the Mozilla 6 | // Public License v. 2.0. If a copy of the MPL was not distributed 7 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 8 | 9 | #ifndef EIGEN_KRONECKER_PRODUCT_MODULE_H 10 | #define EIGEN_KRONECKER_PRODUCT_MODULE_H 11 | 12 | #include "../../Eigen/Core" 13 | 14 | #include "../../Eigen/src/Core/util/DisableStupidWarnings.h" 15 | 16 | #include "../../Eigen/src/SparseCore/SparseUtil.h" 17 | 18 | namespace Eigen { 19 | 20 | /** 21 | * \defgroup KroneckerProduct_Module KroneckerProduct module 22 | * 23 | * This module contains an experimental Kronecker product implementation. 24 | * 25 | * \code 26 | * #include 27 | * \endcode 28 | */ 29 | 30 | } // namespace Eigen 31 | 32 | #include "src/KroneckerProduct/KroneckerTensorProduct.h" 33 | 34 | #include "../../Eigen/src/Core/util/ReenableStupidWarnings.h" 35 | 36 | #endif // EIGEN_KRONECKER_PRODUCT_MODULE_H 37 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/QtAlignedMalloc: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_QTMALLOC_MODULE_H 9 | #define EIGEN_QTMALLOC_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #if (!EIGEN_MALLOC_ALREADY_ALIGNED) 14 | 15 | #include "src/Core/util/DisableStupidWarnings.h" 16 | 17 | void *qMalloc(std::size_t size) 18 | { 19 | return Eigen::internal::aligned_malloc(size); 20 | } 21 | 22 | void qFree(void *ptr) 23 | { 24 | Eigen::internal::aligned_free(ptr); 25 | } 26 | 27 | void *qRealloc(void *ptr, std::size_t size) 28 | { 29 | void* newPtr = Eigen::internal::aligned_malloc(size); 30 | std::memcpy(newPtr, ptr, size); 31 | Eigen::internal::aligned_free(ptr); 32 | return newPtr; 33 | } 34 | 35 | #include "src/Core/util/ReenableStupidWarnings.h" 36 | 37 | #endif 38 | 39 | #endif // EIGEN_QTMALLOC_MODULE_H 40 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 41 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.3) 2 | project(flexiv_rdk-tests) 3 | 4 | # Show verbose build info 5 | SET(CMAKE_VERBOSE_MAKEFILE ON) 6 | 7 | message("OS: ${CMAKE_SYSTEM_NAME}") 8 | message("Processor: ${CMAKE_SYSTEM_PROCESSOR}") 9 | 10 | # Configure build type 11 | if(NOT CMAKE_BUILD_TYPE) 12 | set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE) 13 | endif() 14 | 15 | # Tests for Windows 16 | set(TEST_LIST 17 | test_log 18 | ) 19 | 20 | # Additional tests for Linux and Mac 21 | if(CMAKE_HOST_UNIX) 22 | list(APPEND TEST_LIST 23 | test_dynamics_engine 24 | test_dynamics_with_tool 25 | test_endurance 26 | test_loop_latency 27 | test_scheduler 28 | test_timeliness_monitor 29 | ) 30 | endif() 31 | 32 | # Find flexiv_rdk INTERFACE library 33 | find_package(flexiv_rdk REQUIRED) 34 | 35 | # Build all selected tests 36 | foreach(test ${TEST_LIST}) 37 | add_executable(${test} ${test}.cpp) 38 | target_link_libraries(${test} flexiv::flexiv_rdk) 39 | # C++14 required 40 | set_target_properties(${test} PROPERTIES 41 | CXX_STANDARD 14 42 | CXX_STANDARD_REQUIRED ON) 43 | endforeach() 44 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/MetisSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_METISSUPPORT_MODULE_H 9 | #define EIGEN_METISSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | 20 | /** \ingroup Support_modules 21 | * \defgroup MetisSupport_Module MetisSupport module 22 | * 23 | * \code 24 | * #include 25 | * \endcode 26 | * This module defines an interface to the METIS reordering package (http://glaros.dtc.umn.edu/gkhome/views/metis). 27 | * It can be used just as any other built-in method as explained in \link OrderingMethods_Module here. \endlink 28 | */ 29 | 30 | 31 | #include "src/MetisSupport/MetisSupport.h" 32 | 33 | #include "src/Core/util/ReenableStupidWarnings.h" 34 | 35 | #endif // EIGEN_METISSUPPORT_MODULE_H 36 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/real_world/publisher/setting.py: -------------------------------------------------------------------------------- 1 | 2 | def init(): 3 | global RESCALE, N_, M_, x0_, y0_, dx_, dy_, fps_ 4 | # RESCALE = 2 5 | 6 | # """ 7 | # N_, M_: the row and column of the marker array 8 | # x0_, y0_: the coordinate of upper-left marker (in original size) 9 | # dx_, dy_: the horizontal and vertical interval between adjacent markers (in original size) 10 | # fps_: the desired frame per second, the algorithm will find the optimal solution in 1/fps seconds 11 | # """ 12 | # N_ = 8 13 | # M_ = 8 14 | # fps_ = 30 15 | # x0_ = 320 / RESCALE 16 | # y0_ = 60 / RESCALE 17 | # dx_ = 84 / RESCALE 18 | # dy_ = 84 / RESCALE 19 | RESCALE = 1 20 | 21 | """ 22 | N_, M_: the row and column of the marker array 23 | x0_, y0_: the coordinate of upper-left marker (in original size) 24 | dx_, dy_: the horizontal and vertical interval between adjacent markers (in original size) 25 | fps_: the desired frame per second, the algorithm will find the optimal solution in 1/fps seconds 26 | """ 27 | N_ = 7 28 | M_ = 9 29 | fps_ = 30 30 | x0_ = 143 / RESCALE 31 | y0_ = 108 / RESCALE 32 | dx_ = 42 / RESCALE 33 | dy_ = 46 / RESCALE -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/AutoDiff: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2009 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_AUTODIFF_MODULE 11 | #define EIGEN_AUTODIFF_MODULE 12 | 13 | namespace Eigen { 14 | 15 | /** 16 | * \defgroup AutoDiff_Module Auto Diff module 17 | * 18 | * This module features forward automatic differentation via a simple 19 | * templated scalar type wrapper AutoDiffScalar. 20 | * 21 | * Warning : this should NOT be confused with numerical differentiation, which 22 | * is a different method and has its own module in Eigen : \ref NumericalDiff_Module. 23 | * 24 | * \code 25 | * #include 26 | * \endcode 27 | */ 28 | //@{ 29 | 30 | } 31 | 32 | #include "src/AutoDiff/AutoDiffScalar.h" 33 | // #include "src/AutoDiff/AutoDiffVector.h" 34 | #include "src/AutoDiff/AutoDiffJacobian.h" 35 | 36 | namespace Eigen { 37 | //@} 38 | } 39 | 40 | #endif // EIGEN_AUTODIFF_MODULE 41 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/SparseCore/SparseFuzzy.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2014 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSE_FUZZY_H 11 | #define EIGEN_SPARSE_FUZZY_H 12 | 13 | namespace Eigen { 14 | 15 | template 16 | template 17 | bool SparseMatrixBase::isApprox(const SparseMatrixBase& other, const RealScalar &prec) const 18 | { 19 | const typename internal::nested_eval::type actualA(derived()); 20 | typename internal::conditional::type, 22 | const PlainObject>::type actualB(other.derived()); 23 | 24 | return (actualA - actualB).squaredNorm() <= prec * prec * numext::mini(actualA.squaredNorm(), actualB.squaredNorm()); 25 | } 26 | 27 | } // end namespace Eigen 28 | 29 | #endif // EIGEN_SPARSE_FUZZY_H 30 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h: -------------------------------------------------------------------------------- 1 | namespace Eigen { 2 | 3 | namespace internal { 4 | 5 | // TODO : move this to GivensQR once there's such a thing in Eigen 6 | 7 | template 8 | void r1mpyq(DenseIndex m, DenseIndex n, Scalar *a, const std::vector > &v_givens, const std::vector > &w_givens) 9 | { 10 | typedef DenseIndex Index; 11 | 12 | /* apply the first set of givens rotations to a. */ 13 | for (Index j = n-2; j>=0; --j) 14 | for (Index i = 0; i 16 | 17 | /** \ingroup Support_modules 18 | * \defgroup PardisoSupport_Module PardisoSupport module 19 | * 20 | * This module brings support for the Intel(R) MKL PARDISO direct sparse solvers. 21 | * 22 | * \code 23 | * #include 24 | * \endcode 25 | * 26 | * In order to use this module, the MKL headers must be accessible from the include paths, and your binary must be linked to the MKL library and its dependencies. 27 | * See this \ref TopicUsingIntelMKL "page" for more information on MKL-Eigen integration. 28 | * 29 | */ 30 | 31 | #include "src/PardisoSupport/PardisoSupport.h" 32 | 33 | #include "src/Core/util/ReenableStupidWarnings.h" 34 | 35 | #endif // EIGEN_PARDISOSUPPORT_MODULE_H 36 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/CXX11/src/ThreadPool/ThreadPoolInterface.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_CXX11_THREADPOOL_THREAD_POOL_INTERFACE_H 11 | #define EIGEN_CXX11_THREADPOOL_THREAD_POOL_INTERFACE_H 12 | 13 | namespace Eigen { 14 | 15 | // This defines an interface that ThreadPoolDevice can take to use 16 | // custom thread pools underneath. 17 | class ThreadPoolInterface { 18 | public: 19 | virtual void Schedule(std::function fn) = 0; 20 | 21 | // Returns the number of threads in the pool. 22 | virtual int NumThreads() const = 0; 23 | 24 | // Returns a logical thread index between 0 and NumThreads() - 1 if called 25 | // from one of the threads in the pool. Returns -1 otherwise. 26 | virtual int CurrentThreadId() const = 0; 27 | 28 | virtual ~ThreadPoolInterface() {} 29 | }; 30 | 31 | } // namespace Eigen 32 | 33 | #endif // EIGEN_CXX11_THREADPOOL_THREAD_POOL_INTERFACE_H 34 | -------------------------------------------------------------------------------- /third_party/mvsdk/mvsdk/utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | import platform 3 | 4 | _rules = """KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="add", ATTR{idVendor}=="f622", MODE="666", TAG="mvusb_dev", A" 5 | KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="add", ATTR{idVendor}=="080b", MODE="666", TAG="mvusb_dev", A" 6 | KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="remove", TAG=="mvusb_dev", R" 7 | """ 8 | _rules_install_path = "/etc/udev/rules.d/88-mvusb.rules" 9 | 10 | 11 | def install_driver() -> bool: 12 | is_win = (platform.system() == "Windows") 13 | if is_win: 14 | print("You should run the .msi installer") 15 | return True 16 | try: 17 | print(f"Writing to {_rules_install_path}") 18 | with open(_rules_install_path, 'w') as f: 19 | f.write(_rules) 20 | except Exception as e: 21 | print(e) 22 | return False 23 | 24 | 25 | def uninstall_driver() -> bool: 26 | is_win = (platform.system() == "Windows") 27 | if is_win: 28 | print("You should uninstall from ControlPanel") 29 | return True 30 | else: 31 | try: 32 | os.remove(_rules_install_path) 33 | return True 34 | except Exception as e: 35 | print(e) 36 | return False 37 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/EulerAngles: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2015 Tal Hadad 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_EULERANGLES_MODULE_H 11 | #define EIGEN_EULERANGLES_MODULE_H 12 | 13 | 14 | #include "Eigen/Core" 15 | #include "Eigen/Geometry" 16 | 17 | #include "Eigen/src/Core/util/DisableStupidWarnings.h" 18 | 19 | namespace Eigen { 20 | 21 | /** 22 | * \defgroup EulerAngles_Module EulerAngles module 23 | * \brief This module provides generic euler angles rotation. 24 | * 25 | * Euler angles are a way to represent 3D rotation. 26 | * 27 | * In order to use this module in your code, include this header: 28 | * \code 29 | * #include 30 | * \endcode 31 | * 32 | * See \ref EulerAngles for more information. 33 | * 34 | */ 35 | 36 | } 37 | 38 | #include "src/EulerAngles/EulerSystem.h" 39 | #include "src/EulerAngles/EulerAngles.h" 40 | 41 | #include "Eigen/src/Core/util/ReenableStupidWarnings.h" 42 | 43 | #endif // EIGEN_EULERANGLES_MODULE_H 44 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/SPQRSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPQRSUPPORT_MODULE_H 9 | #define EIGEN_SPQRSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "SuiteSparseQR.hpp" 16 | 17 | /** \ingroup Support_modules 18 | * \defgroup SPQRSupport_Module SuiteSparseQR module 19 | * 20 | * This module provides an interface to the SPQR library, which is part of the suitesparse package. 21 | * 22 | * \code 23 | * #include 24 | * \endcode 25 | * 26 | * In order to use this module, the SPQR headers must be accessible from the include paths, and your binary must be linked to the SPQR library and its dependencies (Cholmod, AMD, COLAMD,...). 27 | * For a cmake based project, you can use our FindSPQR.cmake and FindCholmod.Cmake modules 28 | * 29 | */ 30 | 31 | #include "src/CholmodSupport/CholmodSupport.h" 32 | #include "src/SPQRSupport/SuiteSparseQRSupport.h" 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/LevenbergMarquardt: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Thomas Capricelli 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_LEVENBERGMARQUARDT_MODULE 11 | #define EIGEN_LEVENBERGMARQUARDT_MODULE 12 | 13 | // #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | /** 23 | * \defgroup LevenbergMarquardt_Module Levenberg-Marquardt module 24 | * 25 | * \code 26 | * #include 27 | * \endcode 28 | * 29 | * 30 | */ 31 | 32 | #include "Eigen/SparseCore" 33 | #ifndef EIGEN_PARSED_BY_DOXYGEN 34 | 35 | #include "src/LevenbergMarquardt/LMqrsolv.h" 36 | #include "src/LevenbergMarquardt/LMcovar.h" 37 | #include "src/LevenbergMarquardt/LMpar.h" 38 | 39 | #endif 40 | 41 | #include "src/LevenbergMarquardt/LevenbergMarquardt.h" 42 | #include "src/LevenbergMarquardt/LMonestep.h" 43 | 44 | 45 | #endif // EIGEN_LEVENBERGMARQUARDT_MODULE 46 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/CXX11/src/ThreadPool/ThreadEnvironment.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_CXX11_THREADPOOL_THREAD_ENVIRONMENT_H 11 | #define EIGEN_CXX11_THREADPOOL_THREAD_ENVIRONMENT_H 12 | 13 | namespace Eigen { 14 | 15 | struct StlThreadEnvironment { 16 | struct Task { 17 | std::function f; 18 | }; 19 | 20 | // EnvThread constructor must start the thread, 21 | // destructor must join the thread. 22 | class EnvThread { 23 | public: 24 | EnvThread(std::function f) : thr_(std::move(f)) {} 25 | ~EnvThread() { thr_.join(); } 26 | 27 | private: 28 | std::thread thr_; 29 | }; 30 | 31 | EnvThread* CreateThread(std::function f) { return new EnvThread(std::move(f)); } 32 | Task CreateTask(std::function f) { return Task{std::move(f)}; } 33 | void ExecuteTask(const Task& t) { t.f(); } 34 | }; 35 | 36 | } // namespace Eigen 37 | 38 | #endif // EIGEN_CXX11_THREADPOOL_THREAD_ENVIRONMENT_H 39 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/task/real_robot_env.yaml: -------------------------------------------------------------------------------- 1 | robot_name: 'flexiv_rizon' 2 | robot_server: 3 | host_ip: "192.168.2.187" 4 | port: 8092 5 | left_robot_ip: "192.168.2.110" 6 | right_robot_ip: "192.168.2.111" 7 | robot_config_file: 'reactive_diffusion_policy/config/robot/charmander.yaml' 8 | bimanual_teleop: &bimanual_teleop True 9 | publisher: 10 | vr_server_ip: &vr_server_ip '192.168.2.205' 11 | # vr_server_ip: &vr_server_ip '192.168.2.203' 12 | vr_server_port: 10002 13 | robot_publisher: 14 | robot_server_ip: ${task.robot_server.host_ip} 15 | robot_server_port: ${task.robot_server.port} 16 | fps: 120 17 | vr_server_ip: *vr_server_ip 18 | vr_server_tcp_port: 10001 19 | vr_server_force_port: 10005 20 | bimanual_teleop: *bimanual_teleop 21 | teleop_server: 22 | host_ip: "192.168.2.187" 23 | port: 8082 24 | fps: 60 25 | use_force_control_for_gripper: False 26 | max_gripper_width: 0.05 27 | min_gripper_width: 0.0 28 | grasp_force: 7.0 29 | gripper_control_time_interval: 30 # 30 for Flexiv Gripper, 20 for PGC-50 gripper 30 | gripper_control_width_precision: 0.01 # 0.01 for PGC-50 gripper, 0.01 for Flexiv Grav Gripper 31 | bimanual_teleop: *bimanual_teleop 32 | device_mapping_server: 33 | host_ip: '127.0.0.1' 34 | port: 8062 35 | transforms: 36 | calibration_path: 'data/calibration/v6' 37 | logger: 38 | level: DEBUG -------------------------------------------------------------------------------- /reactive_diffusion_policy/model/diffusion/conv1d_components.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | 4 | class Downsample1d(nn.Module): 5 | def __init__(self, dim): 6 | super().__init__() 7 | self.conv = nn.Conv1d(dim, dim, 3, 2, 1) 8 | 9 | def forward(self, x): 10 | return self.conv(x) 11 | 12 | class Upsample1d(nn.Module): 13 | def __init__(self, dim): 14 | super().__init__() 15 | self.conv = nn.ConvTranspose1d(dim, dim, 4, 2, 1) 16 | 17 | def forward(self, x): 18 | return self.conv(x) 19 | 20 | class Conv1dBlock(nn.Module): 21 | ''' 22 | Conv1d --> GroupNorm --> Mish 23 | ''' 24 | 25 | def __init__(self, inp_channels, out_channels, kernel_size, n_groups=8): 26 | super().__init__() 27 | 28 | self.block = nn.Sequential( 29 | nn.Conv1d(inp_channels, out_channels, kernel_size, padding=kernel_size // 2), 30 | # Rearrange('batch channels horizon -> batch channels 1 horizon'), 31 | nn.GroupNorm(n_groups, out_channels), 32 | # Rearrange('batch channels 1 horizon -> batch channels horizon'), 33 | nn.Mish(), 34 | ) 35 | 36 | def forward(self, x): 37 | return self.block(x) 38 | 39 | 40 | def test(): 41 | cb = Conv1dBlock(256, 128, kernel_size=3) 42 | x = torch.zeros((1,256,16)) 43 | o = cb(x) 44 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/test/test_log.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @test test_log.cpp 3 | * Test log functions of the flexiv::Log class 4 | * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. 5 | * @author Flexiv 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | void printHelp() 15 | { 16 | // clang-format off 17 | std::cout << "Required arguments: None" << std::endl; 18 | std::cout << "Optional arguments: None" << std::endl; 19 | std::cout << std::endl; 20 | // clang-format on 21 | } 22 | 23 | int main(int argc, char* argv[]) 24 | { 25 | if (flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { 26 | printHelp(); 27 | return 1; 28 | } 29 | 30 | // log object for printing message with timestamp and coloring 31 | flexiv::Log log; 32 | 33 | // print info message 34 | log.info("This is an INFO message with timestamp and GREEN coloring"); 35 | std::this_thread::sleep_for(std::chrono::seconds(1)); 36 | 37 | // print warning message 38 | log.warn("This is a WARNING message with timestamp and YELLOW coloring"); 39 | std::this_thread::sleep_for(std::chrono::seconds(1)); 40 | 41 | // print error message 42 | log.error("This is an ERROR message with timestamp and RED coloring"); 43 | 44 | return 0; 45 | } 46 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/SparseQR: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSEQR_MODULE_H 9 | #define EIGEN_SPARSEQR_MODULE_H 10 | 11 | #include "SparseCore" 12 | #include "OrderingMethods" 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup SparseQR_Module SparseQR module 16 | * \brief Provides QR decomposition for sparse matrices 17 | * 18 | * This module provides a simplicial version of the left-looking Sparse QR decomposition. 19 | * The columns of the input matrix should be reordered to limit the fill-in during the 20 | * decomposition. Built-in methods (COLAMD, AMD) or external methods (METIS) can be used to this end. 21 | * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list 22 | * of built-in and external ordering methods. 23 | * 24 | * \code 25 | * #include 26 | * \endcode 27 | * 28 | * 29 | */ 30 | 31 | #include "OrderingMethods" 32 | #include "src/SparseCore/SparseColEtree.h" 33 | #include "src/SparseQR/SparseQR.h" 34 | 35 | #include "src/Core/util/ReenableStupidWarnings.h" 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/real_world/device_mapping/device_mapping_utils.py: -------------------------------------------------------------------------------- 1 | # utils.py 2 | # dynamically get the currently running topics 3 | from sensor_msgs.msg import Image, PointCloud2 4 | from geometry_msgs.msg import PoseStamped, TwistStamped, WrenchStamped 5 | from sensor_msgs.msg import JointState 6 | from reactive_diffusion_policy.real_world.device_mapping.device_mapping_server import DeviceToTopic 7 | from loguru import logger 8 | 9 | def get_topic_and_type(device_to_topic: DeviceToTopic): 10 | subs_name_type = [] 11 | 12 | for camera_name, info in device_to_topic.realsense.items(): 13 | logger.debug(f'camera info: {info}') 14 | subs_name_type.append((f'/{camera_name}/color/image_raw', Image)) 15 | 16 | for camera_name, info in device_to_topic.usb.items(): 17 | subs_name_type.append((f'/{camera_name}/color/image_raw', Image)) 18 | subs_name_type.append((f'/{camera_name}/marker_offset/information', PointCloud2)) 19 | 20 | subs_name_type.extend([ 21 | ('/left_tcp_pose', PoseStamped), 22 | ('/right_tcp_pose', PoseStamped), 23 | ('/left_gripper_state', JointState), 24 | ('/right_gripper_state', JointState), 25 | ('/left_tcp_vel', TwistStamped), 26 | ('/right_tcp_vel', TwistStamped), 27 | ('/left_tcp_wrench', WrenchStamped), 28 | ('/right_tcp_wrench', WrenchStamped) 29 | ]) 30 | 31 | return subs_name_type 32 | 33 | 34 | -------------------------------------------------------------------------------- /docs/tactile_embedding_guide.md: -------------------------------------------------------------------------------- 1 | ## Guidance for Tactile Dataset and Tactile Embedding 2 | ### 1. Prepare Objects 3 | Prepare a few (>10) objects with different shapes, sizes and textures. The 20 objects we used in our tactile dataset is shown below: 4 | ![tactile_dataset](../assets/tactile_dataset.jpg) 5 | 6 | ### 2. Collect Data 7 | Use your hand to collect random interaction data between the object and the tactile sensor (e.g., GelSight Mini). Make sure that the interaction covers the following interaction patterns: 8 | ![marker_motion](../assets/marker_motion.png) 9 | You can download the raw videos of the [tactile dataset](https://huggingface.co/datasets/WendiChen/reactive_diffusion_policy_dataset/tree/main/dataset_tactile_embedding) used in our paper for reference. 10 | 11 | ### 3. Extract Marker Motion 12 | You can use [scripts/extract_gelsight_marker_motion.py](../reactive_diffusion_policy/scripts/extract_gelsight_marker_motion.py) to extract the marker motion ([zarr](https://github.com/zarr-developers/zarr-python) format) from the raw videos. 13 | 14 | ### 4. Generate Tactile Embedding 15 | You can use the [scripts/generate_pca_embedding.py](../reactive_diffusion_policy/scripts/generate_pca_embedding.py) to generate the PCA tactile embedding (.npy format) from the marker motion dataset (zarr format). 16 | You can find the pre-calculated PCA transformation matrix for our experiments in [data/PCA_Transform_GelSight](../data/PCA_Transform_GelSight). -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/Cholesky: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_CHOLESKY_MODULE_H 9 | #define EIGEN_CHOLESKY_MODULE_H 10 | 11 | #include "Core" 12 | #include "Jacobi" 13 | 14 | #include "src/Core/util/DisableStupidWarnings.h" 15 | 16 | /** \defgroup Cholesky_Module Cholesky module 17 | * 18 | * 19 | * 20 | * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices. 21 | * Those decompositions are also accessible via the following methods: 22 | * - MatrixBase::llt() 23 | * - MatrixBase::ldlt() 24 | * - SelfAdjointView::llt() 25 | * - SelfAdjointView::ldlt() 26 | * 27 | * \code 28 | * #include 29 | * \endcode 30 | */ 31 | 32 | #include "src/Cholesky/LLT.h" 33 | #include "src/Cholesky/LDLT.h" 34 | #ifdef EIGEN_USE_LAPACKE 35 | #ifdef EIGEN_USE_MKL 36 | #include "mkl_lapacke.h" 37 | #else 38 | #include "src/misc/lapacke.h" 39 | #endif 40 | #include "src/Cholesky/LLT_LAPACKE.h" 41 | #endif 42 | 43 | #include "src/Core/util/ReenableStupidWarnings.h" 44 | 45 | #endif // EIGEN_CHOLESKY_MODULE_H 46 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 47 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/task/real_peel_image_wrench_at_24fps.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - real_peel_two_realsense_one_gelsight_one_mctac_24fps@_here_ 3 | - _self_ 4 | 5 | name: real_peel_image_wrench_at_24fps 6 | 7 | image_shape: [3, 240, 320] 8 | dataset_path: data/peel_v1_zarr 9 | 10 | shape_meta: &shape_meta 11 | # acceptable types: rgb, low_dim 12 | obs: 13 | left_robot_tcp_pose: 14 | shape: [3] 15 | type: low_dim 16 | left_robot_gripper_width: 17 | shape: [1] 18 | type: low_dim 19 | left_robot_tcp_wrench: 20 | shape: [6] 21 | type: low_dim 22 | extended_obs: # for the AT RNN decoder of the latent diffusion policy 23 | left_robot_tcp_wrench: 24 | shape: [6] 25 | type: low_dim 26 | action: 27 | shape: [4] # (3+6+1) (x, y, z, 6d rotation, gripper_width) 28 | 29 | dataset: 30 | _target_: reactive_diffusion_policy.dataset.real_image_tactile_dataset.RealImageTactileDataset 31 | shape_meta: *shape_meta 32 | dataset_path: ${task.dataset_path} 33 | horizon: ${horizon} 34 | pad_before: ${eval:'${dataset_obs_steps}-1+${n_latency_steps}'} 35 | pad_after: ${eval:'${n_action_steps}-1'} 36 | n_obs_steps: ${dataset_obs_steps} 37 | obs_temporal_downsample_ratio: ${dataset_obs_temporal_downsample_ratio} 38 | n_latency_steps: ${n_latency_steps} 39 | seed: 42 40 | val_ratio: 0.00 41 | max_train_episodes: null 42 | delta_action: False 43 | relative_action: True 44 | 45 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/task/real_wipe_image_wrench_at_24fps.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - real_wipe_two_realsense_one_gelsight_one_mctac_24fps@_here_ 3 | - _self_ 4 | 5 | name: real_wipe_image_wrench_at_24fps 6 | 7 | image_shape: [3, 240, 320] 8 | dataset_path: data/wipe_v1_zarr 9 | 10 | shape_meta: &shape_meta 11 | # acceptable types: rgb, low_dim 12 | obs: 13 | left_robot_tcp_pose: 14 | shape: [9] 15 | type: low_dim 16 | left_robot_gripper_width: 17 | shape: [1] 18 | type: low_dim 19 | left_robot_tcp_wrench: 20 | shape: [6] 21 | type: low_dim 22 | extended_obs: # for the AT RNN decoder of the latent diffusion policy 23 | left_robot_tcp_wrench: 24 | shape: [6] 25 | type: low_dim 26 | action: 27 | shape: [10] # (3+6+1) (x, y, z, 6d rotation, gripper_width) 28 | 29 | dataset: 30 | _target_: reactive_diffusion_policy.dataset.real_image_tactile_dataset.RealImageTactileDataset 31 | shape_meta: *shape_meta 32 | dataset_path: ${task.dataset_path} 33 | horizon: ${horizon} 34 | pad_before: ${eval:'${dataset_obs_steps}-1+${n_latency_steps}'} 35 | pad_after: ${eval:'${n_action_steps}-1'} 36 | n_obs_steps: ${dataset_obs_steps} 37 | obs_temporal_downsample_ratio: ${dataset_obs_temporal_downsample_ratio} 38 | n_latency_steps: ${n_latency_steps} 39 | seed: 42 40 | val_ratio: 0.00 41 | max_train_episodes: null 42 | delta_action: False 43 | relative_action: True 44 | 45 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/real_world/publisher/marker_track/README.md: -------------------------------------------------------------------------------- 1 | # Marker Tracking Source Code 2 | 3 | ## Introduction 4 | 5 | In this directory (./reactive_diffusion_policy/real_world/publisher/marker_track), we provide the source code and compilation files for find_marker.so (located at ./reactive_diffusion_policy/real_world/publisher/lib/find_marker.so). 6 | 7 | While our precompiled find_marker.so suffices for most scenarios, we provide the source code here to accommodate platform-specific compilation requirements and enable user customization. 8 | 9 | ## Requirements 10 | 11 | * opencv 12 | * pybind11 13 | * numpy 14 | 15 | ``` 16 | pip3 install pybind11 numpy opencv-python 17 | ``` 18 | 19 | ## Build from source 20 | 21 | Enter the current directory 22 | 23 | ``` 24 | cd ./reactive_diffusion_policy/real_world/publisher/marker_track 25 | ``` 26 | 27 | Modify **makefile** based on your own platform 28 | 29 | Make the project 30 | 31 | ``` 32 | make 33 | ``` 34 | 35 | Now **find_marker.so** should be under directory ./reactive_diffusion_policy/real_world/publisher/marker_track/lib, simply **replace** it with the original find_marker.so(located at ./reactive_diffusion_policy/real_world/publisher/lib/find_marker.so). 36 | 37 | ## Third-party Components 38 | 39 | The marker tracking component of this project is based on: 40 | 41 | [tracking] by Shaoxiong Wang 42 | 43 | Source: https://github.com/Gelsight/tracking 44 | 45 | License: MIT 46 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/Core/arch/AVX/TypeCasting.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2015 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_TYPE_CASTING_AVX_H 11 | #define EIGEN_TYPE_CASTING_AVX_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | // For now we use SSE to handle integers, so we can't use AVX instructions to cast 18 | // from int to float 19 | template <> 20 | struct type_casting_traits { 21 | enum { 22 | VectorizedCast = 0, 23 | SrcCoeffRatio = 1, 24 | TgtCoeffRatio = 1 25 | }; 26 | }; 27 | 28 | template <> 29 | struct type_casting_traits { 30 | enum { 31 | VectorizedCast = 0, 32 | SrcCoeffRatio = 1, 33 | TgtCoeffRatio = 1 34 | }; 35 | }; 36 | 37 | 38 | 39 | template<> EIGEN_STRONG_INLINE Packet8i pcast(const Packet8f& a) { 40 | return _mm256_cvtps_epi32(a); 41 | } 42 | 43 | template<> EIGEN_STRONG_INLINE Packet8f pcast(const Packet8i& a) { 44 | return _mm256_cvtepi32_ps(a); 45 | } 46 | 47 | } // end namespace internal 48 | 49 | } // end namespace Eigen 50 | 51 | #endif // EIGEN_TYPE_CASTING_AVX_H 52 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/common/visualization_utils.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import open3d as o3d 4 | from typing import List 5 | 6 | 7 | def visualize_rgb_image(image: np.ndarray, name: str = 'Image'): 8 | cv2.imshow(name, image) 9 | cv2.waitKey(0) 10 | # cv2.destroyAllWindows() 11 | 12 | 13 | def visualize_pcd_from_numpy(xyz_rgb: np.ndarray): 14 | """ 15 | Visualize point cloud data 16 | :param xyz_rgb: numpy array of shape (N, 6) (x, y, z, r, g, b) 17 | """ 18 | raw_pcd = o3d.geometry.PointCloud() 19 | raw_pcd.points = o3d.utility.Vector3dVector(xyz_rgb[:, :3]) 20 | raw_pcd.colors = o3d.utility.Vector3dVector(xyz_rgb[:, 3:6]) # [0., 1.] 21 | coord = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0]) 22 | o3d.visualization.draw_geometries([raw_pcd, coord]) 23 | 24 | 25 | def visualize_tactile_marker(geometries: List[o3d.geometry.TriangleMesh], vis): 26 | ''' 27 | Visualizes tactile markers from the given geometries. 28 | :param geometries: List of Open3D TriangleMesh objects representing markers and arrows. 29 | :param vis: o3d.visualization.Visualizer() 30 | ''' 31 | vis.clear_geometries() 32 | for geom in geometries: 33 | vis.add_geometry(geom) 34 | 35 | view_ctl = vis.get_view_control() 36 | view_ctl.set_lookat([100, 100, 0]) 37 | view_ctl.set_up([0, 0, 1]) 38 | view_ctl.set_front([1, 1, 1]) 39 | 40 | vis.poll_events() 41 | vis.update_renderer() -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/CXX11/TensorSymmetry: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2013 Christian Seiler 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_CXX11_TENSORSYMMETRY_MODULE 11 | #define EIGEN_CXX11_TENSORSYMMETRY_MODULE 12 | 13 | #include 14 | 15 | #include 16 | 17 | #include "src/util/CXX11Meta.h" 18 | 19 | /** \defgroup CXX11_TensorSymmetry_Module Tensor Symmetry Module 20 | * 21 | * This module provides a classes that allow for the definition of 22 | * symmetries w.r.t. tensor indices. 23 | * 24 | * Including this module will implicitly include the Tensor module. 25 | * 26 | * \code 27 | * #include 28 | * \endcode 29 | */ 30 | 31 | #include "src/TensorSymmetry/util/TemplateGroupTheory.h" 32 | #include "src/TensorSymmetry/Symmetry.h" 33 | #include "src/TensorSymmetry/StaticSymmetry.h" 34 | #include "src/TensorSymmetry/DynamicSymmetry.h" 35 | 36 | #include 37 | 38 | #endif // EIGEN_CXX11_TENSORSYMMETRY_MODULE 39 | 40 | /* 41 | * kate: space-indent on; indent-width 2; mixedindent off; indent-mode cstyle; 42 | */ 43 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorGlobalFunctions.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 Eugene Brevdo 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_CXX11_TENSOR_TENSOR_GLOBAL_FUNCTIONS_H 11 | #define EIGEN_CXX11_TENSOR_TENSOR_GLOBAL_FUNCTIONS_H 12 | 13 | namespace Eigen { 14 | 15 | /** \cpp11 \returns an expression of the coefficient-wise betainc(\a x, \a a, \a b) to the given tensors. 16 | * 17 | * This function computes the regularized incomplete beta function (integral). 18 | * 19 | */ 20 | template 21 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const 22 | TensorCwiseTernaryOp, 23 | const ADerived, const BDerived, const XDerived> 24 | betainc(const ADerived& a, const BDerived& b, const XDerived& x) { 25 | return TensorCwiseTernaryOp< 26 | internal::scalar_betainc_op, const ADerived, 27 | const BDerived, const XDerived>( 28 | a, b, x, internal::scalar_betainc_op()); 29 | } 30 | 31 | } // end namespace Eigen 32 | 33 | #endif // EIGEN_CXX11_TENSOR_TENSOR_GLOBAL_FUNCTIONS_H 34 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/IterativeSolvers: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2009 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_ITERATIVE_SOLVERS_MODULE_H 11 | #define EIGEN_ITERATIVE_SOLVERS_MODULE_H 12 | 13 | #include 14 | 15 | /** 16 | * \defgroup IterativeSolvers_Module Iterative solvers module 17 | * This module aims to provide various iterative linear and non linear solver algorithms. 18 | * It currently provides: 19 | * - a constrained conjugate gradient 20 | * - a Householder GMRES implementation 21 | * \code 22 | * #include 23 | * \endcode 24 | */ 25 | //@{ 26 | 27 | #ifndef EIGEN_MPL2_ONLY 28 | #include "src/IterativeSolvers/IterationController.h" 29 | #include "src/IterativeSolvers/ConstrainedConjGrad.h" 30 | #endif 31 | 32 | #include "src/IterativeSolvers/IncompleteLU.h" 33 | #include "../../Eigen/Jacobi" 34 | #include "../../Eigen/Householder" 35 | #include "src/IterativeSolvers/GMRES.h" 36 | #include "src/IterativeSolvers/DGMRES.h" 37 | //#include "src/IterativeSolvers/SSORPreconditioner.h" 38 | #include "src/IterativeSolvers/MINRES.h" 39 | 40 | //@} 41 | 42 | #endif // EIGEN_ITERATIVE_SOLVERS_MODULE_H 43 | -------------------------------------------------------------------------------- /train_rdp.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | GPU_ID=0 4 | 5 | TASK_NAME="peel" 6 | DATASET_PATH="/home/wendi/Desktop/record_data/peel_v3_downsample1_zarr" 7 | LOGGING_MODE="online" 8 | 9 | TIMESTAMP=$(date +%m%d%H%M%S) 10 | SEARCH_PATH="./data/outputs" 11 | 12 | # Stage 1: Train Asymmetric Tokenizer 13 | echo "Stage 1: training Asymmetric Tokenizer..." 14 | CUDA_VISIBLE_DEVICES=${GPU_ID} python train.py \ 15 | --config-name=train_at_workspace \ 16 | task=real_${TASK_NAME}_image_gelsight_emb_at_24fps \ 17 | task.dataset_path=${DATASET_PATH} \ 18 | task.name=real_${TASK_NAME}_image_gelsight_emb_at_24fps_${TIMESTAMP} \ 19 | at=at_peel \ 20 | logging.mode=${LOGGING_MODE} 21 | 22 | # find the latest checkpoint 23 | echo "" 24 | echo "Searching for the latest AT checkpoint..." 25 | AT_LOAD_DIR=$(find "${SEARCH_PATH}" -maxdepth 2 -path "*${TIMESTAMP}*" -type d)/checkpoints/latest.ckpt 26 | 27 | if [ ! -f "${AT_LOAD_DIR}" ]; then 28 | echo "Error: VAE checkpoint not found at ${AT_LOAD_DIR}" 29 | exit 1 30 | fi 31 | 32 | # Stage 2: Train Latent Diffusion Policy 33 | echo "" 34 | echo "Stage 2: training Latent Diffusion Policy..." 35 | CUDA_VISIBLE_DEVICES=${GPU_ID} accelerate launch train.py \ 36 | --config-name=train_latent_diffusion_unet_real_image_workspace \ 37 | task=real_${TASK_NAME}_image_gelsight_emb_ldp_24fps \ 38 | task.dataset_path=${DATASET_PATH} \ 39 | task.name=real_${TASK_NAME}_image_gelsight_emb_ldp_24fps_${TIMESTAMP} \ 40 | at=at_peel \ 41 | at_load_dir=${AT_LOAD_DIR} \ 42 | logging.mode=${LOGGING_MODE} -------------------------------------------------------------------------------- /reactive_diffusion_policy/model/common/dict_of_tensor_mixin.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | 4 | class DictOfTensorMixin(nn.Module): 5 | def __init__(self, params_dict=None): 6 | super().__init__() 7 | if params_dict is None: 8 | params_dict = nn.ParameterDict() 9 | self.params_dict = params_dict 10 | 11 | @property 12 | def device(self): 13 | return next(iter(self.parameters())).device 14 | 15 | def _load_from_state_dict(self, state_dict, prefix, local_metadata, strict, missing_keys, unexpected_keys, error_msgs): 16 | def dfs_add(dest, keys, value: torch.Tensor): 17 | if len(keys) == 1: 18 | dest[keys[0]] = value 19 | return 20 | 21 | if keys[0] not in dest: 22 | dest[keys[0]] = nn.ParameterDict() 23 | dfs_add(dest[keys[0]], keys[1:], value) 24 | 25 | def load_dict(state_dict, prefix): 26 | out_dict = nn.ParameterDict() 27 | for key, value in state_dict.items(): 28 | value: torch.Tensor 29 | if key.startswith(prefix): 30 | param_keys = key[len(prefix):].split('.')[1:] 31 | # if len(param_keys) == 0: 32 | # import pdb; pdb.set_trace() 33 | dfs_add(out_dict, param_keys, value.clone()) 34 | return out_dict 35 | 36 | self.params_dict = load_dict(state_dict, prefix + 'params_dict') 37 | self.params_dict.requires_grad_(False) 38 | return 39 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/QR: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_QR_MODULE_H 9 | #define EIGEN_QR_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "Cholesky" 16 | #include "Jacobi" 17 | #include "Householder" 18 | 19 | /** \defgroup QR_Module QR module 20 | * 21 | * 22 | * 23 | * This module provides various QR decompositions 24 | * This module also provides some MatrixBase methods, including: 25 | * - MatrixBase::householderQr() 26 | * - MatrixBase::colPivHouseholderQr() 27 | * - MatrixBase::fullPivHouseholderQr() 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | */ 33 | 34 | #include "src/QR/HouseholderQR.h" 35 | #include "src/QR/FullPivHouseholderQR.h" 36 | #include "src/QR/ColPivHouseholderQR.h" 37 | #include "src/QR/CompleteOrthogonalDecomposition.h" 38 | #ifdef EIGEN_USE_LAPACKE 39 | #ifdef EIGEN_USE_MKL 40 | #include "mkl_lapacke.h" 41 | #else 42 | #include "src/misc/lapacke.h" 43 | #endif 44 | #include "src/QR/HouseholderQR_LAPACKE.h" 45 | #include "src/QR/ColPivHouseholderQR_LAPACKE.h" 46 | #endif 47 | 48 | #include "src/Core/util/ReenableStupidWarnings.h" 49 | 50 | #endif // EIGEN_QR_MODULE_H 51 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 52 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/UmfPackSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_UMFPACKSUPPORT_MODULE_H 9 | #define EIGEN_UMFPACKSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | /** \ingroup Support_modules 20 | * \defgroup UmfPackSupport_Module UmfPackSupport module 21 | * 22 | * This module provides an interface to the UmfPack library which is part of the suitesparse package. 23 | * It provides the following factorization class: 24 | * - class UmfPackLU: a multifrontal sequential LU factorization. 25 | * 26 | * \code 27 | * #include 28 | * \endcode 29 | * 30 | * In order to use this module, the umfpack headers must be accessible from the include paths, and your binary must be linked to the umfpack library and its dependencies. 31 | * The dependencies depend on how umfpack has been compiled. 32 | * For a cmake based project, you can use our FindUmfPack.cmake module to help you in this task. 33 | * 34 | */ 35 | 36 | #include "src/UmfPackSupport/UmfPackSupport.h" 37 | 38 | #include "src/Core/util/ReenableStupidWarnings.h" 39 | 40 | #endif // EIGEN_UMFPACKSUPPORT_MODULE_H 41 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/scripts/reencode_videos.py: -------------------------------------------------------------------------------- 1 | import os 2 | import subprocess 3 | 4 | def find_mp4_files(directory): 5 | """Recursively find all MP4 files in the specified directory and its subdirectories""" 6 | mp4_files = [] 7 | for root, dirs, files in os.walk(directory): 8 | for file in files: 9 | if file.endswith(".mp4"): 10 | mp4_files.append(os.path.join(root, file)) 11 | return mp4_files 12 | 13 | def reencode_mp4(input_file, output_file): 14 | """Re-encode MP4 files using FFmpeg""" 15 | command = [ 16 | 'ffmpeg', 17 | '-i', input_file, 18 | '-c:v', 'libx264', 19 | '-crf', '18', 20 | '-preset', 'medium', 21 | '-c:a', 'aac', 22 | '-b:a', '128k', 23 | output_file 24 | ] 25 | subprocess.run(command, check=True) 26 | 27 | def replace_with_reencoded_mp4(directory): 28 | """Traverse all MP4 files in the directory and its subdirectories, and replace the original files with re-encoded files""" 29 | mp4_files = find_mp4_files(directory) 30 | for input_file in mp4_files: 31 | # 生成输出文件路径 32 | output_file = input_file.rsplit('.', 1)[0] + '_reencoded.mp4' 33 | # 重新编码文件 34 | reencode_mp4(input_file, output_file) 35 | # 删除原文件 36 | os.remove(input_file) 37 | # 重命名重新编码的文件为原文件名 38 | os.rename(output_file, input_file) 39 | print(f"Reencoded and replaced: {input_file}") 40 | 41 | if __name__ == "__main__": 42 | directory = "" # Replace with your directory path 43 | replace_with_reencoded_mp4(directory) -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/SparseCholesky: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2013 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSECHOLESKY_MODULE_H 11 | #define EIGEN_SPARSECHOLESKY_MODULE_H 12 | 13 | #include "SparseCore" 14 | #include "OrderingMethods" 15 | 16 | #include "src/Core/util/DisableStupidWarnings.h" 17 | 18 | /** 19 | * \defgroup SparseCholesky_Module SparseCholesky module 20 | * 21 | * This module currently provides two variants of the direct sparse Cholesky decomposition for selfadjoint (hermitian) matrices. 22 | * Those decompositions are accessible via the following classes: 23 | * - SimplicialLLt, 24 | * - SimplicialLDLt 25 | * 26 | * Such problems can also be solved using the ConjugateGradient solver from the IterativeLinearSolvers module. 27 | * 28 | * \code 29 | * #include 30 | * \endcode 31 | */ 32 | 33 | #ifdef EIGEN_MPL2_ONLY 34 | #error The SparseCholesky module has nothing to offer in MPL2 only mode 35 | #endif 36 | 37 | #include "src/SparseCholesky/SimplicialCholesky.h" 38 | 39 | #ifndef EIGEN_MPL2_ONLY 40 | #include "src/SparseCholesky/SimplicialCholesky_impl.h" 41 | #endif 42 | 43 | #include "src/Core/util/ReenableStupidWarnings.h" 44 | 45 | #endif // EIGEN_SPARSECHOLESKY_MODULE_H 46 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/SparseExtra: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2009 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSE_EXTRA_MODULE_H 11 | #define EIGEN_SPARSE_EXTRA_MODULE_H 12 | 13 | #include "../../Eigen/Sparse" 14 | 15 | #include "../../Eigen/src/Core/util/DisableStupidWarnings.h" 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #ifdef EIGEN_GOOGLEHASH_SUPPORT 26 | #include 27 | #endif 28 | 29 | /** 30 | * \defgroup SparseExtra_Module SparseExtra module 31 | * 32 | * This module contains some experimental features extending the sparse module. 33 | * 34 | * \code 35 | * #include 36 | * \endcode 37 | */ 38 | 39 | 40 | #include "src/SparseExtra/DynamicSparseMatrix.h" 41 | #include "src/SparseExtra/BlockOfDynamicSparseMatrix.h" 42 | #include "src/SparseExtra/RandomSetter.h" 43 | 44 | #include "src/SparseExtra/MarketIO.h" 45 | 46 | #if !defined(_WIN32) 47 | #include 48 | #include "src/SparseExtra/MatrixMarketIterator.h" 49 | #endif 50 | 51 | #include "../../Eigen/src/Core/util/ReenableStupidWarnings.h" 52 | 53 | #endif // EIGEN_SPARSE_EXTRA_MODULE_H 54 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/task/real_peel_image_mctac_emb_at_24fps.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - real_peel_two_realsense_one_gelsight_one_mctac_24fps@_here_ 3 | - _self_ 4 | 5 | name: real_peel_image_mctac_emb_at_24fps 6 | 7 | image_shape: [3, 240, 320] 8 | dataset_path: data/peel_v1_zarr 9 | 10 | shape_meta: &shape_meta 11 | # acceptable types: rgb, low_dim 12 | obs: 13 | left_robot_tcp_pose: 14 | shape: [3] 15 | type: low_dim 16 | left_robot_gripper_width: 17 | shape: [1] 18 | type: low_dim 19 | left_gripper2_marker_offset_emb: 20 | shape: [ 15 ] # the raw dimension is (63*2), we use PCA to reduce the dimension to 15 21 | type: low_dim 22 | extended_obs: # for the AT RNN decoder of the latent diffusion policy 23 | left_gripper2_marker_offset_emb: 24 | shape: [ 15 ] # the raw dimension is (63*2), we use PCA to reduce the dimension to 15 25 | type: low_dim 26 | action: 27 | shape: [4] # (3+6+1) (x, y, z, 6d rotation, gripper_width) 28 | 29 | dataset: 30 | _target_: reactive_diffusion_policy.dataset.real_image_tactile_dataset.RealImageTactileDataset 31 | shape_meta: *shape_meta 32 | dataset_path: ${task.dataset_path} 33 | horizon: ${horizon} 34 | pad_before: ${eval:'${dataset_obs_steps}-1+${n_latency_steps}'} 35 | pad_after: ${eval:'${n_action_steps}-1'} 36 | n_obs_steps: ${dataset_obs_steps} 37 | obs_temporal_downsample_ratio: ${dataset_obs_temporal_downsample_ratio} 38 | n_latency_steps: ${n_latency_steps} 39 | seed: 42 40 | val_ratio: 0.00 41 | max_train_episodes: null 42 | delta_action: False 43 | relative_action: True 44 | 45 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorMacros.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2015 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_CXX11_TENSOR_TENSOR_META_MACROS_H 11 | #define EIGEN_CXX11_TENSOR_TENSOR_META_MACROS_H 12 | 13 | 14 | /** use this macro in sfinae selection in templated functions 15 | * 16 | * template::value , int >::type = 0 18 | * > 19 | * void foo(){} 20 | * 21 | * becomes => 22 | * 23 | * template::value ) 25 | * > 26 | * void foo(){} 27 | */ 28 | 29 | // SFINAE requires variadic templates 30 | #ifndef __CUDACC__ 31 | #if EIGEN_HAS_VARIADIC_TEMPLATES 32 | // SFINAE doesn't work for gcc <= 4.7 33 | #ifdef EIGEN_COMP_GNUC 34 | #if EIGEN_GNUC_AT_LEAST(4,8) 35 | #define EIGEN_HAS_SFINAE 36 | #endif 37 | #else 38 | #define EIGEN_HAS_SFINAE 39 | #endif 40 | #endif 41 | #endif 42 | 43 | #define EIGEN_SFINAE_ENABLE_IF( __condition__ ) \ 44 | typename internal::enable_if< ( __condition__ ) , int >::type = 0 45 | 46 | 47 | #if EIGEN_HAS_CONSTEXPR 48 | #define EIGEN_CONSTEXPR constexpr 49 | #else 50 | #define EIGEN_CONSTEXPR 51 | #endif 52 | 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/dataset/base_dataset.py: -------------------------------------------------------------------------------- 1 | from typing import Dict 2 | 3 | import torch.nn 4 | from reactive_diffusion_policy.model.common.normalizer import LinearNormalizer 5 | 6 | class BaseLowdimDataset(torch.utils.data.Dataset): 7 | def get_validation_dataset(self) -> 'BaseLowdimDataset': 8 | # return an empty dataset by default 9 | return BaseLowdimDataset() 10 | 11 | def get_normalizer(self, **kwargs) -> LinearNormalizer: 12 | raise NotImplementedError() 13 | 14 | def get_all_actions(self) -> torch.Tensor: 15 | raise NotImplementedError() 16 | 17 | def __len__(self) -> int: 18 | return 0 19 | 20 | def __getitem__(self, idx: int) -> Dict[str, torch.Tensor]: 21 | """ 22 | output: 23 | obs: T, Do 24 | action: T, Da 25 | """ 26 | raise NotImplementedError() 27 | 28 | 29 | class BaseImageDataset(torch.utils.data.Dataset): 30 | def get_validation_dataset(self) -> 'BaseLowdimDataset': 31 | # return an empty dataset by default 32 | return BaseImageDataset() 33 | 34 | def get_normalizer(self, **kwargs) -> LinearNormalizer: 35 | raise NotImplementedError() 36 | 37 | def get_all_actions(self) -> torch.Tensor: 38 | raise NotImplementedError() 39 | 40 | def __len__(self) -> int: 41 | return 0 42 | 43 | def __getitem__(self, idx: int) -> Dict[str, torch.Tensor]: 44 | """ 45 | output: 46 | obs: 47 | key: T, * 48 | action: T, Da 49 | """ 50 | raise NotImplementedError() 51 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/example/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.3) 2 | project(flexiv_rdk-examples) 3 | 4 | # Show verbose build info 5 | SET(CMAKE_VERBOSE_MAKEFILE ON) 6 | 7 | message("OS: ${CMAKE_SYSTEM_NAME}") 8 | message("Processor: ${CMAKE_SYSTEM_PROCESSOR}") 9 | 10 | # Configure build type 11 | if(NOT CMAKE_BUILD_TYPE) 12 | set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE) 13 | endif() 14 | 15 | # Minumum example list for all OS 16 | set(EXAMPLE_LIST 17 | basics1_display_robot_states 18 | basics2_clear_fault 19 | basics3_primitive_execution 20 | basics4_plan_execution 21 | basics5_zero_force_torque_sensors 22 | basics6_gripper_control 23 | basics7_auto_recovery 24 | intermediate7_teach_by_demonstration 25 | ) 26 | 27 | 28 | # Additional examples for Linux and Mac 29 | if(CMAKE_HOST_UNIX) 30 | list(APPEND EXAMPLE_LIST 31 | intermediate1_realtime_joint_position_control 32 | intermediate2_realtime_joint_torque_control 33 | intermediate3_realtime_joint_floating 34 | intermediate4_realtime_cartesian_pure_motion_control 35 | intermediate5_realtime_cartesian_motion_force_control 36 | intermediate6_robot_dynamics 37 | ) 38 | endif() 39 | 40 | # Find flexiv_rdk INTERFACE library 41 | find_package(flexiv_rdk REQUIRED) 42 | 43 | # Build all selected examples 44 | foreach(example ${EXAMPLE_LIST}) 45 | add_executable(${example} ${example}.cpp) 46 | target_link_libraries(${example} flexiv::flexiv_rdk) 47 | # C++14 required 48 | set_target_properties(${example} PROPERTIES 49 | CXX_STANDARD 14 50 | CXX_STANDARD_REQUIRED ON) 51 | endforeach() 52 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/task/real_peel_image_gelsight_emb_at_24fps.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - real_peel_two_realsense_one_gelsight_one_mctac_24fps@_here_ 3 | - _self_ 4 | 5 | name: real_peel_image_gelsight_emb_at_24fps 6 | 7 | image_shape: [3, 240, 320] 8 | dataset_path: data/peel_v1_zarr 9 | 10 | shape_meta: &shape_meta 11 | # acceptable types: rgb, low_dim 12 | obs: 13 | left_robot_tcp_pose: 14 | shape: [3] 15 | type: low_dim 16 | left_robot_gripper_width: 17 | shape: [1] 18 | type: low_dim 19 | left_gripper1_marker_offset_emb: 20 | shape: [ 15 ] # the raw dimension is (63*2), we use PCA to reduce the dimension to 15 21 | type: low_dim 22 | extended_obs: # for the AT RNN decoder of the latent diffusion policy 23 | left_gripper1_marker_offset_emb: 24 | shape: [ 15 ] # the raw dimension is (63*2), we use PCA to reduce the dimension to 15 25 | type: low_dim 26 | action: 27 | shape: [4] # (3+6+1) (x, y, z, 6d rotation, gripper_width) 28 | 29 | dataset: 30 | _target_: reactive_diffusion_policy.dataset.real_image_tactile_dataset.RealImageTactileDataset 31 | shape_meta: *shape_meta 32 | dataset_path: ${task.dataset_path} 33 | horizon: ${horizon} 34 | pad_before: ${eval:'${dataset_obs_steps}-1+${n_latency_steps}'} 35 | pad_after: ${eval:'${n_action_steps}-1'} 36 | n_obs_steps: ${dataset_obs_steps} 37 | obs_temporal_downsample_ratio: ${dataset_obs_temporal_downsample_ratio} 38 | n_latency_steps: ${n_latency_steps} 39 | seed: 42 40 | val_ratio: 0.00 41 | max_train_episodes: null 42 | delta_action: False 43 | relative_action: True 44 | 45 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/task/real_wipe_image_gelsight_emb_at_24fps.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - real_wipe_two_realsense_one_gelsight_one_mctac_24fps@_here_ 3 | - _self_ 4 | 5 | name: real_wipe_image_gelsight_emb_at_24fps 6 | 7 | image_shape: [3, 240, 320] 8 | dataset_path: data/wipe_v1_zarr 9 | 10 | shape_meta: &shape_meta 11 | # acceptable types: rgb, low_dim 12 | obs: 13 | left_robot_tcp_pose: 14 | shape: [9] 15 | type: low_dim 16 | left_robot_gripper_width: 17 | shape: [1] 18 | type: low_dim 19 | left_gripper1_marker_offset_emb: 20 | shape: [ 15 ] # the raw dimension is (63*2), we use PCA to reduce the dimension to 15 21 | type: low_dim 22 | extended_obs: # for the AT RNN decoder of the latent diffusion policy 23 | left_gripper1_marker_offset_emb: 24 | shape: [ 15 ] # the raw dimension is (63*2), we use PCA to reduce the dimension to 15 25 | type: low_dim 26 | action: 27 | shape: [10] # (3+6+1) (x, y, z, 6d rotation, gripper_width) 28 | 29 | dataset: 30 | _target_: reactive_diffusion_policy.dataset.real_image_tactile_dataset.RealImageTactileDataset 31 | shape_meta: *shape_meta 32 | dataset_path: ${task.dataset_path} 33 | horizon: ${horizon} 34 | pad_before: ${eval:'${dataset_obs_steps}-1+${n_latency_steps}'} 35 | pad_after: ${eval:'${n_action_steps}-1'} 36 | n_obs_steps: ${dataset_obs_steps} 37 | obs_temporal_downsample_ratio: ${dataset_obs_temporal_downsample_ratio} 38 | n_latency_steps: ${n_latency_steps} 39 | seed: 42 40 | val_ratio: 0.00 41 | max_train_episodes: null 42 | delta_action: False 43 | relative_action: True 44 | 45 | -------------------------------------------------------------------------------- /tests/test_precise_sleep.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | 4 | ROOT_DIR = os.path.dirname(os.path.dirname(__file__)) 5 | sys.path.append(ROOT_DIR) 6 | os.chdir(ROOT_DIR) 7 | 8 | import time 9 | import numpy as np 10 | from reactive_diffusion_policy.common.precise_sleep import precise_sleep, precise_wait 11 | 12 | 13 | def test_sleep(): 14 | dt = 0.1 15 | tol = 1e-3 16 | time_samples = list() 17 | for i in range(100): 18 | precise_sleep(dt) 19 | # time.sleep(dt) 20 | time_samples.append(time.monotonic()) 21 | time_deltas = np.diff(time_samples) 22 | 23 | from matplotlib import pyplot as plt 24 | plt.plot(time_deltas) 25 | plt.ylim((dt-tol,dt+tol)) 26 | 27 | 28 | def test_wait(): 29 | dt = 0.1 30 | tol = 1e-3 31 | errors = list() 32 | t_start = time.monotonic() 33 | for i in range(1,100): 34 | t_end_desired = t_start + i * dt 35 | time.sleep(t_end_desired - time.monotonic()) 36 | t_end = time.monotonic() 37 | errors.append(t_end - t_end_desired) 38 | 39 | new_errors = list() 40 | t_start = time.monotonic() 41 | for i in range(1,100): 42 | t_end_desired = t_start + i * dt 43 | precise_wait(t_end_desired) 44 | t_end = time.monotonic() 45 | new_errors.append(t_end - t_end_desired) 46 | 47 | from matplotlib import pyplot as plt 48 | plt.plot(errors, label='time.sleep') 49 | plt.plot(new_errors, label='sleep/spin hybrid') 50 | plt.ylim((-tol,+tol)) 51 | plt.title('0.1 sec sleep error') 52 | plt.legend() 53 | 54 | 55 | if __name__ == '__main__': 56 | test_sleep() 57 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h: -------------------------------------------------------------------------------- 1 | namespace Eigen { 2 | 3 | namespace internal { 4 | 5 | template 6 | void rwupdt( 7 | Matrix< Scalar, Dynamic, Dynamic > &r, 8 | const Matrix< Scalar, Dynamic, 1> &w, 9 | Matrix< Scalar, Dynamic, 1> &b, 10 | Scalar alpha) 11 | { 12 | typedef DenseIndex Index; 13 | 14 | const Index n = r.cols(); 15 | eigen_assert(r.rows()>=n); 16 | std::vector > givens(n); 17 | 18 | /* Local variables */ 19 | Scalar temp, rowj; 20 | 21 | /* Function Body */ 22 | for (Index j = 0; j < n; ++j) { 23 | rowj = w[j]; 24 | 25 | /* apply the previous transformations to */ 26 | /* r(i,j), i=0,1,...,j-1, and to w(j). */ 27 | for (Index i = 0; i < j; ++i) { 28 | temp = givens[i].c() * r(i,j) + givens[i].s() * rowj; 29 | rowj = -givens[i].s() * r(i,j) + givens[i].c() * rowj; 30 | r(i,j) = temp; 31 | } 32 | 33 | /* determine a givens rotation which eliminates w(j). */ 34 | givens[j].makeGivens(-r(j,j), rowj); 35 | 36 | if (rowj == 0.) 37 | continue; // givens[j] is identity 38 | 39 | /* apply the current transformation to r(j,j), b(j), and alpha. */ 40 | r(j,j) = givens[j].c() * r(j,j) + givens[j].s() * rowj; 41 | temp = givens[j].c() * b[j] + givens[j].s() * alpha; 42 | alpha = -givens[j].s() * b[j] + givens[j].c() * alpha; 43 | b[j] = temp; 44 | } 45 | } 46 | 47 | } // end namespace internal 48 | 49 | } // end namespace Eigen 50 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/task/real_peel_image_mctac_emb_at_ablation_absolute_24fps.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - real_peel_two_realsense_one_gelsight_one_mctac_24fps@_here_ 3 | - _self_ 4 | 5 | name: real_peel_image_mctac_emb_at_ablation_absolute_24fps 6 | 7 | image_shape: [3, 240, 320] 8 | dataset_path: data/peel_v1_zarr 9 | 10 | shape_meta: &shape_meta 11 | # acceptable types: rgb, low_dim 12 | obs: 13 | left_robot_tcp_pose: 14 | shape: [3] 15 | type: low_dim 16 | left_robot_gripper_width: 17 | shape: [1] 18 | type: low_dim 19 | left_gripper2_marker_offset_emb: 20 | shape: [ 15 ] # the raw dimension is (63*2), we use PCA to reduce the dimension to 15 21 | type: low_dim 22 | extended_obs: # for the AT RNN decoder of the latent diffusion policy 23 | left_gripper2_marker_offset_emb: 24 | shape: [ 15 ] # the raw dimension is (63*2), we use PCA to reduce the dimension to 15 25 | type: low_dim 26 | action: 27 | shape: [4] # (3+6+1) (x, y, z, 6d rotation, gripper_width) 28 | 29 | dataset: 30 | _target_: reactive_diffusion_policy.dataset.real_image_tactile_dataset.RealImageTactileDataset 31 | shape_meta: *shape_meta 32 | dataset_path: ${task.dataset_path} 33 | horizon: ${horizon} 34 | pad_before: ${eval:'${dataset_obs_steps}-1+${n_latency_steps}'} 35 | pad_after: ${eval:'${n_action_steps}-1'} 36 | n_obs_steps: ${dataset_obs_steps} 37 | obs_temporal_downsample_ratio: ${dataset_obs_temporal_downsample_ratio} 38 | n_latency_steps: ${n_latency_steps} 39 | seed: 42 40 | val_ratio: 0.00 41 | max_train_episodes: null 42 | delta_action: False 43 | relative_action: False 44 | 45 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/include/flexiv/Log.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Log.hpp 3 | * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. 4 | */ 5 | 6 | #ifndef FLEXIVRDK_LOG_HPP_ 7 | #define FLEXIVRDK_LOG_HPP_ 8 | 9 | #include 10 | 11 | namespace flexiv { 12 | 13 | /** 14 | * @class Log 15 | * @brief Helper functions to print messages with timestamp and coloring. 16 | * Logging raw data to csv file coming soon. 17 | */ 18 | class Log 19 | { 20 | public: 21 | Log() = default; 22 | virtual ~Log() = default; 23 | 24 | /** 25 | * @brief [Non-blocking] Print info message with timestamp and coloring. 26 | * @param[in] message Info message. 27 | * @note Color = green font. 28 | */ 29 | void info(const std::string& message) const; 30 | 31 | /** 32 | * @brief [Non-blocking] Print warning message with timestamp and coloring. 33 | * @param[in] message Warning message. 34 | * @note Color = yellow font. 35 | */ 36 | void warn(const std::string& message) const; 37 | 38 | /** 39 | * @brief [Non-blocking] Print error message with timestamp and coloring. 40 | * @param[in] message Error message. 41 | * @note Color = red font. 42 | */ 43 | void error(const std::string& message) const; 44 | 45 | /** 46 | * @brief [Non-blocking] Print critical error message with timestamp and 47 | * coloring. 48 | * @param[in] message Critical error message. 49 | * @note Color = white font with red background. 50 | */ 51 | void critical(const std::string& message) const; 52 | }; 53 | 54 | } /* namespace flexiv */ 55 | 56 | #endif /* FLEXIVRDK_LOG_HPP_ */ 57 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/robot/charmander.yaml: -------------------------------------------------------------------------------- 1 | # Deoxys adopts a distinct approach for intializing both the PC and the NUC. 2 | # We adopt the official example configurations for the robot here. 3 | 4 | PC: 5 | NAME: "control_pc" 6 | IP: 192.168.110.111 # the ip_address of PC in the PC-NUC network 7 | 8 | NUC: 9 | NAME: "franka_nuc" 10 | IP: 192.168.110.1 # the ip_address of NUC in the PC-NUC network 11 | PUB_PORT: 5556 # the port for NUC to publish robot state to PC 12 | SUB_PORT: 5555 # the port for NUC to subscribe to control commands from PC 13 | GRIPPER_PUB_PORT: 5558 # the port for NUC to publish gripper state to PC 14 | GRIPPER_SUB_PORT: 5557 # the port for NUC to subscribe to gripper commands from PC 15 | 16 | ROBOT: 17 | IP: 172.16.0.2 # the ip_address of Franka robot in the NUC-Robot network 18 | 19 | CONTROL: 20 | STATE_PUBLISHER_RATE: 100 # the rate at which state is published 21 | POLICY_RATE: 20 # the rate at which policy is updated (overridden by control_freq in Python code) 22 | TRAJ_RATE: 300 # the rate at which trajectory is interpolated (matches your 300Hz) 23 | ZMQ_NOBLOCK: true 24 | 25 | ARM_LOGGER: 26 | CONSOLE: 27 | LOGGER_NAME: "arm_logger" 28 | LEVEL: "info" 29 | USE: true 30 | FILE: 31 | LOGGER_NAME: "logs/deoxys_arm.log" 32 | LEVEL: "debug" 33 | USE: false 34 | 35 | GRIPPER_LOGGER: 36 | CONSOLE: 37 | LOGGER_NAME: "gripper_logger" 38 | LEVEL: "info" 39 | USE: true 40 | FILE: 41 | LOGGER_NAME: "logs/deoxys_gripper.log" 42 | LEVEL: "debug" 43 | USE: false -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/LU: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_LU_MODULE_H 9 | #define EIGEN_LU_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup LU_Module LU module 16 | * This module includes %LU decomposition and related notions such as matrix inversion and determinant. 17 | * This module defines the following MatrixBase methods: 18 | * - MatrixBase::inverse() 19 | * - MatrixBase::determinant() 20 | * 21 | * \code 22 | * #include 23 | * \endcode 24 | */ 25 | 26 | #include "src/misc/Kernel.h" 27 | #include "src/misc/Image.h" 28 | #include "src/LU/FullPivLU.h" 29 | #include "src/LU/PartialPivLU.h" 30 | #ifdef EIGEN_USE_LAPACKE 31 | #ifdef EIGEN_USE_MKL 32 | #include "mkl_lapacke.h" 33 | #else 34 | #include "src/misc/lapacke.h" 35 | #endif 36 | #include "src/LU/PartialPivLU_LAPACKE.h" 37 | #endif 38 | #include "src/LU/Determinant.h" 39 | #include "src/LU/InverseImpl.h" 40 | 41 | // Use the SSE optimized version whenever possible. At the moment the 42 | // SSE version doesn't compile when AVX is enabled 43 | #if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX 44 | #include "src/LU/arch/Inverse_SSE.h" 45 | #endif 46 | 47 | #include "src/Core/util/ReenableStupidWarnings.h" 48 | 49 | #endif // EIGEN_LU_MODULE_H 50 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 51 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/common/pcd_utils.py: -------------------------------------------------------------------------------- 1 | import open3d as o3d 2 | import numpy as np 3 | 4 | def random_sample_points(points: np.ndarray, random_sample_num_points: int): 5 | if points.shape[0] > random_sample_num_points: 6 | all_idxs = np.arange(points.shape[0]) 7 | rs = np.random.RandomState() 8 | selected_idxs = rs.choice(all_idxs, size=random_sample_num_points, replace=False) 9 | sel_points = points[selected_idxs, :] 10 | else: 11 | sel_points = points 12 | return sel_points 13 | 14 | def random_sample_pcd(pcd: o3d.geometry.PointCloud, 15 | random_sample_num_points: int, 16 | return_pcd: bool = True): 17 | ''' 18 | pcd: an o3d color pointcloud object 19 | random_sample_num_points: number of points to be randomly sampled 20 | return_pcd: if True, return the pcd object; if False, return points and colors 21 | 22 | ''' 23 | # random select fixed number of points 24 | pts_xyz = np.asarray(pcd.points) 25 | pts_rgb = np.asarray(pcd.colors) 26 | if pts_xyz.shape[0] > random_sample_num_points: 27 | all_idxs = np.arange(pts_xyz.shape[0]) 28 | rs = np.random.RandomState() 29 | selected_idxs = rs.choice(all_idxs, size=random_sample_num_points, replace=False) 30 | pc_xyz_slim = pts_xyz[selected_idxs, :] 31 | pc_rgb_slim = pts_rgb[selected_idxs, :] 32 | if return_pcd: 33 | pcd.points = o3d.utility.Vector3dVector(pc_xyz_slim) 34 | pcd.colors = o3d.utility.Vector3dVector(pc_rgb_slim) 35 | else: 36 | return pc_xyz_slim, pc_rgb_slim 37 | 38 | if return_pcd: 39 | return pcd 40 | else: 41 | return pts_xyz, pts_rgb -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/task/real_lift_image_wrench_at_absolute_24fps.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - real_lift_three_realsense_one_mctac_one_gelsight_24fps@_here_ 3 | - _self_ 4 | 5 | name: real_lift_image_wrench_at_absolute_24fps 6 | 7 | image_shape: [3, 240, 320] 8 | dataset_path: data/lift_v1_zarr 9 | 10 | shape_meta: &shape_meta 11 | # acceptable types: rgb, low_dim 12 | obs: 13 | left_robot_tcp_pose: 14 | shape: [3] 15 | type: low_dim 16 | left_robot_gripper_width: 17 | shape: [1] 18 | type: low_dim 19 | right_robot_tcp_pose: 20 | shape: [3] 21 | type: low_dim 22 | right_robot_gripper_width: 23 | shape: [1] 24 | type: low_dim 25 | left_robot_tcp_wrench: 26 | shape: [ 6 ] 27 | type: low_dim 28 | right_robot_tcp_wrench: 29 | shape: [ 6 ] 30 | type: low_dim 31 | extended_obs: # for the AT RNN decoder of the latent diffusion policy 32 | left_robot_tcp_wrench: 33 | shape: [ 6 ] 34 | type: low_dim 35 | right_robot_tcp_wrench: 36 | shape: [ 6 ] 37 | type: low_dim 38 | action: 39 | shape: [8] # (4 + 4) (x_l, y_l, z_l, x_r, y_r, z_r, gripper_width_l, gripper_width_r) 40 | 41 | dataset: 42 | _target_: reactive_diffusion_policy.dataset.real_image_tactile_dataset.RealImageTactileDataset 43 | shape_meta: *shape_meta 44 | dataset_path: ${task.dataset_path} 45 | horizon: ${horizon} 46 | pad_before: ${eval:'${dataset_obs_steps}-1+${n_latency_steps}'} 47 | pad_after: ${eval:'${n_action_steps}-1'} 48 | n_obs_steps: ${dataset_obs_steps} 49 | obs_temporal_downsample_ratio: ${dataset_obs_temporal_downsample_ratio} 50 | n_latency_steps: ${n_latency_steps} 51 | seed: 42 52 | val_ratio: 0.00 53 | max_train_episodes: null 54 | delta_action: False 55 | relative_action: False 56 | transform_params: ${task.transforms} 57 | 58 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/SVD: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SVD_MODULE_H 9 | #define EIGEN_SVD_MODULE_H 10 | 11 | #include "QR" 12 | #include "Householder" 13 | #include "Jacobi" 14 | 15 | #include "src/Core/util/DisableStupidWarnings.h" 16 | 17 | /** \defgroup SVD_Module SVD module 18 | * 19 | * 20 | * 21 | * This module provides SVD decomposition for matrices (both real and complex). 22 | * Two decomposition algorithms are provided: 23 | * - JacobiSVD implementing two-sided Jacobi iterations is numerically very accurate, fast for small matrices, but very slow for larger ones. 24 | * - BDCSVD implementing a recursive divide & conquer strategy on top of an upper-bidiagonalization which remains fast for large problems. 25 | * These decompositions are accessible via the respective classes and following MatrixBase methods: 26 | * - MatrixBase::jacobiSvd() 27 | * - MatrixBase::bdcSvd() 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | */ 33 | 34 | #include "src/misc/RealSvd2x2.h" 35 | #include "src/SVD/UpperBidiagonalization.h" 36 | #include "src/SVD/SVDBase.h" 37 | #include "src/SVD/JacobiSVD.h" 38 | #include "src/SVD/BDCSVD.h" 39 | #if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT) 40 | #ifdef EIGEN_USE_MKL 41 | #include "mkl_lapacke.h" 42 | #else 43 | #include "src/misc/lapacke.h" 44 | #endif 45 | #include "src/SVD/JacobiSVD_LAPACKE.h" 46 | #endif 47 | 48 | #include "src/Core/util/ReenableStupidWarnings.h" 49 | 50 | #endif // EIGEN_SVD_MODULE_H 51 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 52 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/SpecialFunctions: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPECIALFUNCTIONS_MODULE 11 | #define EIGEN_SPECIALFUNCTIONS_MODULE 12 | 13 | #include 14 | 15 | #include "../../Eigen/Core" 16 | 17 | #include "../../Eigen/src/Core/util/DisableStupidWarnings.h" 18 | 19 | namespace Eigen { 20 | 21 | /** 22 | * \defgroup SpecialFunctions_Module Special math functions module 23 | * 24 | * This module features additional coefficient-wise math functions available 25 | * within the numext:: namespace for the scalar version, and as method and/or free 26 | * functions of Array. Those include: 27 | * 28 | * - erf 29 | * - erfc 30 | * - lgamma 31 | * - igamma 32 | * - igammac 33 | * - digamma 34 | * - polygamma 35 | * - zeta 36 | * - betainc 37 | * 38 | * \code 39 | * #include 40 | * \endcode 41 | */ 42 | //@{ 43 | 44 | } 45 | 46 | #include "src/SpecialFunctions/SpecialFunctionsImpl.h" 47 | #include "src/SpecialFunctions/SpecialFunctionsPacketMath.h" 48 | #include "src/SpecialFunctions/SpecialFunctionsHalf.h" 49 | #include "src/SpecialFunctions/SpecialFunctionsFunctors.h" 50 | #include "src/SpecialFunctions/SpecialFunctionsArrayAPI.h" 51 | 52 | #if defined EIGEN_VECTORIZE_CUDA 53 | #include "src/SpecialFunctions/arch/CUDA/CudaSpecialFunctions.h" 54 | #endif 55 | 56 | namespace Eigen { 57 | //@} 58 | } 59 | 60 | 61 | #include "../../Eigen/src/Core/util/ReenableStupidWarnings.h" 62 | 63 | #endif // EIGEN_SPECIALFUNCTIONS_MODULE 64 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/PaStiXSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_PASTIXSUPPORT_MODULE_H 9 | #define EIGEN_PASTIXSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | #include 18 | } 19 | 20 | #ifdef complex 21 | #undef complex 22 | #endif 23 | 24 | /** \ingroup Support_modules 25 | * \defgroup PaStiXSupport_Module PaStiXSupport module 26 | * 27 | * This module provides an interface to the PaSTiX library. 28 | * PaSTiX is a general \b supernodal, \b parallel and \b opensource sparse solver. 29 | * It provides the two following main factorization classes: 30 | * - class PastixLLT : a supernodal, parallel LLt Cholesky factorization. 31 | * - class PastixLDLT: a supernodal, parallel LDLt Cholesky factorization. 32 | * - class PastixLU : a supernodal, parallel LU factorization (optimized for a symmetric pattern). 33 | * 34 | * \code 35 | * #include 36 | * \endcode 37 | * 38 | * In order to use this module, the PaSTiX headers must be accessible from the include paths, and your binary must be linked to the PaSTiX library and its dependencies. 39 | * The dependencies depend on how PaSTiX has been compiled. 40 | * For a cmake based project, you can use our FindPaSTiX.cmake module to help you in this task. 41 | * 42 | */ 43 | 44 | #include "src/PaStiXSupport/PaStiXSupport.h" 45 | 46 | #include "src/Core/util/ReenableStupidWarnings.h" 47 | 48 | #endif // EIGEN_PASTIXSUPPORT_MODULE_H 49 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorContractionBlocking.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_BLOCKING_H 11 | #define EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_BLOCKING_H 12 | 13 | 14 | namespace Eigen { 15 | namespace internal { 16 | 17 | enum { 18 | ShardByRow = 0, 19 | ShardByCol = 1 20 | }; 21 | 22 | 23 | // Default Blocking Strategy 24 | template 25 | class TensorContractionBlocking { 26 | public: 27 | 28 | typedef typename LhsMapper::Scalar LhsScalar; 29 | typedef typename RhsMapper::Scalar RhsScalar; 30 | 31 | EIGEN_DEVICE_FUNC TensorContractionBlocking(Index k, Index m, Index n, Index num_threads = 1) : 32 | kc_(k), mc_(m), nc_(n) 33 | { 34 | if (ShardingType == ShardByCol) { 35 | computeProductBlockingSizes(kc_, mc_, nc_, num_threads); 36 | } 37 | else { 38 | computeProductBlockingSizes(kc_, nc_, mc_, num_threads); 39 | } 40 | } 41 | 42 | EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Index kc() const { return kc_; } 43 | EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Index mc() const { return mc_; } 44 | EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Index nc() const { return nc_; } 45 | 46 | private: 47 | Index kc_; 48 | Index mc_; 49 | Index nc_; 50 | }; 51 | 52 | 53 | } // end namespace internal 54 | } // end namespace Eigen 55 | 56 | #endif // EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_BLOCKING_H 57 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/SparseLU: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // Copyright (C) 2012 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_SPARSELU_MODULE_H 12 | #define EIGEN_SPARSELU_MODULE_H 13 | 14 | #include "SparseCore" 15 | 16 | /** 17 | * \defgroup SparseLU_Module SparseLU module 18 | * This module defines a supernodal factorization of general sparse matrices. 19 | * The code is fully optimized for supernode-panel updates with specialized kernels. 20 | * Please, see the documentation of the SparseLU class for more details. 21 | */ 22 | 23 | // Ordering interface 24 | #include "OrderingMethods" 25 | 26 | #include "src/SparseLU/SparseLU_gemm_kernel.h" 27 | 28 | #include "src/SparseLU/SparseLU_Structs.h" 29 | #include "src/SparseLU/SparseLU_SupernodalMatrix.h" 30 | #include "src/SparseLU/SparseLUImpl.h" 31 | #include "src/SparseCore/SparseColEtree.h" 32 | #include "src/SparseLU/SparseLU_Memory.h" 33 | #include "src/SparseLU/SparseLU_heap_relax_snode.h" 34 | #include "src/SparseLU/SparseLU_relax_snode.h" 35 | #include "src/SparseLU/SparseLU_pivotL.h" 36 | #include "src/SparseLU/SparseLU_panel_dfs.h" 37 | #include "src/SparseLU/SparseLU_kernel_bmod.h" 38 | #include "src/SparseLU/SparseLU_panel_bmod.h" 39 | #include "src/SparseLU/SparseLU_column_dfs.h" 40 | #include "src/SparseLU/SparseLU_column_bmod.h" 41 | #include "src/SparseLU/SparseLU_copy_to_ucol.h" 42 | #include "src/SparseLU/SparseLU_pruneL.h" 43 | #include "src/SparseLU/SparseLU_Utils.h" 44 | #include "src/SparseLU/SparseLU.h" 45 | 46 | #endif // EIGEN_SPARSELU_MODULE_H 47 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/Core/SelfCwiseBinaryOp.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009-2010 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SELFCWISEBINARYOP_H 11 | #define EIGEN_SELFCWISEBINARYOP_H 12 | 13 | namespace Eigen { 14 | 15 | // TODO generalize the scalar type of 'other' 16 | 17 | template 18 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator*=(const Scalar& other) 19 | { 20 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::mul_assign_op()); 21 | return derived(); 22 | } 23 | 24 | template 25 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator+=(const Scalar& other) 26 | { 27 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::add_assign_op()); 28 | return derived(); 29 | } 30 | 31 | template 32 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator-=(const Scalar& other) 33 | { 34 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::sub_assign_op()); 35 | return derived(); 36 | } 37 | 38 | template 39 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator/=(const Scalar& other) 40 | { 41 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::div_assign_op()); 42 | return derived(); 43 | } 44 | 45 | } // end namespace Eigen 46 | 47 | #endif // EIGEN_SELFCWISEBINARYOP_H 48 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/SparseCore/SparseRedux.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2014 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSEREDUX_H 11 | #define EIGEN_SPARSEREDUX_H 12 | 13 | namespace Eigen { 14 | 15 | template 16 | typename internal::traits::Scalar 17 | SparseMatrixBase::sum() const 18 | { 19 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 20 | Scalar res(0); 21 | internal::evaluator thisEval(derived()); 22 | for (Index j=0; j::InnerIterator iter(thisEval,j); iter; ++iter) 24 | res += iter.value(); 25 | return res; 26 | } 27 | 28 | template 29 | typename internal::traits >::Scalar 30 | SparseMatrix<_Scalar,_Options,_Index>::sum() const 31 | { 32 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 33 | if(this->isCompressed()) 34 | return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); 35 | else 36 | return Base::sum(); 37 | } 38 | 39 | template 40 | typename internal::traits >::Scalar 41 | SparseVector<_Scalar,_Options,_Index>::sum() const 42 | { 43 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 44 | return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); 45 | } 46 | 47 | } // end namespace Eigen 48 | 49 | #endif // EIGEN_SPARSEREDUX_H 50 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/real_world/simple_camera/simple_usb_camera.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | from loguru import logger 3 | import pyudev 4 | 5 | 6 | class SimpleUSBCamera: 7 | def __init__(self, camera_index=0): 8 | self.context = pyudev.Context() 9 | self.camera_index = camera_index 10 | self.cap = None 11 | 12 | def start(self): 13 | if self.cap is None: 14 | self.cap = cv2.VideoCapture(self.camera_index) 15 | if not self.cap.isOpened(): 16 | logger.error("Could not open video device") 17 | raise Exception("Could not open video device") 18 | logger.info("Camera started") 19 | else: 20 | logger.warning("Camera is already running") 21 | 22 | def stop(self): 23 | if self.cap is not None: 24 | self.cap.release() 25 | self.cap = None 26 | logger.info("Camera stopped") 27 | else: 28 | logger.warning("Camera is not running") 29 | 30 | def get_rgb_frame(self): 31 | if self.cap is not None: 32 | ret, frame = self.cap.read() 33 | if not ret: 34 | logger.error("Failed to capture image") 35 | raise Exception("Failed to capture image") 36 | return frame 37 | else: 38 | logger.error("Camera is not running") 39 | raise Exception("Camera is not running") 40 | 41 | 42 | # Example usage: 43 | if __name__ == "__main__": 44 | camera = SimpleUSBCamera(camera_index=8) 45 | 46 | try: 47 | camera.start() 48 | while True: 49 | frame = camera.get_rgb_frame() 50 | # Display the frame 51 | cv2.imshow('RGB Frame', frame) 52 | if cv2.waitKey(1) & 0xFF == ord('q'): 53 | break 54 | 55 | except Exception as e: 56 | logger.exception(e) 57 | finally: 58 | camera.stop() 59 | cv2.destroyAllWindows() -------------------------------------------------------------------------------- /tests/test_replay_buffer.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | 4 | ROOT_DIR = os.path.dirname(os.path.dirname(__file__)) 5 | sys.path.append(ROOT_DIR) 6 | os.chdir(ROOT_DIR) 7 | 8 | import zarr 9 | from reactive_diffusion_policy.common.replay_buffer import ReplayBuffer 10 | 11 | def test(): 12 | import numpy as np 13 | buff = ReplayBuffer.create_empty_numpy() 14 | buff.add_episode({ 15 | 'obs': np.zeros((100,10), dtype=np.float16) 16 | }) 17 | buff.add_episode({ 18 | 'obs': np.ones((50,10)), 19 | 'action': np.ones((50,2)) 20 | }) 21 | # buff.rechunk(256) 22 | obs = buff.get_episode(0) 23 | 24 | import numpy as np 25 | buff = ReplayBuffer.create_empty_zarr() 26 | buff.add_episode({ 27 | 'obs': np.zeros((100,10), dtype=np.float16) 28 | }) 29 | buff.add_episode({ 30 | 'obs': np.ones((50,10)), 31 | 'action': np.ones((50,2)) 32 | }) 33 | obs = buff.get_episode(0) 34 | buff.set_chunks({ 35 | 'obs': (100,10), 36 | 'action': (100,2) 37 | }) 38 | 39 | 40 | def test_real(): 41 | import os 42 | dist_group = zarr.open( 43 | os.path.expanduser('~/dev/diffusion_policy/data/pusht/pusht_cchi_v2.zarr'), 'r') 44 | 45 | buff = ReplayBuffer.create_empty_numpy() 46 | key, group = next(iter(dist_group.items())) 47 | for key, group in dist_group.items(): 48 | buff.add_episode(group) 49 | 50 | # out_path = os.path.expanduser('~/dev/diffusion_policy/data/pusht_cchi2_v2_replay.zarr') 51 | out_path = os.path.expanduser('~/dev/diffusion_policy/data/test.zarr') 52 | out_store = zarr.DirectoryStore(out_path) 53 | buff.save_to_store(out_store) 54 | 55 | buff = ReplayBuffer.copy_from_path(out_path, store=zarr.MemoryStore()) 56 | buff.pop_episode() 57 | 58 | 59 | def test_pop(): 60 | buff = ReplayBuffer.create_from_path( 61 | '/home/chengchi/dev/diffusion_policy/data/pusht_cchi_v3_replay.zarr', 62 | mode='rw') 63 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/Core/arch/Default/Settings.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2010 Gael Guennebaud 5 | // Copyright (C) 2006-2008 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | 12 | /* All the parameters defined in this file can be specialized in the 13 | * architecture specific files, and/or by the user. 14 | * More to come... */ 15 | 16 | #ifndef EIGEN_DEFAULT_SETTINGS_H 17 | #define EIGEN_DEFAULT_SETTINGS_H 18 | 19 | /** Defines the maximal loop size to enable meta unrolling of loops. 20 | * Note that the value here is expressed in Eigen's own notion of "number of FLOPS", 21 | * it does not correspond to the number of iterations or the number of instructions 22 | */ 23 | #ifndef EIGEN_UNROLLING_LIMIT 24 | #define EIGEN_UNROLLING_LIMIT 100 25 | #endif 26 | 27 | /** Defines the threshold between a "small" and a "large" matrix. 28 | * This threshold is mainly used to select the proper product implementation. 29 | */ 30 | #ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 31 | #define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 8 32 | #endif 33 | 34 | /** Defines the maximal width of the blocks used in the triangular product and solver 35 | * for vectors (level 2 blas xTRMV and xTRSV). The default is 8. 36 | */ 37 | #ifndef EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH 38 | #define EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH 8 39 | #endif 40 | 41 | 42 | /** Defines the default number of registers available for that architecture. 43 | * Currently it must be 8 or 16. Other values will fail. 44 | */ 45 | #ifndef EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 46 | #define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 8 47 | #endif 48 | 49 | #endif // EIGEN_DEFAULT_SETTINGS_H 50 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/common/checkpoint_util.py: -------------------------------------------------------------------------------- 1 | from typing import Optional, Dict 2 | import os 3 | 4 | class TopKCheckpointManager: 5 | def __init__(self, 6 | save_dir, 7 | monitor_key: str, 8 | mode='min', 9 | k=1, 10 | format_str='epoch={epoch:03d}-train_loss={train_loss:.3f}.ckpt' 11 | ): 12 | assert mode in ['max', 'min'] 13 | assert k >= 0 14 | 15 | self.save_dir = save_dir 16 | self.monitor_key = monitor_key 17 | self.mode = mode 18 | self.k = k 19 | self.format_str = format_str 20 | self.path_value_map = dict() 21 | 22 | def get_ckpt_path(self, data: Dict[str, float]) -> Optional[str]: 23 | if self.k == 0: 24 | return None 25 | 26 | value = data[self.monitor_key] 27 | ckpt_path = os.path.join( 28 | self.save_dir, self.format_str.format(**data)) 29 | 30 | if len(self.path_value_map) < self.k: 31 | # under-capacity 32 | self.path_value_map[ckpt_path] = value 33 | return ckpt_path 34 | 35 | # at capacity 36 | sorted_map = sorted(self.path_value_map.items(), key=lambda x: x[1]) 37 | min_path, min_value = sorted_map[0] 38 | max_path, max_value = sorted_map[-1] 39 | 40 | delete_path = None 41 | if self.mode == 'max': 42 | if value > min_value: 43 | delete_path = min_path 44 | else: 45 | if value < max_value: 46 | delete_path = max_path 47 | 48 | if delete_path is None: 49 | return None 50 | else: 51 | del self.path_value_map[delete_path] 52 | self.path_value_map[ckpt_path] = value 53 | 54 | if not os.path.exists(self.save_dir): 55 | os.mkdir(self.save_dir) 56 | 57 | if os.path.exists(delete_path): 58 | os.remove(delete_path) 59 | return ckpt_path 60 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/misc/RealSvd2x2.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009-2010 Benoit Jacob 5 | // Copyright (C) 2013-2016 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_REALSVD2X2_H 12 | #define EIGEN_REALSVD2X2_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | 18 | template 19 | void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q, 20 | JacobiRotation *j_left, 21 | JacobiRotation *j_right) 22 | { 23 | using std::sqrt; 24 | using std::abs; 25 | Matrix m; 26 | m << numext::real(matrix.coeff(p,p)), numext::real(matrix.coeff(p,q)), 27 | numext::real(matrix.coeff(q,p)), numext::real(matrix.coeff(q,q)); 28 | JacobiRotation rot1; 29 | RealScalar t = m.coeff(0,0) + m.coeff(1,1); 30 | RealScalar d = m.coeff(1,0) - m.coeff(0,1); 31 | 32 | if(abs(d) < (std::numeric_limits::min)()) 33 | { 34 | rot1.s() = RealScalar(0); 35 | rot1.c() = RealScalar(1); 36 | } 37 | else 38 | { 39 | // If d!=0, then t/d cannot overflow because the magnitude of the 40 | // entries forming d are not too small compared to the ones forming t. 41 | RealScalar u = t / d; 42 | RealScalar tmp = sqrt(RealScalar(1) + numext::abs2(u)); 43 | rot1.s() = RealScalar(1) / tmp; 44 | rot1.c() = u / tmp; 45 | } 46 | m.applyOnTheLeft(0,1,rot1); 47 | j_right->makeJacobi(m,0,1); 48 | *j_left = rot1 * j_right->transpose(); 49 | } 50 | 51 | } // end namespace internal 52 | 53 | } // end namespace Eigen 54 | 55 | #endif // EIGEN_REALSVD2X2_H 56 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/NumericalDiff: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Thomas Capricelli 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_NUMERICALDIFF_MODULE 11 | #define EIGEN_NUMERICALDIFF_MODULE 12 | 13 | #include 14 | 15 | namespace Eigen { 16 | 17 | /** 18 | * \defgroup NumericalDiff_Module Numerical differentiation module 19 | * 20 | * \code 21 | * #include 22 | * \endcode 23 | * 24 | * See http://en.wikipedia.org/wiki/Numerical_differentiation 25 | * 26 | * Warning : this should NOT be confused with automatic differentiation, which 27 | * is a different method and has its own module in Eigen : \ref 28 | * AutoDiff_Module. 29 | * 30 | * Currently only "Forward" and "Central" schemes are implemented. Those 31 | * are basic methods, and there exist some more elaborated way of 32 | * computing such approximates. They are implemented using both 33 | * proprietary and free software, and usually requires linking to an 34 | * external library. It is very easy for you to write a functor 35 | * using such software, and the purpose is quite orthogonal to what we 36 | * want to achieve with Eigen. 37 | * 38 | * This is why we will not provide wrappers for every great numerical 39 | * differentiation software that exist, but should rather stick with those 40 | * basic ones, that still are useful for testing. 41 | * 42 | * Also, the \ref NonLinearOptimization_Module needs this in order to 43 | * provide full features compatibility with the original (c)minpack 44 | * package. 45 | * 46 | */ 47 | } 48 | 49 | //@{ 50 | 51 | #include "src/NumericalDiff/NumericalDiff.h" 52 | 53 | //@} 54 | 55 | 56 | #endif // EIGEN_NUMERICALDIFF_MODULE 57 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/CXX11/ThreadPool: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_CXX11_THREADPOOL_MODULE 11 | #define EIGEN_CXX11_THREADPOOL_MODULE 12 | 13 | #include "../../../Eigen/Core" 14 | 15 | #include 16 | 17 | /** \defgroup CXX11_ThreadPool_Module C++11 ThreadPool Module 18 | * 19 | * This module provides 2 threadpool implementations 20 | * - a simple reference implementation 21 | * - a faster non blocking implementation 22 | * 23 | * This module requires C++11. 24 | * 25 | * \code 26 | * #include 27 | * \endcode 28 | */ 29 | 30 | 31 | // The code depends on CXX11, so only include the module if the 32 | // compiler supports it. 33 | #if __cplusplus > 199711L || EIGEN_COMP_MSVC >= 1900 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include "src/util/CXX11Meta.h" 49 | #include "src/util/MaxSizeVector.h" 50 | 51 | #include "src/ThreadPool/ThreadLocal.h" 52 | #include "src/ThreadPool/ThreadYield.h" 53 | #include "src/ThreadPool/EventCount.h" 54 | #include "src/ThreadPool/RunQueue.h" 55 | #include "src/ThreadPool/ThreadPoolInterface.h" 56 | #include "src/ThreadPool/ThreadEnvironment.h" 57 | #include "src/ThreadPool/SimpleThreadPool.h" 58 | #include "src/ThreadPool/NonBlockingThreadPool.h" 59 | 60 | #endif 61 | 62 | #include 63 | 64 | #endif // EIGEN_CXX11_THREADPOOL_MODULE 65 | 66 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/Eigenvalues: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_EIGENVALUES_MODULE_H 9 | #define EIGEN_EIGENVALUES_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "Cholesky" 16 | #include "Jacobi" 17 | #include "Householder" 18 | #include "LU" 19 | #include "Geometry" 20 | 21 | /** \defgroup Eigenvalues_Module Eigenvalues module 22 | * 23 | * 24 | * 25 | * This module mainly provides various eigenvalue solvers. 26 | * This module also provides some MatrixBase methods, including: 27 | * - MatrixBase::eigenvalues(), 28 | * - MatrixBase::operatorNorm() 29 | * 30 | * \code 31 | * #include 32 | * \endcode 33 | */ 34 | 35 | #include "src/misc/RealSvd2x2.h" 36 | #include "src/Eigenvalues/Tridiagonalization.h" 37 | #include "src/Eigenvalues/RealSchur.h" 38 | #include "src/Eigenvalues/EigenSolver.h" 39 | #include "src/Eigenvalues/SelfAdjointEigenSolver.h" 40 | #include "src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h" 41 | #include "src/Eigenvalues/HessenbergDecomposition.h" 42 | #include "src/Eigenvalues/ComplexSchur.h" 43 | #include "src/Eigenvalues/ComplexEigenSolver.h" 44 | #include "src/Eigenvalues/RealQZ.h" 45 | #include "src/Eigenvalues/GeneralizedEigenSolver.h" 46 | #include "src/Eigenvalues/MatrixBaseEigenvalues.h" 47 | #ifdef EIGEN_USE_LAPACKE 48 | #ifdef EIGEN_USE_MKL 49 | #include "mkl_lapacke.h" 50 | #else 51 | #include "src/misc/lapacke.h" 52 | #endif 53 | #include "src/Eigenvalues/RealSchur_LAPACKE.h" 54 | #include "src/Eigenvalues/ComplexSchur_LAPACKE.h" 55 | #include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h" 56 | #endif 57 | 58 | #include "src/Core/util/ReenableStupidWarnings.h" 59 | 60 | #endif // EIGEN_EIGENVALUES_MODULE_H 61 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 62 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/CholmodSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_CHOLMODSUPPORT_MODULE_H 9 | #define EIGEN_CHOLMODSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | /** \ingroup Support_modules 20 | * \defgroup CholmodSupport_Module CholmodSupport module 21 | * 22 | * This module provides an interface to the Cholmod library which is part of the suitesparse package. 23 | * It provides the two following main factorization classes: 24 | * - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization. 25 | * - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial). 26 | * 27 | * For the sake of completeness, this module also propose the two following classes: 28 | * - class CholmodSimplicialLLT 29 | * - class CholmodSimplicialLDLT 30 | * Note that these classes does not bring any particular advantage compared to the built-in 31 | * SimplicialLLT and SimplicialLDLT factorization classes. 32 | * 33 | * \code 34 | * #include 35 | * \endcode 36 | * 37 | * In order to use this module, the cholmod headers must be accessible from the include paths, and your binary must be linked to the cholmod library and its dependencies. 38 | * The dependencies depend on how cholmod has been compiled. 39 | * For a cmake based project, you can use our FindCholmod.cmake module to help you in this task. 40 | * 41 | */ 42 | 43 | #include "src/CholmodSupport/CholmodSupport.h" 44 | 45 | #include "src/Core/util/ReenableStupidWarnings.h" 46 | 47 | #endif // EIGEN_CHOLMODSUPPORT_MODULE_H 48 | 49 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/Core/arch/Default/ConjHelper.h: -------------------------------------------------------------------------------- 1 | 2 | // This file is part of Eigen, a lightweight C++ template library 3 | // for linear algebra. 4 | // 5 | // Copyright (C) 2017 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_ARCH_CONJ_HELPER_H 12 | #define EIGEN_ARCH_CONJ_HELPER_H 13 | 14 | #define EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(PACKET_CPLX, PACKET_REAL) \ 15 | template<> struct conj_helper { \ 16 | EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_REAL& x, const PACKET_CPLX& y, const PACKET_CPLX& c) const \ 17 | { return padd(c, pmul(x,y)); } \ 18 | EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_REAL& x, const PACKET_CPLX& y) const \ 19 | { return PACKET_CPLX(Eigen::internal::pmul(x, y.v)); } \ 20 | }; \ 21 | \ 22 | template<> struct conj_helper { \ 23 | EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_CPLX& x, const PACKET_REAL& y, const PACKET_CPLX& c) const \ 24 | { return padd(c, pmul(x,y)); } \ 25 | EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_CPLX& x, const PACKET_REAL& y) const \ 26 | { return PACKET_CPLX(Eigen::internal::pmul(x.v, y)); } \ 27 | }; 28 | 29 | #endif // EIGEN_ARCH_CONJ_HELPER_H 30 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/Core/arch/SSE/TypeCasting.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2015 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_TYPE_CASTING_SSE_H 11 | #define EIGEN_TYPE_CASTING_SSE_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | #ifndef EIGEN_VECTORIZE_AVX 18 | template <> 19 | struct type_casting_traits { 20 | enum { 21 | VectorizedCast = 1, 22 | SrcCoeffRatio = 1, 23 | TgtCoeffRatio = 1 24 | }; 25 | }; 26 | 27 | template <> 28 | struct type_casting_traits { 29 | enum { 30 | VectorizedCast = 1, 31 | SrcCoeffRatio = 1, 32 | TgtCoeffRatio = 1 33 | }; 34 | }; 35 | 36 | template <> 37 | struct type_casting_traits { 38 | enum { 39 | VectorizedCast = 1, 40 | SrcCoeffRatio = 2, 41 | TgtCoeffRatio = 1 42 | }; 43 | }; 44 | 45 | template <> 46 | struct type_casting_traits { 47 | enum { 48 | VectorizedCast = 1, 49 | SrcCoeffRatio = 1, 50 | TgtCoeffRatio = 2 51 | }; 52 | }; 53 | #endif 54 | 55 | template<> EIGEN_STRONG_INLINE Packet4i pcast(const Packet4f& a) { 56 | return _mm_cvttps_epi32(a); 57 | } 58 | 59 | template<> EIGEN_STRONG_INLINE Packet4f pcast(const Packet4i& a) { 60 | return _mm_cvtepi32_ps(a); 61 | } 62 | 63 | template<> EIGEN_STRONG_INLINE Packet4f pcast(const Packet2d& a, const Packet2d& b) { 64 | return _mm_shuffle_ps(_mm_cvtpd_ps(a), _mm_cvtpd_ps(b), (1 << 2) | (1 << 6)); 65 | } 66 | 67 | template<> EIGEN_STRONG_INLINE Packet2d pcast(const Packet4f& a) { 68 | // Simply discard the second half of the input 69 | return _mm_cvtps_pd(a); 70 | } 71 | 72 | 73 | } // end namespace internal 74 | 75 | } // end namespace Eigen 76 | 77 | #endif // EIGEN_TYPE_CASTING_SSE_H 78 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/model/common/lr_scheduler.py: -------------------------------------------------------------------------------- 1 | from diffusers.optimization import ( 2 | Union, SchedulerType, Optional, 3 | Optimizer, TYPE_TO_SCHEDULER_FUNCTION 4 | ) 5 | 6 | def get_scheduler( 7 | name: Union[str, SchedulerType], 8 | optimizer: Optimizer, 9 | num_warmup_steps: Optional[int] = None, 10 | num_training_steps: Optional[int] = None, 11 | **kwargs 12 | ): 13 | """ 14 | Added kwargs vs diffuser's original implementation 15 | 16 | Unified API to get any scheduler from its name. 17 | 18 | Args: 19 | name (`str` or `SchedulerType`): 20 | The name of the scheduler to use. 21 | optimizer (`torch.optim.Optimizer`): 22 | The optimizer that will be used during training. 23 | num_warmup_steps (`int`, *optional*): 24 | The number of warmup steps to do. This is not required by all schedulers (hence the argument being 25 | optional), the function will raise an error if it's unset and the scheduler type requires it. 26 | num_training_steps (`int``, *optional*): 27 | The number of training steps to do. This is not required by all schedulers (hence the argument being 28 | optional), the function will raise an error if it's unset and the scheduler type requires it. 29 | """ 30 | name = SchedulerType(name) 31 | schedule_func = TYPE_TO_SCHEDULER_FUNCTION[name] 32 | if name == SchedulerType.CONSTANT: 33 | return schedule_func(optimizer, **kwargs) 34 | 35 | # All other schedulers require `num_warmup_steps` 36 | if num_warmup_steps is None: 37 | raise ValueError(f"{name} requires `num_warmup_steps`, please provide that argument.") 38 | 39 | if name == SchedulerType.CONSTANT_WITH_WARMUP: 40 | return schedule_func(optimizer, num_warmup_steps=num_warmup_steps, **kwargs) 41 | 42 | # All other schedulers require `num_training_steps` 43 | if num_training_steps is None: 44 | raise ValueError(f"{name} requires `num_training_steps`, please provide that argument.") 45 | 46 | return schedule_func(optimizer, num_warmup_steps=num_warmup_steps, num_training_steps=num_training_steps, **kwargs) 47 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/task/real_lift_image_tactile_emb_at_absolute_24fps.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - real_lift_three_realsense_one_mctac_one_gelsight_24fps@_here_ 3 | - _self_ 4 | 5 | name: real_lift_image_tactile_emb_at_absolute_24fps 6 | 7 | image_shape: [3, 240, 320] 8 | dataset_path: data/lift_v1_zarr 9 | 10 | shape_meta: &shape_meta 11 | # acceptable types: rgb, low_dim 12 | obs: 13 | left_robot_tcp_pose: 14 | shape: [3] 15 | type: low_dim 16 | left_robot_gripper_width: 17 | shape: [1] 18 | type: low_dim 19 | right_robot_tcp_pose: 20 | shape: [3] 21 | type: low_dim 22 | right_robot_gripper_width: 23 | shape: [1] 24 | type: low_dim 25 | left_gripper2_marker_offset_emb: 26 | shape: [ 15 ] # the raw dimension is (63*2), we use PCA to reduce the dimension to 15 27 | type: low_dim 28 | right_gripper1_marker_offset_emb: 29 | shape: [15] # the raw dimension is (63*2), we use PCA to reduce the dimension to 15 30 | type: low_dim 31 | extended_obs: # for the AT RNN decoder of the latent diffusion policy 32 | left_gripper2_marker_offset_emb: 33 | shape: [15] # the raw dimension is (63*2), we use PCA to reduce the dimension to 15 34 | type: low_dim 35 | right_gripper1_marker_offset_emb: 36 | shape: [15] # the raw dimension is (63*2), we use PCA to reduce the dimension to 15 37 | type: low_dim 38 | action: 39 | shape: [8] # (4 + 4) (x_l, y_l, z_l, x_r, y_r, z_r, gripper_width_l, gripper_width_r) 40 | 41 | dataset: 42 | _target_: reactive_diffusion_policy.dataset.real_image_tactile_dataset.RealImageTactileDataset 43 | shape_meta: *shape_meta 44 | dataset_path: ${task.dataset_path} 45 | horizon: ${horizon} 46 | pad_before: ${eval:'${dataset_obs_steps}-1+${n_latency_steps}'} 47 | pad_after: ${eval:'${n_action_steps}-1'} 48 | n_obs_steps: ${dataset_obs_steps} 49 | obs_temporal_downsample_ratio: ${dataset_obs_temporal_downsample_ratio} 50 | n_latency_steps: ${n_latency_steps} 51 | seed: 42 52 | val_ratio: 0.00 53 | max_train_episodes: null 54 | delta_action: False 55 | relative_action: False 56 | transform_params: ${task.transforms} 57 | 58 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Proto 3 | BasedOnStyle: Google 4 | --- 5 | Language: Cpp 6 | AccessModifierOffset: -4 7 | AlignAfterOpenBracket: false 8 | AlignConsecutiveAssignments: false 9 | AlignConsecutiveDeclarations: false 10 | AlignEscapedNewlinesLeft: false 11 | AlignOperands: true 12 | AlignTrailingComments: true 13 | AllowAllParametersOfDeclarationOnNextLine: false 14 | AllowShortBlocksOnASingleLine: false 15 | AllowShortCaseLabelsOnASingleLine: false 16 | AllowShortFunctionsOnASingleLine: Inline 17 | AllowShortIfStatementsOnASingleLine: false 18 | AllowShortLoopsOnASingleLine: false 19 | AlwaysBreakAfterDefinitionReturnType: None 20 | AlwaysBreakAfterReturnType: None 21 | AlwaysBreakBeforeMultilineStrings: true 22 | AlwaysBreakTemplateDeclarations: true 23 | BasedOnStyle: WebKit 24 | BinPackArguments: true 25 | BinPackParameters: true 26 | BreakBeforeBraces: Mozilla 27 | BreakBeforeTernaryOperators: true 28 | BreakConstructorInitializersBeforeComma: true 29 | ColumnLimit: 100 30 | CommentPragmas: "^ IWYU pragma:" 31 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 32 | ConstructorInitializerIndentWidth: 0 33 | ContinuationIndentWidth: 4 34 | Cpp11BracedListStyle: true 35 | DerivePointerAlignment: false 36 | DisableFormat: false 37 | ExperimentalAutoDetectBinPacking: false 38 | ForEachMacros: [foreach, Q_FOREACH, BOOST_FOREACH] 39 | IndentCaseLabels: true 40 | IndentWidth: 4 41 | IndentWrappedFunctionNames: false 42 | KeepEmptyLinesAtTheStartOfBlocks: true 43 | MacroBlockBegin: "" 44 | MacroBlockEnd: "" 45 | MaxEmptyLinesToKeep: 1 46 | NamespaceIndentation: None 47 | PenaltyBreakBeforeFirstCallParameter: 100 48 | PenaltyBreakComment: 300 49 | PenaltyBreakFirstLessLess: 120 50 | PenaltyBreakString: 1000 51 | PenaltyExcessCharacter: 10000 52 | PointerAlignment: Left 53 | ReflowComments: true 54 | SortIncludes: false 55 | SpaceAfterCStyleCast: false 56 | SpaceBeforeAssignmentOperators: true 57 | SpaceBeforeParens: ControlStatements 58 | SpaceInEmptyParentheses: false 59 | SpacesInContainerLiterals: true 60 | SpacesInCStyleCastParentheses: false 61 | SpacesInParentheses: false 62 | SpacesInSquareBrackets: false 63 | Standard: Cpp11 64 | TabWidth: 4 65 | UseTab: Never 66 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/dataset/real_image_tactile_latent_diffusion_dataset.py: -------------------------------------------------------------------------------- 1 | import einops 2 | import numpy as np 3 | import tqdm 4 | from reactive_diffusion_policy.model.vae.model import VAE 5 | from reactive_diffusion_policy.dataset.real_image_tactile_dataset import RealImageTactileDataset 6 | from reactive_diffusion_policy.model.common.normalizer import LinearNormalizer, SingleFieldLinearNormalizer 7 | 8 | class RealImageTactileLatentDiffusionDataset(RealImageTactileDataset): 9 | def __init__(self, 10 | at: VAE, 11 | use_latent_action_before_vq: bool, 12 | **kwargs): 13 | super().__init__(**kwargs) 14 | self.at = at 15 | self.at.eval() 16 | self.use_latent_action_before_vq = use_latent_action_before_vq 17 | 18 | def get_normalizer(self, **kwargs) -> LinearNormalizer: 19 | normalizer = super().get_normalizer(**kwargs) 20 | 21 | latent_action_all = [] 22 | 23 | for data in tqdm.tqdm(self, leave=False, desc='Calculating latent action for normalizer'): 24 | action = data['action'].to(self.at.device).unsqueeze(0) 25 | action = normalizer['action'].normalize(action) 26 | latent_action = self.at.encoder( 27 | self.at.preprocess(action / self.at.act_scale) 28 | ) 29 | if self.at.use_vq: 30 | if not self.use_latent_action_before_vq: 31 | latent_action, _, _ = self.at.quant_state_with_vq(latent_action) 32 | else: 33 | latent_action, _ = self.at.quant_state_without_vq(latent_action) 34 | if self.at.use_conv_encoder: 35 | latent_action = einops.rearrange(latent_action, "N (T A) -> N T A", T=self.at.downsampled_input_h) 36 | else: 37 | latent_action = einops.rearrange(latent_action, "N (T A) -> N T A", T=1) 38 | latent_action_all.append(latent_action[0].cpu().detach().numpy()) 39 | 40 | latent_action_all = np.concatenate(latent_action_all, axis=0) 41 | 42 | normalizer['latent_action'] = SingleFieldLinearNormalizer.create_fit(latent_action_all) 43 | 44 | return normalizer 45 | -------------------------------------------------------------------------------- /tests/test_pca_explainality.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import zarr 3 | import cv2 4 | from functools import partial 5 | import copy 6 | 7 | from reactive_diffusion_policy.model.common.pca_embedding import PCAEmbedding 8 | from reactive_diffusion_policy.common.tactile_marker_utils import display_motion 9 | 10 | test_zarr_path = 'data/marker_motion/mctac_v1.zarr' 11 | TRANSFORM_MATRIX_PATH = 'data/PCA_Transform_McTAC_v1/pca_transform_matrix.npy' 12 | MEAN_MATRIX_PATH = 'data/PCA_Transform_McTAC_v1/pca_mean_matrix.npy' 13 | 14 | if __name__ == '__main__': 15 | pca_embedding = PCAEmbedding( 16 | n_components=15, normalize=False, mode='Eval', store=False, 17 | transformation_matrix_path=TRANSFORM_MATRIX_PATH, 18 | mean_matrix_path=MEAN_MATRIX_PATH 19 | ) 20 | zarr_data = zarr.open(test_zarr_path, mode='r') 21 | # Read rgb image and initial marker from zarr path 22 | background_image = zarr_data['color_image'][0][:] 23 | initial_markers = zarr_data['initial_marker'][0][:] 24 | original_shape = initial_markers.shape 25 | h, w = background_image.shape[:2] 26 | 27 | coefficients = np.ones(15) 28 | 29 | def on_trackbar(val, dim): 30 | global coefficients 31 | coefficients[dim] = (val - 100) / 10.0 # the actual range of the coefficients is (-10, 10) 32 | 33 | cv2.namedWindow('Tick Bar') 34 | 35 | for i in range(15): 36 | cv2.createTrackbar(f'Coeff {i+1}', 'Tick Bar', 100, 200, partial(on_trackbar, dim=i)) 37 | 38 | while True: 39 | background_frame = copy.deepcopy(background_image) 40 | background_marker = copy.deepcopy(initial_markers[:, :2] * np.array([w, h])) 41 | 42 | demo_motion = coefficients # (15,) 43 | 44 | demo_motion_reconstructed = (demo_motion @ pca_embedding.W.T + pca_embedding.mean).reshape(( 45 | original_shape[0], -1)) * np.array([w, h]) 46 | 47 | adjusted_marker_img = display_motion(background_frame, background_marker, demo_motion_reconstructed, 0.5, 2) 48 | cv2.imshow('frame with adjusted marker motion' , adjusted_marker_img) 49 | 50 | if cv2.waitKey(1) & 0xFF == ord('q'): 51 | break 52 | 53 | cv2.destroyAllWindows() 54 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/src/NonLinearOptimization/chkder.h: -------------------------------------------------------------------------------- 1 | #define chkder_log10e 0.43429448190325182765 2 | #define chkder_factor 100. 3 | 4 | namespace Eigen { 5 | 6 | namespace internal { 7 | 8 | template 9 | void chkder( 10 | const Matrix< Scalar, Dynamic, 1 > &x, 11 | const Matrix< Scalar, Dynamic, 1 > &fvec, 12 | const Matrix< Scalar, Dynamic, Dynamic > &fjac, 13 | Matrix< Scalar, Dynamic, 1 > &xp, 14 | const Matrix< Scalar, Dynamic, 1 > &fvecp, 15 | int mode, 16 | Matrix< Scalar, Dynamic, 1 > &err 17 | ) 18 | { 19 | using std::sqrt; 20 | using std::abs; 21 | using std::log; 22 | 23 | typedef DenseIndex Index; 24 | 25 | const Scalar eps = sqrt(NumTraits::epsilon()); 26 | const Scalar epsf = chkder_factor * NumTraits::epsilon(); 27 | const Scalar epslog = chkder_log10e * log(eps); 28 | Scalar temp; 29 | 30 | const Index m = fvec.size(), n = x.size(); 31 | 32 | if (mode != 2) { 33 | /* mode = 1. */ 34 | xp.resize(n); 35 | for (Index j = 0; j < n; ++j) { 36 | temp = eps * abs(x[j]); 37 | if (temp == 0.) 38 | temp = eps; 39 | xp[j] = x[j] + temp; 40 | } 41 | } 42 | else { 43 | /* mode = 2. */ 44 | err.setZero(m); 45 | for (Index j = 0; j < n; ++j) { 46 | temp = abs(x[j]); 47 | if (temp == 0.) 48 | temp = 1.; 49 | err += temp * fjac.col(j); 50 | } 51 | for (Index i = 0; i < m; ++i) { 52 | temp = 1.; 53 | if (fvec[i] != 0. && fvecp[i] != 0. && abs(fvecp[i] - fvec[i]) >= epsf * abs(fvec[i])) 54 | temp = eps * abs((fvecp[i] - fvec[i]) / eps - err[i]) / (abs(fvec[i]) + abs(fvecp[i])); 55 | err[i] = 1.; 56 | if (temp > NumTraits::epsilon() && temp < eps) 57 | err[i] = (chkder_log10e * log(temp) - epslog) / epslog; 58 | if (temp >= eps) 59 | err[i] = 0.; 60 | } 61 | } 62 | } 63 | 64 | } // end namespace internal 65 | 66 | } // end namespace Eigen 67 | -------------------------------------------------------------------------------- /tests/test_camera_res_and_fps.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | 3 | 4 | def test_camera_capabilities(): 5 | # Create a list of common resolutions to test 6 | test_resolutions = [ 7 | (352, 288), 8 | (640, 480), # VGA 9 | (800, 600), # SVGA 10 | (1280, 720), # HD 11 | (1920, 1080), # Full HD 12 | (2560, 1440), # 2K 13 | (3840, 2160) # 4K 14 | ] 15 | 16 | # Common frame rates to test 17 | test_fps = [15, 25, 30, 60] 18 | 19 | # Initialize the camera (0 is usually the default USB camera) 20 | cap = cv2.VideoCapture(12) 21 | 22 | if not cap.isOpened(): 23 | print("Error: Could not open camera") 24 | return 25 | 26 | print("Camera detected. Testing capabilities...") 27 | print("\nSupported combinations of resolution and frame rate:") 28 | print("-" * 50) 29 | 30 | # Test each resolution 31 | for resolution in test_resolutions: 32 | width, height = resolution 33 | 34 | # Set resolution 35 | cap.set(cv2.CAP_PROP_FRAME_WIDTH, width) 36 | cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height) 37 | 38 | # Get actual resolution (might be different from requested) 39 | actual_width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) 40 | actual_height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) 41 | 42 | # Test each frame rate 43 | for fps in test_fps: 44 | # Set frame rate 45 | cap.set(cv2.CAP_PROP_FPS, fps) 46 | 47 | # Get actual frame rate 48 | actual_fps = cap.get(cv2.CAP_PROP_FPS) 49 | 50 | # Capture a few frames to test if this combination works 51 | success = True 52 | for _ in range(5): 53 | ret, frame = cap.read() 54 | if not ret: 55 | success = False 56 | break 57 | 58 | if success: 59 | print(f"Resolution: {actual_width}x{actual_height}") 60 | print(f"Frame Rate: {actual_fps:.1f} FPS") 61 | print("-" * 50) 62 | 63 | # Release the camera 64 | cap.release() 65 | 66 | print("\nTesting completed!") 67 | 68 | 69 | if __name__ == "__main__": 70 | test_camera_capabilities() 71 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/src/NonLinearOptimization/covar.h: -------------------------------------------------------------------------------- 1 | namespace Eigen { 2 | 3 | namespace internal { 4 | 5 | template 6 | void covar( 7 | Matrix< Scalar, Dynamic, Dynamic > &r, 8 | const VectorXi &ipvt, 9 | Scalar tol = std::sqrt(NumTraits::epsilon()) ) 10 | { 11 | using std::abs; 12 | typedef DenseIndex Index; 13 | 14 | /* Local variables */ 15 | Index i, j, k, l, ii, jj; 16 | bool sing; 17 | Scalar temp; 18 | 19 | /* Function Body */ 20 | const Index n = r.cols(); 21 | const Scalar tolr = tol * abs(r(0,0)); 22 | Matrix< Scalar, Dynamic, 1 > wa(n); 23 | eigen_assert(ipvt.size()==n); 24 | 25 | /* form the inverse of r in the full upper triangle of r. */ 26 | l = -1; 27 | for (k = 0; k < n; ++k) 28 | if (abs(r(k,k)) > tolr) { 29 | r(k,k) = 1. / r(k,k); 30 | for (j = 0; j <= k-1; ++j) { 31 | temp = r(k,k) * r(j,k); 32 | r(j,k) = 0.; 33 | r.col(k).head(j+1) -= r.col(j).head(j+1) * temp; 34 | } 35 | l = k; 36 | } 37 | 38 | /* form the full upper triangle of the inverse of (r transpose)*r */ 39 | /* in the full upper triangle of r. */ 40 | for (k = 0; k <= l; ++k) { 41 | for (j = 0; j <= k-1; ++j) 42 | r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k); 43 | r.col(k).head(k+1) *= r(k,k); 44 | } 45 | 46 | /* form the full lower triangle of the covariance matrix */ 47 | /* in the strict lower triangle of r and in wa. */ 48 | for (j = 0; j < n; ++j) { 49 | jj = ipvt[j]; 50 | sing = j > l; 51 | for (i = 0; i <= j; ++i) { 52 | if (sing) 53 | r(i,j) = 0.; 54 | ii = ipvt[i]; 55 | if (ii > jj) 56 | r(ii,jj) = r(i,j); 57 | if (ii < jj) 58 | r(jj,ii) = r(i,j); 59 | } 60 | wa[jj] = r(j,j); 61 | } 62 | 63 | /* symmetrize the covariance matrix in r. */ 64 | r.topLeftCorner(n,n).template triangularView() = r.topLeftCorner(n,n).transpose(); 65 | r.diagonal() = wa; 66 | } 67 | 68 | } // end namespace internal 69 | 70 | } // end namespace Eigen 71 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/IterativeLinearSolvers: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 9 | #define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 10 | 11 | #include "SparseCore" 12 | #include "OrderingMethods" 13 | 14 | #include "src/Core/util/DisableStupidWarnings.h" 15 | 16 | /** 17 | * \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module 18 | * 19 | * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse. 20 | * Those solvers are accessible via the following classes: 21 | * - ConjugateGradient for selfadjoint (hermitian) matrices, 22 | * - LeastSquaresConjugateGradient for rectangular least-square problems, 23 | * - BiCGSTAB for general square matrices. 24 | * 25 | * These iterative solvers are associated with some preconditioners: 26 | * - IdentityPreconditioner - not really useful 27 | * - DiagonalPreconditioner - also called Jacobi preconditioner, work very well on diagonal dominant matrices. 28 | * - IncompleteLUT - incomplete LU factorization with dual thresholding 29 | * 30 | * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport. 31 | * 32 | \code 33 | #include 34 | \endcode 35 | */ 36 | 37 | #include "src/IterativeLinearSolvers/SolveWithGuess.h" 38 | #include "src/IterativeLinearSolvers/IterativeSolverBase.h" 39 | #include "src/IterativeLinearSolvers/BasicPreconditioners.h" 40 | #include "src/IterativeLinearSolvers/ConjugateGradient.h" 41 | #include "src/IterativeLinearSolvers/LeastSquareConjugateGradient.h" 42 | #include "src/IterativeLinearSolvers/BiCGSTAB.h" 43 | #include "src/IterativeLinearSolvers/IncompleteLUT.h" 44 | #include "src/IterativeLinearSolvers/IncompleteCholesky.h" 45 | 46 | #include "src/Core/util/ReenableStupidWarnings.h" 47 | 48 | #endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 49 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/real_world/real_inference_util.py: -------------------------------------------------------------------------------- 1 | from typing import Dict, Callable, Tuple 2 | import numpy as np 3 | from reactive_diffusion_policy.common.cv2_util import get_image_transform 4 | 5 | def get_real_obs_dict( 6 | env_obs: Dict[str, np.ndarray], 7 | shape_meta: dict, 8 | is_extended_obs: bool = False 9 | ) -> Dict[str, np.ndarray]: 10 | obs_dict_np = dict() 11 | if is_extended_obs: 12 | obs_shape_meta = shape_meta['extended_obs'] 13 | else: 14 | obs_shape_meta = shape_meta['obs'] 15 | for key, attr in obs_shape_meta.items(): 16 | type = attr.get('type', 'low_dim') 17 | shape = attr.get('shape') 18 | if type == 'rgb': 19 | this_imgs_in = env_obs[key] 20 | t,hi,wi,ci = this_imgs_in.shape 21 | co,ho,wo = shape 22 | assert ci == co 23 | out_imgs = this_imgs_in 24 | if (ho != hi) or (wo != wi) or (this_imgs_in.dtype == np.uint8): 25 | tf = get_image_transform( 26 | input_res=(wi,hi), 27 | output_res=(wo,ho), 28 | bgr_to_rgb=False) 29 | out_imgs = np.stack([tf(x) for x in this_imgs_in]) 30 | if this_imgs_in.dtype == np.uint8: 31 | out_imgs = out_imgs.astype(np.float32) / 255 32 | # THWC to TCHW 33 | obs_dict_np[key] = np.moveaxis(out_imgs,-1,1) 34 | elif type == 'low_dim': 35 | if "wrt" in key: 36 | continue 37 | this_data_in = env_obs[key] 38 | if 'pose' in key and shape == (2,): 39 | # take X,Y coordinates 40 | this_data_in = this_data_in[...,[0,1]] 41 | obs_dict_np[key] = this_data_in 42 | return obs_dict_np 43 | 44 | 45 | def get_real_obs_resolution( 46 | shape_meta: dict 47 | ) -> Tuple[int, int]: 48 | out_res = None 49 | obs_shape_meta = shape_meta['obs'] 50 | for key, attr in obs_shape_meta.items(): 51 | type = attr.get('type', 'low_dim') 52 | shape = attr.get('shape') 53 | if type == 'rgb': 54 | co,ho,wo = shape 55 | if out_res is None: 56 | out_res = (wo, ho) 57 | assert out_res == (wo, ho) 58 | return out_res 59 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/Geometry: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_GEOMETRY_MODULE_H 9 | #define EIGEN_GEOMETRY_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "SVD" 16 | #include "LU" 17 | #include 18 | 19 | /** \defgroup Geometry_Module Geometry module 20 | * 21 | * This module provides support for: 22 | * - fixed-size homogeneous transformations 23 | * - translation, scaling, 2D and 3D rotations 24 | * - \link Quaternion quaternions \endlink 25 | * - cross products (\ref MatrixBase::cross, \ref MatrixBase::cross3) 26 | * - orthognal vector generation (\ref MatrixBase::unitOrthogonal) 27 | * - some linear components: \link ParametrizedLine parametrized-lines \endlink and \link Hyperplane hyperplanes \endlink 28 | * - \link AlignedBox axis aligned bounding boxes \endlink 29 | * - \link umeyama least-square transformation fitting \endlink 30 | * 31 | * \code 32 | * #include 33 | * \endcode 34 | */ 35 | 36 | #include "src/Geometry/OrthoMethods.h" 37 | #include "src/Geometry/EulerAngles.h" 38 | 39 | #include "src/Geometry/Homogeneous.h" 40 | #include "src/Geometry/RotationBase.h" 41 | #include "src/Geometry/Rotation2D.h" 42 | #include "src/Geometry/Quaternion.h" 43 | #include "src/Geometry/AngleAxis.h" 44 | #include "src/Geometry/Transform.h" 45 | #include "src/Geometry/Translation.h" 46 | #include "src/Geometry/Scaling.h" 47 | #include "src/Geometry/Hyperplane.h" 48 | #include "src/Geometry/ParametrizedLine.h" 49 | #include "src/Geometry/AlignedBox.h" 50 | #include "src/Geometry/Umeyama.h" 51 | 52 | // Use the SSE optimized version whenever possible. At the moment the 53 | // SSE version doesn't compile when AVX is enabled 54 | #if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX 55 | #include "src/Geometry/arch/Geometry_SSE.h" 56 | #endif 57 | 58 | #include "src/Core/util/ReenableStupidWarnings.h" 59 | 60 | #endif // EIGEN_GEOMETRY_MODULE_H 61 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 62 | 63 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/SparseLU/SparseLU_Utils.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | 11 | #ifndef EIGEN_SPARSELU_UTILS_H 12 | #define EIGEN_SPARSELU_UTILS_H 13 | 14 | namespace Eigen { 15 | namespace internal { 16 | 17 | /** 18 | * \brief Count Nonzero elements in the factors 19 | */ 20 | template 21 | void SparseLUImpl::countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu) 22 | { 23 | nnzL = 0; 24 | nnzU = (glu.xusub)(n); 25 | Index nsuper = (glu.supno)(n); 26 | Index jlen; 27 | Index i, j, fsupc; 28 | if (n <= 0 ) return; 29 | // For each supernode 30 | for (i = 0; i <= nsuper; i++) 31 | { 32 | fsupc = glu.xsup(i); 33 | jlen = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); 34 | 35 | for (j = fsupc; j < glu.xsup(i+1); j++) 36 | { 37 | nnzL += jlen; 38 | nnzU += j - fsupc + 1; 39 | jlen--; 40 | } 41 | } 42 | } 43 | 44 | /** 45 | * \brief Fix up the data storage lsub for L-subscripts. 46 | * 47 | * It removes the subscripts sets for structural pruning, 48 | * and applies permutation to the remaining subscripts 49 | * 50 | */ 51 | template 52 | void SparseLUImpl::fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu) 53 | { 54 | Index fsupc, i, j, k, jstart; 55 | 56 | StorageIndex nextl = 0; 57 | Index nsuper = (glu.supno)(n); 58 | 59 | // For each supernode 60 | for (i = 0; i <= nsuper; i++) 61 | { 62 | fsupc = glu.xsup(i); 63 | jstart = glu.xlsub(fsupc); 64 | glu.xlsub(fsupc) = nextl; 65 | for (j = jstart; j < glu.xlsub(fsupc + 1); j++) 66 | { 67 | glu.lsub(nextl) = perm_r(glu.lsub(j)); // Now indexed into P*A 68 | nextl++; 69 | } 70 | for (k = fsupc+1; k < glu.xsup(i+1); k++) 71 | glu.xlsub(k) = nextl; // other columns in supernode i 72 | } 73 | 74 | glu.xlsub(n) = nextl; 75 | } 76 | 77 | } // end namespace internal 78 | 79 | } // end namespace Eigen 80 | #endif // EIGEN_SPARSELU_UTILS_H 81 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/example_py/utility.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """utility.py 4 | 5 | Utility methods. 6 | """ 7 | 8 | __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." 9 | __author__ = "Flexiv" 10 | 11 | import math 12 | # pip install scipy 13 | from scipy.spatial.transform import Rotation as R 14 | 15 | 16 | def quat2eulerZYX(quat, degree=False): 17 | """ 18 | Convert quaternion to Euler angles with ZYX axis rotations. 19 | 20 | Parameters 21 | ---------- 22 | quat : float list 23 | Quaternion input in [w,x,y,z] order. 24 | degree : bool 25 | Return values in degrees, otherwise in radians. 26 | 27 | Returns 28 | ---------- 29 | float list 30 | Euler angles in [x,y,z] order, radian by default unless specified otherwise. 31 | """ 32 | 33 | # Convert target quaternion to Euler ZYX using scipy package's 'xyz' extrinsic rotation 34 | # NOTE: scipy uses [x,y,z,w] order to represent quaternion 35 | eulerZYX = R.from_quat([quat[1], quat[2], 36 | quat[3], quat[0]]).as_euler('xyz', degrees=degree).tolist() 37 | 38 | return eulerZYX 39 | 40 | 41 | def list2str(ls): 42 | """ 43 | Convert a list to a string. 44 | 45 | Parameters 46 | ---------- 47 | ls : list 48 | Source list of any size. 49 | 50 | Returns 51 | ---------- 52 | str 53 | A string with format "ls[0] ls[1] ... ls[n] ", i.e. each value 54 | followed by a space, including the last one. 55 | """ 56 | 57 | ret_str = "" 58 | for i in ls: 59 | ret_str += str(i) + " " 60 | return ret_str 61 | 62 | 63 | def parse_pt_states(pt_states, parse_target): 64 | """ 65 | Parse the value of a specified primitive state from the pt_states string list. 66 | 67 | Parameters 68 | ---------- 69 | pt_states : str list 70 | Primitive states string list returned from Robot::getPrimitiveStates(). 71 | parse_target : str 72 | Name of the primitive state to parse for. 73 | 74 | Returns 75 | ---------- 76 | str 77 | Value of the specified primitive state in string format. Empty string is 78 | returned if parse_target does not exist. 79 | """ 80 | for state in pt_states: 81 | # Split the state sentence into words 82 | words = state.split() 83 | 84 | if words[0] == parse_target: 85 | return words[-1] 86 | 87 | return "" 88 | -------------------------------------------------------------------------------- /reactive_diffusion_policy/config/task/real_lift_three_realsense_one_mctac_one_gelsight_24fps.yaml: -------------------------------------------------------------------------------- 1 | defaults: 2 | - real_robot_env@_here_ 3 | - _self_ 4 | 5 | teleop_server: 6 | # gripper params 7 | use_force_control_for_gripper: False 8 | max_gripper_width: 0.07 9 | min_gripper_width: 0.0 10 | grasp_force: 20.0 11 | gripper_control_time_interval: 30 12 | gripper_control_width_precision: 0.01 13 | gripper_never_open: False 14 | # teleoperation mode 15 | teleop_mode: 'dual_arm_3D_translation' 16 | relative_translation_scale: 1.0 # for dexterous teleoperation 17 | 18 | publisher: 19 | # two realsense cameras 20 | realsense_camera_publisher: 21 | - camera_serial_number: '036422060422' 22 | camera_type: 'D400' 23 | camera_name: 'external_camera' 24 | rgb_resolution: [320, 240] 25 | depth_resolution: [320, 240] 26 | random_sample_point_num: 3000 27 | fps: 24 28 | decimate: 2 # (0-4) decimation_filter magnitude for point cloud 29 | cpu_core_id: 30 | - 10 31 | - camera_serial_number: '135122079769' 32 | camera_type: 'D400' 33 | camera_name: 'left_wrist_camera' 34 | rgb_resolution: [320, 240] 35 | depth_resolution: [320, 240] 36 | random_sample_point_num: 3000 37 | fps: 24 38 | decimate: 2 # (0-4) decimation_filter magni；tude for point cloud 39 | cpu_core_id: 40 | - 10 41 | - camera_serial_number: '819612073139' 42 | camera_type: 'D400' 43 | camera_name: 'right_wrist_camera' 44 | rgb_resolution: [320, 240] 45 | depth_resolution: [320, 240] 46 | random_sample_point_num: 3000 47 | fps: 24 48 | decimate: 2 # (0-4) decimation_filter magni；tude for point cloud 49 | cpu_core_id: 50 | - 10 51 | # USB camera (GelSight) 52 | usb_camera_publisher: 53 | - camera_name: 'right_gripper_camera_1' 54 | camera_type: 'gelsight' 55 | fps: 24 56 | vr_server_ip: ${task.publisher.vr_server_ip} 57 | vr_server_port: ${task.publisher.vr_server_port} 58 | dimension: 2 59 | marker_vis_rotation_angle: 90 60 | cpu_core_id: 61 | - 11 62 | - 12 63 | - 13 64 | - camera_name: 'left_gripper_camera_2' 65 | camera_type: 'MCTac' 66 | fps: 24 67 | vr_server_ip: ${task.publisher.vr_server_ip} 68 | vr_server_port: ${task.publisher.vr_server_port} 69 | dimension: 2 70 | image_rotation: 0 71 | marker_vis_rotation_angle: 0 72 | cpu_core_id: 73 | - 15 -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/unsupported/Eigen/src/SpecialFunctions/SpecialFunctionsHalf.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPECIALFUNCTIONS_HALF_H 9 | #define EIGEN_SPECIALFUNCTIONS_HALF_H 10 | 11 | namespace Eigen { 12 | namespace numext { 13 | 14 | #if EIGEN_HAS_C99_MATH 15 | template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half lgamma(const Eigen::half& a) { 16 | return Eigen::half(Eigen::numext::lgamma(static_cast(a))); 17 | } 18 | template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half digamma(const Eigen::half& a) { 19 | return Eigen::half(Eigen::numext::digamma(static_cast(a))); 20 | } 21 | template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half zeta(const Eigen::half& x, const Eigen::half& q) { 22 | return Eigen::half(Eigen::numext::zeta(static_cast(x), static_cast(q))); 23 | } 24 | template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half polygamma(const Eigen::half& n, const Eigen::half& x) { 25 | return Eigen::half(Eigen::numext::polygamma(static_cast(n), static_cast(x))); 26 | } 27 | template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half erf(const Eigen::half& a) { 28 | return Eigen::half(Eigen::numext::erf(static_cast(a))); 29 | } 30 | template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half erfc(const Eigen::half& a) { 31 | return Eigen::half(Eigen::numext::erfc(static_cast(a))); 32 | } 33 | template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half igamma(const Eigen::half& a, const Eigen::half& x) { 34 | return Eigen::half(Eigen::numext::igamma(static_cast(a), static_cast(x))); 35 | } 36 | template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half igammac(const Eigen::half& a, const Eigen::half& x) { 37 | return Eigen::half(Eigen::numext::igammac(static_cast(a), static_cast(x))); 38 | } 39 | template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half betainc(const Eigen::half& a, const Eigen::half& b, const Eigen::half& x) { 40 | return Eigen::half(Eigen::numext::betainc(static_cast(a), static_cast(b), static_cast(x))); 41 | } 42 | #endif 43 | 44 | } // end namespace numext 45 | } // end namespace Eigen 46 | 47 | #endif // EIGEN_SPECIALFUNCTIONS_HALF_H 48 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/src/SparseCore/MappedSparseMatrix.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2014 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_MAPPED_SPARSEMATRIX_H 11 | #define EIGEN_MAPPED_SPARSEMATRIX_H 12 | 13 | namespace Eigen { 14 | 15 | /** \deprecated Use Map > 16 | * \class MappedSparseMatrix 17 | * 18 | * \brief Sparse matrix 19 | * 20 | * \param _Scalar the scalar type, i.e. the type of the coefficients 21 | * 22 | * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme. 23 | * 24 | */ 25 | namespace internal { 26 | template 27 | struct traits > : traits > 28 | {}; 29 | } // end namespace internal 30 | 31 | template 32 | class MappedSparseMatrix 33 | : public Map > 34 | { 35 | typedef Map > Base; 36 | 37 | public: 38 | 39 | typedef typename Base::StorageIndex StorageIndex; 40 | typedef typename Base::Scalar Scalar; 41 | 42 | inline MappedSparseMatrix(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr, Scalar* valuePtr, StorageIndex* innerNonZeroPtr = 0) 43 | : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZeroPtr) 44 | {} 45 | 46 | /** Empty destructor */ 47 | inline ~MappedSparseMatrix() {} 48 | }; 49 | 50 | namespace internal { 51 | 52 | template 53 | struct evaluator > 54 | : evaluator > > 55 | { 56 | typedef MappedSparseMatrix<_Scalar,_Options,_StorageIndex> XprType; 57 | typedef evaluator > Base; 58 | 59 | evaluator() : Base() {} 60 | explicit evaluator(const XprType &mat) : Base(mat) {} 61 | }; 62 | 63 | } 64 | 65 | } // end namespace Eigen 66 | 67 | #endif // EIGEN_MAPPED_SPARSEMATRIX_H 68 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/include/flexiv/Mode.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Mode.hpp 3 | * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. 4 | */ 5 | 6 | #ifndef FLEXIVRDK_MODE_HPP_ 7 | #define FLEXIVRDK_MODE_HPP_ 8 | 9 | namespace flexiv { 10 | 11 | /** 12 | * @enum Mode 13 | * @brief Robot control modes. Must put the robot into the correct control mode 14 | * before sending any corresponding commands. 15 | */ 16 | enum Mode 17 | { 18 | /** Mode not set */ 19 | UNKNOWN = -1, 20 | 21 | /** 22 | * No operation to execute, the robot holds position and waits for new 23 | * command. 24 | */ 25 | IDLE, 26 | 27 | /** 28 | * Run real-time joint torque control to track continuous commands @1kHz. 29 | * @note Real-time (RT) mode 30 | * @see flexiv::Robot::streamJointTorque() 31 | */ 32 | RT_JOINT_TORQUE, 33 | 34 | /** 35 | * Run real-time joint position control to track continuous commands @ 1kHz. 36 | * @note Real-time (RT) mode 37 | * @see flexiv::Robot::streamJointPosition() 38 | */ 39 | RT_JOINT_POSITION, 40 | 41 | /** 42 | * Run non-real-time joint position control to track discrete commands 43 | * (smoothened by internal motion generator). 44 | * @note Non-real-time (NRT) mode 45 | * @see flexiv::Robot::sendJointPosition() 46 | */ 47 | NRT_JOINT_POSITION, 48 | 49 | /** 50 | * Execute pre-configured robot task plans. 51 | * @note Non-real-time (NRT) mode 52 | * @see flexiv::Robot::executePlan() 53 | */ 54 | NRT_PLAN_EXECUTION, 55 | 56 | /** 57 | * Execute robot primitives (unit skills). 58 | * @note Non-real-time (NRT) mode 59 | * @see flexiv::Robot::executePrimitive() 60 | * @see [Flexiv Primitives](https://www.flexiv.com/primitives/) 61 | * documentation 62 | */ 63 | NRT_PRIMITIVE_EXECUTION, 64 | 65 | /** 66 | * Run real-time Cartesian motion-force control to track continuous commands 67 | * @ 1kHz. 68 | * @note Real-time (RT) mode 69 | * @see flexiv::Robot::streamCartesianMotionForce() 70 | */ 71 | RT_CARTESIAN_MOTION_FORCE, 72 | 73 | /** 74 | * Run non-real-time Cartesian motion-force control to track discrete 75 | * commands, smoothened by internal motion generator. 76 | * @note Non-real-time (NRT) mode 77 | * @see flexiv::Robot::sendCartesianMotionForce() 78 | */ 79 | NRT_CARTESIAN_MOTION_FORCE, 80 | }; 81 | 82 | } /* namespace flexiv */ 83 | 84 | #endif /* FLEXIVRDK_MODE_HPP_ */ 85 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/SuperLUSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SUPERLUSUPPORT_MODULE_H 9 | #define EIGEN_SUPERLUSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #ifdef EMPTY 16 | #define EIGEN_EMPTY_WAS_ALREADY_DEFINED 17 | #endif 18 | 19 | typedef int int_t; 20 | #include 21 | #include 22 | #include 23 | 24 | // slu_util.h defines a preprocessor token named EMPTY which is really polluting, 25 | // so we remove it in favor of a SUPERLU_EMPTY token. 26 | // If EMPTY was already defined then we don't undef it. 27 | 28 | #if defined(EIGEN_EMPTY_WAS_ALREADY_DEFINED) 29 | # undef EIGEN_EMPTY_WAS_ALREADY_DEFINED 30 | #elif defined(EMPTY) 31 | # undef EMPTY 32 | #endif 33 | 34 | #define SUPERLU_EMPTY (-1) 35 | 36 | namespace Eigen { struct SluMatrix; } 37 | 38 | /** \ingroup Support_modules 39 | * \defgroup SuperLUSupport_Module SuperLUSupport module 40 | * 41 | * This module provides an interface to the SuperLU library. 42 | * It provides the following factorization class: 43 | * - class SuperLU: a supernodal sequential LU factorization. 44 | * - class SuperILU: a supernodal sequential incomplete LU factorization (to be used as a preconditioner for iterative methods). 45 | * 46 | * \warning This wrapper requires at least versions 4.0 of SuperLU. The 3.x versions are not supported. 47 | * 48 | * \warning When including this module, you have to use SUPERLU_EMPTY instead of EMPTY which is no longer defined because it is too polluting. 49 | * 50 | * \code 51 | * #include 52 | * \endcode 53 | * 54 | * In order to use this module, the superlu headers must be accessible from the include paths, and your binary must be linked to the superlu library and its dependencies. 55 | * The dependencies depend on how superlu has been compiled. 56 | * For a cmake based project, you can use our FindSuperLU.cmake module to help you in this task. 57 | * 58 | */ 59 | 60 | #include "src/SuperLUSupport/SuperLUSupport.h" 61 | 62 | #include "src/Core/util/ReenableStupidWarnings.h" 63 | 64 | #endif // EIGEN_SUPERLUSUPPORT_MODULE_H 65 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/thirdparty/eigen3/Eigen/SparseCore: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSECORE_MODULE_H 9 | #define EIGEN_SPARSECORE_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | /** 22 | * \defgroup SparseCore_Module SparseCore module 23 | * 24 | * This module provides a sparse matrix representation, and basic associated matrix manipulations 25 | * and operations. 26 | * 27 | * See the \ref TutorialSparse "Sparse tutorial" 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | * 33 | * This module depends on: Core. 34 | */ 35 | 36 | #include "src/SparseCore/SparseUtil.h" 37 | #include "src/SparseCore/SparseMatrixBase.h" 38 | #include "src/SparseCore/SparseAssign.h" 39 | #include "src/SparseCore/CompressedStorage.h" 40 | #include "src/SparseCore/AmbiVector.h" 41 | #include "src/SparseCore/SparseCompressedBase.h" 42 | #include "src/SparseCore/SparseMatrix.h" 43 | #include "src/SparseCore/SparseMap.h" 44 | #include "src/SparseCore/MappedSparseMatrix.h" 45 | #include "src/SparseCore/SparseVector.h" 46 | #include "src/SparseCore/SparseRef.h" 47 | #include "src/SparseCore/SparseCwiseUnaryOp.h" 48 | #include "src/SparseCore/SparseCwiseBinaryOp.h" 49 | #include "src/SparseCore/SparseTranspose.h" 50 | #include "src/SparseCore/SparseBlock.h" 51 | #include "src/SparseCore/SparseDot.h" 52 | #include "src/SparseCore/SparseRedux.h" 53 | #include "src/SparseCore/SparseView.h" 54 | #include "src/SparseCore/SparseDiagonalProduct.h" 55 | #include "src/SparseCore/ConservativeSparseSparseProduct.h" 56 | #include "src/SparseCore/SparseSparseProductWithPruning.h" 57 | #include "src/SparseCore/SparseProduct.h" 58 | #include "src/SparseCore/SparseDenseProduct.h" 59 | #include "src/SparseCore/SparseSelfAdjointView.h" 60 | #include "src/SparseCore/SparseTriangularView.h" 61 | #include "src/SparseCore/TriangularSolver.h" 62 | #include "src/SparseCore/SparsePermutation.h" 63 | #include "src/SparseCore/SparseFuzzy.h" 64 | #include "src/SparseCore/SparseSolverBase.h" 65 | 66 | #include "src/Core/util/ReenableStupidWarnings.h" 67 | 68 | #endif // EIGEN_SPARSECORE_MODULE_H 69 | 70 | -------------------------------------------------------------------------------- /third_party/flexiv_rdk-main/example_py/basics2_clear_fault.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """basics2_clear_fault.py 4 | 5 | This tutorial clears minor faults from the robot server if any. Note that critical faults cannot 6 | be cleared, see RDK manual for more details. 7 | """ 8 | 9 | __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." 10 | __author__ = "Flexiv" 11 | 12 | import time 13 | import argparse 14 | 15 | # Import Flexiv RDK Python library 16 | # fmt: off 17 | import sys 18 | sys.path.insert(0, "../lib_py") 19 | import flexivrdk 20 | # fmt: on 21 | 22 | 23 | def print_description(): 24 | """ 25 | Print tutorial description. 26 | 27 | """ 28 | print( 29 | "This tutorial clears minor faults from the robot server if any. Note that " 30 | "critical faults cannot be cleared, see RDK manual for more details." 31 | ) 32 | print() 33 | 34 | 35 | def main(): 36 | # Program Setup 37 | # ============================================================================================== 38 | # Parse arguments 39 | argparser = argparse.ArgumentParser() 40 | argparser.add_argument("robot_ip", help="IP address of the robot server") 41 | argparser.add_argument("local_ip", help="IP address of this PC") 42 | args = argparser.parse_args() 43 | 44 | # Define alias 45 | log = flexivrdk.Log() 46 | 47 | # Print description 48 | log.info("Tutorial description:") 49 | print_description() 50 | 51 | try: 52 | # RDK Initialization 53 | # ========================================================================================== 54 | # Instantiate robot interface 55 | robot = flexivrdk.Robot(args.robot_ip, args.local_ip) 56 | 57 | # Fault Clearing 58 | # ========================================================================================== 59 | # Check if the robot has fault 60 | if robot.isFault(): 61 | log.warn("Fault occurred on robot server, trying to clear ...") 62 | # Try to clear the fault 63 | robot.clearFault() 64 | time.sleep(2) 65 | # Check again 66 | if robot.isFault(): 67 | log.error("Fault cannot be cleared, exiting ...") 68 | return 69 | log.info("Fault on robot server is cleared") 70 | else: 71 | log.info("No fault on robot server") 72 | 73 | except Exception as e: 74 | # Print exception error message 75 | log.error(str(e)) 76 | 77 | 78 | if __name__ == "__main__": 79 | main() 80 | --------------------------------------------------------------------------------