diff --git a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt index e1ddec0c..a9e8ad72 100644 --- a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt +++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt @@ -38,10 +38,6 @@ touch_screen_task { vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 } amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 } twist_filter_alpha: 1.0 - enable_joint_limit_avoidance: true - joint_limit_avoidance_gain: 0.2 - joint_limit_avoidance_margin_ratio: 0.15 - joint_limit_avoidance_max_push: 0.25 r_camera_to_visp { m00: 1.0 m01: 0.0 m02: 0.0 m10: 0.0 m11: 1.0 m12: 0.0 diff --git a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt index b106ad9d..d607f7e4 100644 --- a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt +++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt @@ -38,10 +38,6 @@ touch_screen_task { vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 } amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 } twist_filter_alpha: 1.0 - enable_joint_limit_avoidance: true - joint_limit_avoidance_gain: 0.2 - joint_limit_avoidance_margin_ratio: 0.15 - joint_limit_avoidance_max_push: 0.25 r_camera_to_visp { m00: 1.0 m01: 0.0 m02: 0.0 m10: 0.0 m11: -1.0 m12: 0.0 diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index a33da009..610c8e81 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -48,7 +48,7 @@ add_executable(grpc_arm_client_test target_link_libraries(grpc_arm_client_test PRIVATE cmvr_es::device::canbus - cmvr_es::device::ti5motor + cmvr_es::device::ti5_canopen_motor_driver osqp gtest gtest_main @@ -70,7 +70,7 @@ add_executable(grpc_hlc_client_test target_link_libraries(grpc_hlc_client_test PRIVATE cmvr_es::device::canbus - cmvr_es::device::ti5motor + cmvr_es::device::ti5_canopen_motor_driver osqp gtest gtest_main diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index 0467c56b..28cad7ea 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -334,9 +334,9 @@ grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext*, for (double v : state.velocity) msg->add_velocity(v); for (double v : state.effort) msg->add_effort(v); fillFeedback(response->mutable_header(), true); - CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getJointState): success, id=" << device_id - << ", joints=" << msg->name_size() - << ", positions=" << msg->position_size(); + // CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getJointState): success, id=" << device_id + // << ", joints=" << msg->name_size() + // << ", positions=" << msg->position_size(); return grpc::Status::OK; } catch (const std::exception& e) { fillFeedback(response->mutable_header(), false, e.what()); diff --git a/cmvr-es/task/CMakeLists.txt b/cmvr-es/task/CMakeLists.txt index 9483367e..c8a2960f 100644 --- a/cmvr-es/task/CMakeLists.txt +++ b/cmvr-es/task/CMakeLists.txt @@ -17,22 +17,22 @@ target_link_libraries(task add_library(cmvr_es::task ALIAS task) install(TARGETS task LIBRARY DESTINATION lib) -add_executable(touch_screen_task_test - touch_screen_task/src/touch_screen_task_test.cpp -) - -target_link_libraries(touch_screen_task_test PRIVATE - cmvr_es::task - cmvr_es::device::arm - cmvr_es::device::motor_system - cmvr_es::device::mujoco_motor - cmvr_es::device::mujoco_camera - cmvr_es::mujoco_viewer - cmvr_es::proto - cmvr_es::device_manager - cmvr_es::service - gtest - gtest_main - pthread - glog -) +#add_executable(touch_screen_task_test +# touch_screen_task/src/touch_screen_task_test.cpp +#) +# +#target_link_libraries(touch_screen_task_test PRIVATE +# cmvr_es::task +# cmvr_es::device::arm +# cmvr_es::device::motor_manager +# cmvr_es::device::mujoco_motor_driver +# cmvr_es::device::mujoco_camera +# cmvr_es::mujoco_viewer +# cmvr_es::proto +# cmvr_es::device_manager +# cmvr_es::service +# gtest +# gtest_main +# pthread +# glog +#) diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp index 9c18e191..7640cc33 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp @@ -723,10 +723,6 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& !ibvs.has_amax6() || !hasVec6(ibvs.amax6()) || !ibvs.has_twist_filter_alpha() || - !ibvs.has_enable_joint_limit_avoidance() || - !ibvs.has_joint_limit_avoidance_gain() || - !ibvs.has_joint_limit_avoidance_margin_ratio() || - !ibvs.has_joint_limit_avoidance_max_push() || !ibvs.has_r_camera_to_visp() || !hasMat3(ibvs.r_camera_to_visp()) || !ibvs.has_r_camera_to_urdf() || @@ -884,10 +880,6 @@ bool TouchScreenTask::applyConfig() { ibvs_.setVelocityLimit6(toArray6(cmvr::common::math::toEigenVec6(ibvs.vmax6()))); ibvs_.setAccelerationLimit6(toArray6(cmvr::common::math::toEigenVec6(ibvs.amax6()))); ibvs_.setTwistFilterAlpha(ibvs.twist_filter_alpha()); - ibvs_.setJointLimitAvoidance(ibvs.enable_joint_limit_avoidance(), - ibvs.joint_limit_avoidance_gain(), - ibvs.joint_limit_avoidance_margin_ratio(), - ibvs.joint_limit_avoidance_max_push()); ibvs_.setAlignCameraToVisp(r_camera_to_visp); ibvs_.setAlignCameraToUrdf(r_camera_to_urdf); CMVR_LOG(DEBUG) << "[TouchScreenTask] Apply config id=" << config_.id() diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp index 8e9b5db4..55a45b27 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp @@ -27,8 +27,8 @@ #include "cmvr/config/touch_screen_task_config/touch_screen_task_config.pb.h" #include "devices/arm/robot_arm_factory.h" #include "devices/camera/mujoco_camera/include/mujoco_camera.h" -#include "devices/motor/motor_system/include/motor_system.h" -#include "devices/motor/mujoco_motor/include/mujoco_motor.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "devices/motor/drivers/mujoco/include/mujoco_motor.h" #include "manager/device_manager/include/device_manager.h" #include "manager/task_manager/include/task_manager.h" #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" @@ -471,12 +471,12 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) { arm_entry->set_enable(true); auto& device_manager = cmvr::device::DeviceManager::getInstance(device_manager_config); - auto motor_system = device_manager.getDevice("mujoco_motors"); + auto motor_system = device_manager.getDevice("mujoco_motors"); if (!motor_system) { - CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MotorSystem not found: mujoco_motors"; + CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MotorManager not found: mujoco_motors"; return; } - auto bridge = cmvr::device::MotorSystem::mujocoBridgeFor("mujoco_motors"); + auto bridge = cmvr::device::MotorManager::mujocoBridgeFor("mujoco_motors"); if (!bridge) { CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco bridge not found: mujoco_motors"; return; diff --git a/protos/cmvr/config/touch_screen_algorithm_config.proto b/protos/cmvr/config/touch_screen_algorithm_config.proto index 9a15bc0b..e7e577fe 100644 --- a/protos/cmvr/config/touch_screen_algorithm_config.proto +++ b/protos/cmvr/config/touch_screen_algorithm_config.proto @@ -20,11 +20,6 @@ message TouchScreenIbvsConfig { .cmvr.common.Vec6 amax6 = 6; optional double twist_filter_alpha = 7; - optional bool enable_joint_limit_avoidance = 8; - optional double joint_limit_avoidance_gain = 9; - optional double joint_limit_avoidance_margin_ratio = 10; - optional double joint_limit_avoidance_max_push = 11; - .cmvr.common.Mat3 r_camera_to_visp = 12; .cmvr.common.Mat3 r_camera_to_urdf = 13;