From 181e14c7ef25adf65a3fea4b3e06f33404fd0598 Mon Sep 17 00:00:00 2001 From: lgv Date: Wed, 24 Jun 2026 15:45:54 +0800 Subject: [PATCH] refactor:Refactor all the code --- CMakeLists.txt | 22 +- cmvr-es/CMakeLists.txt | 24 +- cmvr-es/algorithms/CMakeLists.txt | 4 + .../controllers}/CMakeLists.txt | 17 +- .../controllers/arm_control/CMakeLists.txt | 13 + .../include/cartesian_velocity_controller.h | 80 + .../src/cartesian_velocity_controller.cpp | 282 +++ .../ibvs}/include/ibvs_controller.h | 17 +- .../controllers/ibvs}/src/ibvs_controller.cpp | 99 +- .../controllers/pid}/include/pid_controller.h | 0 .../controllers/pid}/src/pid_controller.cpp | 2 +- .../tests}/src/controller_test.cpp | 22 +- .../kinematics}/ik_solver/CMakeLists.txt | 29 +- .../ik_solver/common}/include/ik_solver.h | 3 +- .../ik_solver/common}/include/urdf_parser.h | 0 .../ik_solver/common}/src/ik_solver.cpp | 15 +- .../ik_solver/common}/src/urdf_parser.cpp | 2 +- .../kinematics/ik_solver/ik_solver_factory.h | 44 + .../lawba}/include/joints_limit_analyzer.h | 2 +- .../lawba}/include/lawba_ik_solver.h | 32 +- .../lawba}/include/opt_psi_selector.h | 2 +- .../lawba}/src/joints_limit_analyzer.cpp | 2 +- .../ik_solver/lawba}/src/lawba_ik_solver.cpp | 106 +- .../ik_solver/lawba}/src/opt_psi_selector.cpp | 2 +- .../include/pinocchio_dls_ik_solver.h | 95 + .../pinocchio}/include/pinocchio_ik_base.h | 65 +- .../include/pinocchio_qp_ik_solver.h | 35 +- .../pinocchio/src/pinocchio_dls_ik_solver.cpp | 473 +++++ .../pinocchio/src/pinocchio_ik_base.cpp | 330 ++++ .../pinocchio}/src/pinocchio_qp_ik_solver.cpp | 214 +- .../ik_solver/srs/include/srs_ik_solver.h | 84 + .../ik_solver/srs/src/srs_ik_solver.cpp | 339 ++++ .../ik_solver/tests}/src/ik_test.cpp | 148 +- .../ik_solver/tests}/src/srs_ik_test.cpp | 211 +- .../algorithms/motion_planner/CMakeLists.txt | 2 + .../motion_planner/arm_motion/CMakeLists.txt | 17 + .../cartesian_motion_planner.h | 47 + .../cartesian_motion_planner_factory.h | 78 + .../pinocchio_dls_cartesian_motion_planner.h | 66 + ...pinocchio_dls_cartesian_motion_planner.cpp | 408 ++++ .../pinocchio_qp_cartesian_motion_planner.h | 76 + .../pinocchio_qp_cartesian_motion_planner.cpp | 590 ++++++ .../common/include/twist_limiter_config.h | 51 + .../joint_motion/joint_motion_planner.h | 31 + .../joint_motion_planner_factory.h | 55 + .../include/toppra_joint_motion_planner.h | 36 + .../src/toppra_joint_motion_planner.cpp | 76 + .../motion_planner/base_motion/CMakeLists.txt | 65 + .../include/cartesian_twist_limiter.h | 2 +- .../src/cartesian_twist_limiter.cpp | 0 .../src/cartesian_twist_limiter_test.cpp | 2 +- .../joint_trajectory_planner.h} | 28 +- .../toppra_joint_trajectory_planner.h} | 6 +- .../src/toppra_joint_trajectory_planner.cpp} | 38 +- .../toppra_joint_trajectory_planner_test.cpp} | 18 +- .../motion_profile/s_curve}/include/s_curve.h | 0 .../include/s_curve_position_planner.h | 0 .../include/s_curve_velocity_planner.h | 0 .../motion_profile/s_curve}/src/s_curve.cpp | 4 +- .../s_curve}/src/s_curve_position_planner.cpp | 0 .../s_curve}/src/s_curve_velocity_planner.cpp | 0 .../perception/CMakeLists.txt | 6 +- .../apriltag}/include/apriltag_perception.h | 0 .../include/tag_relative_target_3d.h | 2 +- .../apriltag}/src/apriltag_perception.cpp | 2 +- .../apriltag}/src/tag_relative_target_3d.cpp | 4 +- .../src/tag_relative_target_3d_test.cpp | 4 +- cmvr-es/applications/CMakeLists.txt | 31 - .../applications/include/touch_screen_app.h | 329 ---- cmvr-es/applications/src/touch_screen_app.cpp | 1686 ---------------- .../src/touch_screen_app_test.cpp | 105 - cmvr-es/common/CMakeLists.txt | 31 +- .../{consts/constant.h => base/constants.h} | 0 .../base/include => common/base}/grpc_utils.h | 4 +- cmvr-es/common/base/logging/CMakeLists.txt | 15 + cmvr-es/common/base/logging/logger.cpp | 365 ++++ cmvr-es/common/base/logging/logger.h | 116 ++ .../include => common/base}/ring_buffer.h | 0 cmvr-es/common/config/cabin_robot.xml | 152 -- .../config/camera_config/camera_config.pb.txt | 74 - cmvr-es/common/config/config_files.h | 152 ++ .../dexhand_config/dexhand_config.pb.txt | 36 - .../lawba_ik_solver_config.txt | 0 .../pinocchio_dls_ik_solver_config.pb.txt | 9 - .../pinocchio_qp_ik_solver_config.pb.txt | 10 - .../microphone_config.pb.txt | 8 - .../speaker_config/speaker_config.pb.txt | 4 - .../touch_screen_app_config.pb.txt | 151 -- .../proto_file_io.h} | 42 +- cmvr-es/common/math/cartesian_motion_math.h | 58 + cmvr-es/common/math/joint_limits.h | 101 + cmvr-es/common/math/proto_geometry.h | 115 ++ cmvr-es/common/math/{include => }/qp_solver.h | 8 +- .../math/{include => }/support_functions.h | 10 +- cmvr-es/common/math/transform_math.h | 94 + .../ffmpeg/camera_capture.cpp} | 44 +- .../ffmpeg/camera_capture.h} | 0 .../ffmpeg/realsense_capture.cpp} | 46 +- .../ffmpeg/realsense_capture.h} | 0 .../ffmpeg/video_frame_encoder.cpp} | 26 +- .../ffmpeg/video_frame_encoder.h} | 0 .../ffmpeg/video_writer.cpp} | 30 +- .../ffmpeg/video_writer.h} | 0 cmvr-es/common/types/arm/arm_types.h | 223 +++ .../types/geometry_types.h} | 2 +- .../config_helper/include/config_helper.h | 163 -- .../config_helper/include/config_setting.h | 8 - .../config_helper/src/config_setting.cpp | 90 - .../visualization => vision}/image_display.h | 0 .../image_projection.h} | 0 .../visualization => vision}/matplotlibcpp.h | 0 cmvr-es/config/cmvr_es.pb.txt | 5 + cmvr-es/config/devices/agv/agv.pb.txt | 9 + cmvr-es/config/devices/arm/arm.pb.txt | 124 ++ .../devices/arm/arm_dls_ik_dls_motion.pb.txt | 92 + .../devices/arm/arm_dls_ik_qp_motion.pb.txt | 138 ++ cmvr-es/config/devices/arm/arm_mujoco.pb.txt | 124 ++ .../arm/arm_mujoco_dls_ik_dls_motion.pb.txt | 92 + .../arm/arm_mujoco_dls_ik_qp_motion.pb.txt | 120 ++ .../arm/arm_mujoco_qp_ik_qp_motion.pb.txt | 112 ++ .../devices/arm/arm_qp_ik_qp_motion.pb.txt | 130 ++ cmvr-es/config/devices/arm/aubo_arm.pb.txt | 22 + .../config/devices/biohead/bio_head.pb.txt | 86 + cmvr-es/config/devices/camera/camera.pb.txt | 81 + cmvr-es/config/devices/dexhand/dexhand.pb.txt | 41 + .../devices/microphone/microphone.pb.txt | 11 + .../config/devices/motor/mujoco_motors.pb.txt | 17 + .../config/devices/motor/ti5_motors.pb.txt | 75 + cmvr-es/config/devices/speaker/speaker.pb.txt | 7 + cmvr-es/config/logger/logger.pb.txt | 38 + cmvr-es/config/manager/device_manager.pb.txt | 61 + cmvr-es/config/manager/task_manager.pb.txt | 17 + .../grpc_server_task/grpc_server_task.pb.txt | 6 + .../touch_screen_task.pb.txt | 101 + .../touch_screen_task_mujoco.pb.txt | 101 + cmvr-es/controller/include/touch_controller.h | 114 -- cmvr-es/data_center/CMakeLists.txt | 19 - cmvr-es/data_center/include/data_center.h | 31 - cmvr-es/data_center/include/motors_info.h | 85 - cmvr-es/data_center/src/data_center.cpp | 13 - cmvr-es/data_center/src/motors_info.cpp | 195 -- .../device_manager/include/device_factory.h | 44 - .../device_manager/include/device_manager.h | 77 - .../device_manager/include/system_monitor.h | 45 - cmvr-es/device_manager/src/device_factory.cpp | 255 --- cmvr-es/device_manager/src/device_manager.cpp | 390 ---- cmvr-es/device_manager/src/system_monitor.cpp | 110 -- cmvr-es/devices/CMakeLists.txt | 2 + cmvr-es/devices/abstract_device.h | 28 +- cmvr-es/devices/agv/CMakeLists.txt | 13 + cmvr-es/devices/agv/abstract_agv.h | 25 +- cmvr-es/devices/agv/agv_factory.h | 44 + cmvr-es/devices/agv/my_agv/CMakeLists.txt | 8 + cmvr-es/devices/agv/my_agv/include/my_agv.h | 39 + cmvr-es/devices/agv/my_agv/src/my_agv.cpp | 75 + cmvr-es/devices/arm/CMakeLists.txt | 15 + cmvr-es/devices/arm/aubo_arm/CMakeLists.txt | 29 + .../devices/arm/aubo_arm/include/aubo_arm.h | 112 ++ cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp | 575 ++++++ .../arm/motor_robot_arm/CMakeLists.txt | 35 + .../motor_robot_arm/include/motor_robot_arm.h | 135 ++ .../motor_robot_arm/src/motor_robot_arm.cpp | 770 ++++++++ .../src/motor_robot_arm_mujoco_test.cpp | 457 +++++ cmvr-es/devices/arm/robot_arm.h | 102 + cmvr-es/devices/arm/robot_arm_factory.h | 49 + cmvr-es/devices/battery/abstract_battery.h | 5 +- cmvr-es/devices/biohead/abstract_biohead.h | 5 +- .../biohead/biohead_esp32/CMakeLists.txt | 2 +- .../biohead_esp32/include/biohead_esp32.h | 10 +- .../biohead_esp32/src/biohead_esp32.cpp | 136 +- cmvr-es/devices/camera/CMakeLists.txt | 13 + cmvr-es/devices/camera/abstract_camera.h | 2 +- cmvr-es/devices/camera/camera_factory.h | 80 + .../devices/camera/mechmind/CMakeLists.txt | 2 +- .../camera/mechmind/include/mechmind_camera.h | 12 +- .../camera/mechmind/src/mechmind_camera.cpp | 188 +- .../mujoco_camera/include/mujoco_camera.h | 1 + .../camera/realsense_camera/CMakeLists.txt | 2 +- .../include/realsense_camera.h | 9 +- .../realsense_camera/src/realsense_camera.cpp | 396 ++-- .../src/realsense_camera_test.cpp | 38 +- .../devices/camera/uvc_camera/CMakeLists.txt | 4 +- .../camera/uvc_camera/include/uvc_camera.h | 11 +- .../camera/uvc_camera/src/uvc_camera.cpp | 318 +-- cmvr-es/devices/canbus/abstract_canbus.h | 9 +- .../canbus/can_client/pcan/pcan_client.cc | 57 +- .../canbus/can_client/pcan/pcan_client.h | 10 +- .../can_client/pcan/pcan_client_test.cpp | 10 +- .../socket/socket_can_client_raw.cc | 74 +- .../can_client/socket/socket_can_client_raw.h | 10 +- .../socket/socket_can_client_raw_test.cc | 10 +- .../devices/canbus/can_comm/can_receiver.h | 31 +- .../canbus/can_comm/can_receiver_test.cc | 10 +- cmvr-es/devices/canbus/can_comm/can_sender.h | 45 +- .../canbus/can_comm/can_sender_test.cc | 7 +- .../devices/canbus/can_comm/message_manager.h | 8 +- .../canbus/canopen/nmt_request_protocol.h | 2 +- .../canbus/canopen/nmt_response_protocol.h | 6 +- .../canbus/canopen/sdo_request_protocol.h | 2 +- .../canbus/canopen/sdo_response_protocol.h | 6 +- .../devices/canbus/canopen/sync_protocol.h | 2 +- cmvr-es/devices/device_types.h | 67 + cmvr-es/devices/dexhand/CMakeLists.txt | 13 + cmvr-es/devices/dexhand/abstract_dexhand.h | 15 +- cmvr-es/devices/dexhand/dexhand_factory.h | 63 + .../dexhand/px_6ax_gen3/include/px_6ax_gen3.h | 9 +- .../dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp | 165 +- .../px_6ax_gen3/src/px_6ax_gen3_test.cpp | 35 +- .../dexhand/rh56dftp_dexhand/CMakeLists.txt | 2 +- .../include/rh56dftp_dexhand.h | 17 +- .../include/rh56dftp_tactile_buffer.h | 5 +- .../rh56dftp_dexhand/src/rh56dftp_dexhand.cpp | 139 +- .../src/rh56dftp_dexhand_test.cpp | 32 +- cmvr-es/devices/gripper/abstract_gripper.h | 5 +- cmvr-es/devices/microphone/CMakeLists.txt | 14 +- .../devices/microphone/abstract_microphone.h | 4 +- .../ffmpeg_microphone/CMakeLists.txt | 3 +- .../include/ffmpeg_microphone.h | 15 +- .../src/ffmpeg_microphone.cpp | 155 +- .../devices/microphone/microphone_factory.h | 60 + cmvr-es/devices/motor/CMakeLists.txt | 2 + cmvr-es/devices/motor/abstract_motor.h | 63 +- cmvr-es/devices/motor/motor_manager_test.cpp | 27 +- .../devices/motor/motor_system/CMakeLists.txt | 19 + .../motor/motor_system/include/motor_system.h | 81 + .../motor/motor_system/src/motor_system.cpp | 407 ++++ .../devices/motor/mujoco_motor/CMakeLists.txt | 13 + .../include/mujoco_joint_bridge.h | 57 + .../motor/mujoco_motor/include/mujoco_motor.h | 50 + .../motor/mujoco_motor/src/mujoco_motor.cpp | 225 +++ .../devices/motor/ti5_motor/CMakeLists.txt | 4 +- .../canopen/protocol/ti5_motor_rpdo1.cpp | 2 +- .../canopen/protocol/ti5_motor_rpdo2.cpp | 2 +- .../protocol/ti5_motor_sdo_response.cpp | 5 +- .../canopen/protocol/ti5_motor_tpdo1.cpp | 8 +- .../canopen/protocol/ti5_motor_tpdo2.cpp | 6 +- .../canopen/ti5_motor_canopen_protocol.cpp | 15 +- cmvr-es/devices/motor/ti5_motor/ti5_motor.h | 25 +- cmvr-es/devices/robot/CMakeLists.txt | 1 - cmvr-es/devices/robot/abstract_robot.h | 112 +- .../devices/robot/aubo_robot/CMakeLists.txt | 3 +- .../robot/aubo_robot/include/aubo_robot.h | 13 +- .../robot/aubo_robot/src/aubo_robot.cpp | 75 +- .../robot/c701/motor/motor_controller.cpp | 30 +- .../c701/motor/motor_controller_test.cpp | 7 +- .../c701/motor/protocol/ti5_motor_rpdo1.cpp | 2 +- .../motor/protocol/ti5_motor_sdo_response.cpp | 5 +- .../c701/motor/protocol/ti5_motor_tpdo1.cpp | 6 +- .../c701/motor/protocol/ti5_motor_tpdo2.cpp | 6 +- .../robot/humanoid_robot/CMakeLists.txt | 47 - .../humanoid_robot/include/humanoid_robot.h | 241 --- .../humanoid_robot/joint_positions_1.csv | 201 -- .../humanoid_robot/src/humanoid_robot.cpp | 1496 -------------- .../src/humanoid_robot_test.cpp | 992 ---------- .../robot/humanoid_robot/traj_with_vel.csv | 201 -- cmvr-es/devices/speaker/CMakeLists.txt | 14 +- cmvr-es/devices/speaker/abstract_speaker.h | 8 +- .../speaker/ffmpeg_speaker/CMakeLists.txt | 4 +- .../ffmpeg_speaker/include/ffmpeg_ptr.h | 65 +- .../ffmpeg_speaker/include/ffmpeg_speaker.h | 13 +- .../ffmpeg_speaker/src/ffmpeg_speaker.cpp | 161 +- cmvr-es/devices/speaker/speaker_factory.h | 60 + cmvr-es/devices/state_define.h | 4 +- cmvr-es/hardware/include/esp32_serial_port.h | 4 +- cmvr-es/hardware/src/esp32_serial_port.cpp | 24 +- cmvr-es/hardware/src/serial_interface.cpp | 45 +- .../ik_solver/include/bias_srs_ik_slover.h | 94 - cmvr-es/ik_solver/include/ik_solver_creator.h | 36 - .../include/pinocchio_dls_ik_solver.h | 316 --- cmvr-es/ik_solver/src/bias_srs_ik_slover.cpp | 360 ---- cmvr-es/ik_solver/src/ik_solver_creator.cpp | 40 - .../ik_solver/src/pinocchio_dls_ik_solver.cpp | 1193 ------------ cmvr-es/ik_solver/src/pinocchio_ik_base.cpp | 154 -- cmvr-es/main.cpp | 163 +- .../device_manager/CMakeLists.txt | 17 +- .../device_manager/include/device_factory.h | 41 + .../device_manager/include/device_manager.h | 57 + .../device_manager/src/device_factory.cpp | 281 +++ .../device_manager/src/device_manager.cpp | 425 ++++ cmvr-es/manager/task_manager/CMakeLists.txt | 17 + .../task_manager/include/task_manager.h | 58 + .../manager/task_manager/src/task_manager.cpp | 369 ++++ cmvr-es/monitor/CMakeLists.txt | 2 - cmvr-es/monitor/abstract_monitor.h | 42 - cmvr-es/monitor/diskmonitor/CMakeLists.txt | 9 - .../diskmonitor/include/disk_monitor.h | 32 - .../monitor/diskmonitor/src/disk_monitor.cpp | 188 -- cmvr-es/monitor_manager/CMakeLists.txt | 15 - .../monitor_manager/include/monitor_factory.h | 27 - .../monitor_manager/include/monitor_manager.h | 37 - .../monitor_manager/src/monitor_factory.cpp | 24 - .../monitor_manager/src/monitor_manager.cpp | 53 - cmvr-es/planner/CMakeLists.txt | 68 - .../include/joint_space_planner_creator.h | 32 - .../src/joint_space_planner.cpp | 18 - .../src/joint_space_planner_creator.cpp | 40 - cmvr-es/service/CMakeLists.txt | 17 +- .../service/grpc/include/grpc_arm_service.h | 58 + .../grpc/include/grpc_camera_service.h | 5 +- .../grpc/include/grpc_dexhand_service.h | 5 +- .../service/grpc/include/grpc_head_service.h | 3 +- .../service/grpc/include/grpc_hlc_service.h | 11 - .../include/grpc_humanoid_robot_service.h | 53 - .../grpc/include/grpc_microphone_service.h | 5 +- .../grpc/include/grpc_speaker_service.h | 5 +- .../grpc/include/grpc_system_service.h | 4 +- cmvr-es/service/grpc/include/server_runner.h | 56 - .../service/grpc/src/grpc_arm_client_test.cpp | 84 + cmvr-es/service/grpc/src/grpc_arm_service.cpp | 353 ++++ .../service/grpc/src/grpc_camera_service.cpp | 117 +- .../service/grpc/src/grpc_dexhand_service.cpp | 101 +- .../service/grpc/src/grpc_head_service.cpp | 63 +- .../service/grpc/src/grpc_hlc_client_test.cpp | 12 +- cmvr-es/service/grpc/src/grpc_hlc_service.cpp | 126 +- .../src/grpc_humanoid_robot_client_test.cpp | 138 -- .../grpc/src/grpc_humanoid_robot_service.cpp | 377 ---- .../grpc/src/grpc_microphone_service.cpp | 51 +- .../service/grpc/src/grpc_speaker_service.cpp | 51 +- .../service/grpc/src/grpc_system_service.cpp | 90 +- cmvr-es/service/grpc/src/server_runner.cpp | 186 -- .../mujoco/mujoco_viewer/CMakeLists.txt | 3 +- .../mujoco_viewer/include/mujoco_viewer.h | 5 +- .../mujoco_viewer/src/mujoco_viewer.cpp | 23 + .../mujoco_viewer/src/mujoco_viewer_test.cpp | 5 +- cmvr-es/task/CMakeLists.txt | 38 + .../include/grpc_server_task.h | 65 + .../grpc_server_task/src/grpc_server_task.cpp | 266 +++ cmvr-es/task/task.h | 57 + cmvr-es/task/task_factory.h | 102 + .../include/touch_screen_task.h | 180 ++ .../src/touch_screen_task.cpp | 1716 +++++++++++++++++ .../src/touch_screen_task_test.cpp | 646 +++++++ cmvr-es/utils/CMakeLists.txt | 27 - .../base/include/abstract_interpolation.h | 72 - cmvr-es/utils/base/include/logger.h | 53 - cmvr-es/utils/base/include/os.h | 33 - cmvr-es/utils/base/include/thread_pool.h | 80 - cmvr-es/utils/base/include/timer.h | 37 - .../utils/base/src/abstract_interpolation.cpp | 107 - cmvr-es/utils/base/src/thread_pool.cpp | 102 - cmvr-es/utils/base/src/timer.cpp | 87 - .../controller/include/cartesian_controller.h | 118 -- .../controller/src/cartesian_controller.cpp | 263 --- cmvr-es/utils/dynamics/include/inertial.h | 54 - cmvr-es/utils/dynamics/include/joint.h | 108 -- cmvr-es/utils/dynamics/include/link.h | 143 -- cmvr-es/utils/dynamics/include/robot.h | 288 --- cmvr-es/utils/dynamics/include/state.h | 141 -- cmvr-es/utils/dynamics/src/inertial.cpp | 69 - cmvr-es/utils/dynamics/src/joint.cpp | 155 -- cmvr-es/utils/dynamics/src/link.cpp | 194 -- cmvr-es/utils/dynamics/src/robot.cpp | 301 --- cmvr-es/utils/dynamics/src/robot.tpp | 828 -------- cmvr-es/utils/math/include/constants.h | 52 - cmvr-es/utils/math/include/liegroup.h | 15 - cmvr-es/utils/math/include/qp_solver.h | 126 -- cmvr-es/utils/math/include/se2.h | 42 - cmvr-es/utils/math/include/se3.h | 90 - cmvr-es/utils/math/include/so3.h | 121 -- .../utils/math/include/velocity_estimator.h | 124 -- cmvr-es/utils/math/src/qp_solver.cpp | 287 --- cmvr-es/utils/math/src/se2.cpp | 42 - cmvr-es/utils/math/src/se3.cpp | 422 ---- cmvr-es/utils/math/src/so3.cpp | 184 -- data/ik_movel_scurve.csv | 1090 +++++------ data/ik_movel_scurve_tcp.csv | 1092 +++++------ protos/cmvr/api/arm_command.proto | 185 ++ ..._robot_service.proto => arm_service.proto} | 16 +- protos/cmvr/api/humanoid_robot_command.proto | 202 -- protos/cmvr/common/geometry.proto | 79 + .../cmvr/config/agv_config/agv_config.proto | 24 + .../cmvr/config/arm_config/arm_config.proto | 168 ++ .../aubo_robot_config/aubo_robot_config.proto | 10 + .../biohead_config/biohead_config.proto | 23 + .../config/camera_config/camera_config.proto | 37 +- .../cmvr_es_config/cmvr_es_config.proto | 14 + .../device_manager_config.proto | 38 + .../dexhand_config/dexhand_config.proto | 25 +- .../grpc_server_config.proto | 12 + protos/cmvr/config/joint_limits_config.proto | 20 + protos/cmvr/config/lawba_ik_config.proto | 15 + .../config/logger_config/logger_config.proto | 37 + .../microphone_config/microphone_config.proto | 20 +- .../config/motor_config/motor_config.proto | 52 + .../cmvr/config/pinocchio_dls_ik_config.proto | 26 + ...fig.proto => pinocchio_qp_ik_config.proto} | 0 .../speaker_config/speaker_config.proto | 25 + .../speaker_config/speaker_conifg.proto | 11 - protos/cmvr/config/srs_ik_config.proto | 13 + .../task_manager_config.proto | 32 + .../touch_screen_algorithm_config.proto | 32 + .../touch_screen_app_config.proto | 155 -- .../config/touch_screen_common_config.proto | 40 + .../touch_screen_task_config.proto | 102 + protos/cmvr/msgs/geometry.proto | 51 - python/hand_eye_calibration/collect_data.py | 4 +- python/vision_servo/follow_target.py | 11 +- .../vision_servo/get_chessboard_position.py | 6 +- python/vision_servo/robot_warpper_test.py | 8 +- 399 files changed, 19676 insertions(+), 19619 deletions(-) create mode 100644 cmvr-es/algorithms/CMakeLists.txt rename cmvr-es/{controller => algorithms/controllers}/CMakeLists.txt (83%) create mode 100644 cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt create mode 100644 cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h create mode 100644 cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp rename cmvr-es/{controller => algorithms/controllers/ibvs}/include/ibvs_controller.h (95%) rename cmvr-es/{controller => algorithms/controllers/ibvs}/src/ibvs_controller.cpp (84%) rename cmvr-es/{controller => algorithms/controllers/pid}/include/pid_controller.h (100%) rename cmvr-es/{controller => algorithms/controllers/pid}/src/pid_controller.cpp (97%) rename cmvr-es/{controller => algorithms/controllers/tests}/src/controller_test.cpp (96%) rename cmvr-es/{ => algorithms/kinematics}/ik_solver/CMakeLists.txt (53%) rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/common}/include/ik_solver.h (98%) rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/common}/include/urdf_parser.h (100%) rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/common}/src/ik_solver.cpp (85%) rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/common}/src/urdf_parser.cpp (98%) create mode 100644 cmvr-es/algorithms/kinematics/ik_solver/ik_solver_factory.h rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/lawba}/include/joints_limit_analyzer.h (98%) rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/lawba}/include/lawba_ik_solver.h (79%) rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/lawba}/include/opt_psi_selector.h (95%) rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/lawba}/src/joints_limit_analyzer.cpp (99%) rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/lawba}/src/lawba_ik_solver.cpp (74%) rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/lawba}/src/opt_psi_selector.cpp (98%) create mode 100644 cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/pinocchio}/include/pinocchio_ik_base.h (59%) rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/pinocchio}/include/pinocchio_qp_ik_solver.h (51%) create mode 100644 cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp create mode 100644 cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/pinocchio}/src/pinocchio_qp_ik_solver.cpp (61%) create mode 100644 cmvr-es/algorithms/kinematics/ik_solver/srs/include/srs_ik_solver.h create mode 100644 cmvr-es/algorithms/kinematics/ik_solver/srs/src/srs_ik_solver.cpp rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/tests}/src/ik_test.cpp (87%) rename cmvr-es/{ik_solver => algorithms/kinematics/ik_solver/tests}/src/srs_ik_test.cpp (91%) create mode 100644 cmvr-es/algorithms/motion_planner/CMakeLists.txt create mode 100644 cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt create mode 100644 cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h create mode 100644 cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h create mode 100644 cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h create mode 100644 cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/src/pinocchio_dls_cartesian_motion_planner.cpp create mode 100644 cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h create mode 100644 cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp create mode 100644 cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h create mode 100644 cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h create mode 100644 cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner_factory.h create mode 100644 cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h create mode 100644 cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp create mode 100644 cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt rename cmvr-es/{planner/cartesian_space_planner => algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter}/include/cartesian_twist_limiter.h (98%) rename cmvr-es/{planner/cartesian_space_planner => algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter}/src/cartesian_twist_limiter.cpp (100%) rename cmvr-es/{planner/cartesian_space_planner => algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter}/src/cartesian_twist_limiter_test.cpp (99%) rename cmvr-es/{planner/joint_space_planner/include/joint_space_planner.h => algorithms/motion_planner/base_motion/joint_trajectory/joint_trajectory_planner.h} (78%) rename cmvr-es/{planner/joint_space_planner/include/toppra_bspline.h => algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h} (97%) rename cmvr-es/{planner/joint_space_planner/src/toppra_bspline.cpp => algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp} (88%) rename cmvr-es/{planner/joint_space_planner/src/joint_space_planner_test.cpp => algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner_test.cpp} (85%) rename cmvr-es/{common/curve => algorithms/motion_planner/base_motion/motion_profile/s_curve}/include/s_curve.h (100%) rename cmvr-es/{planner/s_curve_planner => algorithms/motion_planner/base_motion/motion_profile/s_curve}/include/s_curve_position_planner.h (100%) rename cmvr-es/{planner/s_curve_planner => algorithms/motion_planner/base_motion/motion_profile/s_curve}/include/s_curve_velocity_planner.h (100%) rename cmvr-es/{common/curve => algorithms/motion_planner/base_motion/motion_profile/s_curve}/src/s_curve.cpp (99%) rename cmvr-es/{planner/s_curve_planner => algorithms/motion_planner/base_motion/motion_profile/s_curve}/src/s_curve_position_planner.cpp (100%) rename cmvr-es/{planner/s_curve_planner => algorithms/motion_planner/base_motion/motion_profile/s_curve}/src/s_curve_velocity_planner.cpp (100%) rename cmvr-es/{ => algorithms}/perception/CMakeLists.txt (86%) rename cmvr-es/{perception => algorithms/perception/apriltag}/include/apriltag_perception.h (100%) rename cmvr-es/{perception => algorithms/perception/apriltag}/include/tag_relative_target_3d.h (99%) rename cmvr-es/{perception => algorithms/perception/apriltag}/src/apriltag_perception.cpp (99%) rename cmvr-es/{perception => algorithms/perception/apriltag}/src/tag_relative_target_3d.cpp (99%) rename cmvr-es/{perception => algorithms/perception/apriltag}/src/tag_relative_target_3d_test.cpp (99%) delete mode 100644 cmvr-es/applications/CMakeLists.txt delete mode 100644 cmvr-es/applications/include/touch_screen_app.h delete mode 100644 cmvr-es/applications/src/touch_screen_app.cpp delete mode 100644 cmvr-es/applications/src/touch_screen_app_test.cpp rename cmvr-es/common/{consts/constant.h => base/constants.h} (100%) rename cmvr-es/{utils/base/include => common/base}/grpc_utils.h (91%) create mode 100644 cmvr-es/common/base/logging/CMakeLists.txt create mode 100644 cmvr-es/common/base/logging/logger.cpp create mode 100644 cmvr-es/common/base/logging/logger.h rename cmvr-es/{utils/base/include => common/base}/ring_buffer.h (100%) delete mode 100644 cmvr-es/common/config/cabin_robot.xml delete mode 100644 cmvr-es/common/config/camera_config/camera_config.pb.txt create mode 100644 cmvr-es/common/config/config_files.h delete mode 100644 cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt delete mode 100644 cmvr-es/common/config/ik_solver_config/lawba_ik_solver_config.txt delete mode 100644 cmvr-es/common/config/ik_solver_config/pinocchio_dls_ik_solver_config.pb.txt delete mode 100644 cmvr-es/common/config/ik_solver_config/pinocchio_qp_ik_solver_config.pb.txt delete mode 100644 cmvr-es/common/config/microphone_config/microphone_config.pb.txt delete mode 100644 cmvr-es/common/config/speaker_config/speaker_config.pb.txt delete mode 100644 cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt rename cmvr-es/common/{utils/io/proto_message_utils.h => io/proto_file_io.h} (66%) create mode 100644 cmvr-es/common/math/cartesian_motion_math.h create mode 100644 cmvr-es/common/math/joint_limits.h create mode 100644 cmvr-es/common/math/proto_geometry.h rename cmvr-es/common/math/{include => }/qp_solver.h (98%) rename cmvr-es/common/math/{include => }/support_functions.h (96%) create mode 100644 cmvr-es/common/math/transform_math.h rename cmvr-es/common/{utils/ffmpeg/src/CameraCapture.cpp => media/ffmpeg/camera_capture.cpp} (81%) rename cmvr-es/common/{utils/ffmpeg/include/CameraCapture.h => media/ffmpeg/camera_capture.h} (100%) rename cmvr-es/common/{utils/ffmpeg/src/RealSenseCapture.cpp => media/ffmpeg/realsense_capture.cpp} (83%) rename cmvr-es/common/{utils/ffmpeg/include/RealSenseCapture.h => media/ffmpeg/realsense_capture.h} (100%) rename cmvr-es/common/{utils/ffmpeg/src/VideoFrameEncoder.cpp => media/ffmpeg/video_frame_encoder.cpp} (88%) rename cmvr-es/common/{utils/ffmpeg/include/VideoFrameEncoder.h => media/ffmpeg/video_frame_encoder.h} (100%) rename cmvr-es/common/{utils/ffmpeg/src/VideoWriter.cpp => media/ffmpeg/video_writer.cpp} (79%) rename cmvr-es/common/{utils/ffmpeg/include/VideoWriter.h => media/ffmpeg/video_writer.h} (100%) create mode 100644 cmvr-es/common/types/arm/arm_types.h rename cmvr-es/{utils/math/include/geometry.h => common/types/geometry_types.h} (97%) delete mode 100644 cmvr-es/common/utils/config_helper/include/config_helper.h delete mode 100644 cmvr-es/common/utils/config_helper/include/config_setting.h delete mode 100644 cmvr-es/common/utils/config_helper/src/config_setting.cpp rename cmvr-es/common/{utils/visualization => vision}/image_display.h (100%) rename cmvr-es/common/{utils/image/image_process.h => vision/image_projection.h} (100%) rename cmvr-es/common/{utils/visualization => vision}/matplotlibcpp.h (100%) create mode 100644 cmvr-es/config/cmvr_es.pb.txt create mode 100644 cmvr-es/config/devices/agv/agv.pb.txt create mode 100644 cmvr-es/config/devices/arm/arm.pb.txt create mode 100644 cmvr-es/config/devices/arm/arm_dls_ik_dls_motion.pb.txt create mode 100644 cmvr-es/config/devices/arm/arm_dls_ik_qp_motion.pb.txt create mode 100644 cmvr-es/config/devices/arm/arm_mujoco.pb.txt create mode 100644 cmvr-es/config/devices/arm/arm_mujoco_dls_ik_dls_motion.pb.txt create mode 100644 cmvr-es/config/devices/arm/arm_mujoco_dls_ik_qp_motion.pb.txt create mode 100644 cmvr-es/config/devices/arm/arm_mujoco_qp_ik_qp_motion.pb.txt create mode 100644 cmvr-es/config/devices/arm/arm_qp_ik_qp_motion.pb.txt create mode 100644 cmvr-es/config/devices/arm/aubo_arm.pb.txt create mode 100644 cmvr-es/config/devices/biohead/bio_head.pb.txt create mode 100644 cmvr-es/config/devices/camera/camera.pb.txt create mode 100644 cmvr-es/config/devices/dexhand/dexhand.pb.txt create mode 100644 cmvr-es/config/devices/microphone/microphone.pb.txt create mode 100644 cmvr-es/config/devices/motor/mujoco_motors.pb.txt create mode 100644 cmvr-es/config/devices/motor/ti5_motors.pb.txt create mode 100644 cmvr-es/config/devices/speaker/speaker.pb.txt create mode 100644 cmvr-es/config/logger/logger.pb.txt create mode 100644 cmvr-es/config/manager/device_manager.pb.txt create mode 100644 cmvr-es/config/manager/task_manager.pb.txt create mode 100644 cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt create mode 100644 cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt create mode 100644 cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt delete mode 100644 cmvr-es/controller/include/touch_controller.h delete mode 100644 cmvr-es/data_center/CMakeLists.txt delete mode 100644 cmvr-es/data_center/include/data_center.h delete mode 100644 cmvr-es/data_center/include/motors_info.h delete mode 100644 cmvr-es/data_center/src/data_center.cpp delete mode 100644 cmvr-es/data_center/src/motors_info.cpp delete mode 100644 cmvr-es/device_manager/include/device_factory.h delete mode 100644 cmvr-es/device_manager/include/device_manager.h delete mode 100644 cmvr-es/device_manager/include/system_monitor.h delete mode 100644 cmvr-es/device_manager/src/device_factory.cpp delete mode 100644 cmvr-es/device_manager/src/device_manager.cpp delete mode 100644 cmvr-es/device_manager/src/system_monitor.cpp create mode 100644 cmvr-es/devices/agv/CMakeLists.txt create mode 100644 cmvr-es/devices/agv/agv_factory.h create mode 100644 cmvr-es/devices/agv/my_agv/CMakeLists.txt create mode 100644 cmvr-es/devices/agv/my_agv/include/my_agv.h create mode 100644 cmvr-es/devices/agv/my_agv/src/my_agv.cpp create mode 100644 cmvr-es/devices/arm/CMakeLists.txt create mode 100644 cmvr-es/devices/arm/aubo_arm/CMakeLists.txt create mode 100644 cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h create mode 100644 cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp create mode 100644 cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt create mode 100644 cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h create mode 100644 cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp create mode 100644 cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp create mode 100644 cmvr-es/devices/arm/robot_arm.h create mode 100644 cmvr-es/devices/arm/robot_arm_factory.h create mode 100644 cmvr-es/devices/camera/camera_factory.h create mode 100644 cmvr-es/devices/device_types.h create mode 100644 cmvr-es/devices/dexhand/dexhand_factory.h create mode 100644 cmvr-es/devices/microphone/microphone_factory.h create mode 100644 cmvr-es/devices/motor/motor_system/CMakeLists.txt create mode 100644 cmvr-es/devices/motor/motor_system/include/motor_system.h create mode 100644 cmvr-es/devices/motor/motor_system/src/motor_system.cpp create mode 100644 cmvr-es/devices/motor/mujoco_motor/CMakeLists.txt create mode 100644 cmvr-es/devices/motor/mujoco_motor/include/mujoco_joint_bridge.h create mode 100644 cmvr-es/devices/motor/mujoco_motor/include/mujoco_motor.h create mode 100644 cmvr-es/devices/motor/mujoco_motor/src/mujoco_motor.cpp delete mode 100644 cmvr-es/devices/robot/humanoid_robot/CMakeLists.txt delete mode 100644 cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h delete mode 100644 cmvr-es/devices/robot/humanoid_robot/joint_positions_1.csv delete mode 100644 cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp delete mode 100644 cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp delete mode 100644 cmvr-es/devices/robot/humanoid_robot/traj_with_vel.csv create mode 100644 cmvr-es/devices/speaker/speaker_factory.h delete mode 100644 cmvr-es/ik_solver/include/bias_srs_ik_slover.h delete mode 100644 cmvr-es/ik_solver/include/ik_solver_creator.h delete mode 100644 cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h delete mode 100644 cmvr-es/ik_solver/src/bias_srs_ik_slover.cpp delete mode 100644 cmvr-es/ik_solver/src/ik_solver_creator.cpp delete mode 100644 cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp delete mode 100644 cmvr-es/ik_solver/src/pinocchio_ik_base.cpp rename cmvr-es/{ => manager}/device_manager/CMakeLists.txt (55%) create mode 100644 cmvr-es/manager/device_manager/include/device_factory.h create mode 100644 cmvr-es/manager/device_manager/include/device_manager.h create mode 100644 cmvr-es/manager/device_manager/src/device_factory.cpp create mode 100644 cmvr-es/manager/device_manager/src/device_manager.cpp create mode 100644 cmvr-es/manager/task_manager/CMakeLists.txt create mode 100644 cmvr-es/manager/task_manager/include/task_manager.h create mode 100644 cmvr-es/manager/task_manager/src/task_manager.cpp delete mode 100644 cmvr-es/monitor/CMakeLists.txt delete mode 100644 cmvr-es/monitor/abstract_monitor.h delete mode 100644 cmvr-es/monitor/diskmonitor/CMakeLists.txt delete mode 100644 cmvr-es/monitor/diskmonitor/include/disk_monitor.h delete mode 100644 cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp delete mode 100644 cmvr-es/monitor_manager/CMakeLists.txt delete mode 100644 cmvr-es/monitor_manager/include/monitor_factory.h delete mode 100644 cmvr-es/monitor_manager/include/monitor_manager.h delete mode 100644 cmvr-es/monitor_manager/src/monitor_factory.cpp delete mode 100644 cmvr-es/monitor_manager/src/monitor_manager.cpp delete mode 100644 cmvr-es/planner/CMakeLists.txt delete mode 100644 cmvr-es/planner/joint_space_planner/include/joint_space_planner_creator.h delete mode 100644 cmvr-es/planner/joint_space_planner/src/joint_space_planner.cpp delete mode 100644 cmvr-es/planner/joint_space_planner/src/joint_space_planner_creator.cpp create mode 100644 cmvr-es/service/grpc/include/grpc_arm_service.h delete mode 100644 cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h delete mode 100644 cmvr-es/service/grpc/include/server_runner.h create mode 100644 cmvr-es/service/grpc/src/grpc_arm_client_test.cpp create mode 100644 cmvr-es/service/grpc/src/grpc_arm_service.cpp delete mode 100644 cmvr-es/service/grpc/src/grpc_humanoid_robot_client_test.cpp delete mode 100644 cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp delete mode 100644 cmvr-es/service/grpc/src/server_runner.cpp create mode 100644 cmvr-es/task/CMakeLists.txt create mode 100644 cmvr-es/task/grpc_server_task/include/grpc_server_task.h create mode 100644 cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp create mode 100644 cmvr-es/task/task.h create mode 100644 cmvr-es/task/task_factory.h create mode 100644 cmvr-es/task/touch_screen_task/include/touch_screen_task.h create mode 100644 cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp create mode 100644 cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp delete mode 100644 cmvr-es/utils/CMakeLists.txt delete mode 100644 cmvr-es/utils/base/include/abstract_interpolation.h delete mode 100644 cmvr-es/utils/base/include/logger.h delete mode 100644 cmvr-es/utils/base/include/os.h delete mode 100644 cmvr-es/utils/base/include/thread_pool.h delete mode 100644 cmvr-es/utils/base/include/timer.h delete mode 100644 cmvr-es/utils/base/src/abstract_interpolation.cpp delete mode 100644 cmvr-es/utils/base/src/thread_pool.cpp delete mode 100644 cmvr-es/utils/base/src/timer.cpp delete mode 100644 cmvr-es/utils/controller/include/cartesian_controller.h delete mode 100644 cmvr-es/utils/controller/src/cartesian_controller.cpp delete mode 100644 cmvr-es/utils/dynamics/include/inertial.h delete mode 100644 cmvr-es/utils/dynamics/include/joint.h delete mode 100644 cmvr-es/utils/dynamics/include/link.h delete mode 100644 cmvr-es/utils/dynamics/include/robot.h delete mode 100644 cmvr-es/utils/dynamics/include/state.h delete mode 100644 cmvr-es/utils/dynamics/src/inertial.cpp delete mode 100644 cmvr-es/utils/dynamics/src/joint.cpp delete mode 100644 cmvr-es/utils/dynamics/src/link.cpp delete mode 100644 cmvr-es/utils/dynamics/src/robot.cpp delete mode 100644 cmvr-es/utils/dynamics/src/robot.tpp delete mode 100644 cmvr-es/utils/math/include/constants.h delete mode 100644 cmvr-es/utils/math/include/liegroup.h delete mode 100644 cmvr-es/utils/math/include/qp_solver.h delete mode 100644 cmvr-es/utils/math/include/se2.h delete mode 100644 cmvr-es/utils/math/include/se3.h delete mode 100644 cmvr-es/utils/math/include/so3.h delete mode 100644 cmvr-es/utils/math/include/velocity_estimator.h delete mode 100644 cmvr-es/utils/math/src/qp_solver.cpp delete mode 100644 cmvr-es/utils/math/src/se2.cpp delete mode 100644 cmvr-es/utils/math/src/se3.cpp delete mode 100644 cmvr-es/utils/math/src/so3.cpp create mode 100644 protos/cmvr/api/arm_command.proto rename protos/cmvr/api/{humanoid_robot_service.proto => arm_service.proto} (86%) delete mode 100644 protos/cmvr/api/humanoid_robot_command.proto create mode 100644 protos/cmvr/common/geometry.proto create mode 100644 protos/cmvr/config/agv_config/agv_config.proto create mode 100644 protos/cmvr/config/arm_config/arm_config.proto create mode 100644 protos/cmvr/config/aubo_robot_config/aubo_robot_config.proto create mode 100644 protos/cmvr/config/biohead_config/biohead_config.proto create mode 100644 protos/cmvr/config/cmvr_es_config/cmvr_es_config.proto create mode 100644 protos/cmvr/config/device_manager_config/device_manager_config.proto create mode 100644 protos/cmvr/config/grpc_server_config/grpc_server_config.proto create mode 100644 protos/cmvr/config/joint_limits_config.proto create mode 100644 protos/cmvr/config/lawba_ik_config.proto create mode 100644 protos/cmvr/config/logger_config/logger_config.proto create mode 100644 protos/cmvr/config/motor_config/motor_config.proto rename protos/cmvr/config/{pinocchio_qp_ik_solver_config.proto => pinocchio_qp_ik_config.proto} (100%) create mode 100644 protos/cmvr/config/speaker_config/speaker_config.proto delete mode 100644 protos/cmvr/config/speaker_config/speaker_conifg.proto create mode 100644 protos/cmvr/config/srs_ik_config.proto create mode 100644 protos/cmvr/config/task_manager_config/task_manager_config.proto create mode 100644 protos/cmvr/config/touch_screen_algorithm_config.proto delete mode 100644 protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto create mode 100644 protos/cmvr/config/touch_screen_common_config.proto create mode 100644 protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto delete mode 100644 protos/cmvr/msgs/geometry.proto diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e100d3a..e12aaf6c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -106,23 +106,29 @@ add_executable(cmvr_es cmvr-es/main.cpp) target_include_directories(cmvr_es PRIVATE ${GLOG_INCLUDE_DIRS}) target_link_libraries(cmvr_es PRIVATE cmvr_es::proto + cmvr_es::logging service ${GLOG_LIBRARIES} jsoncpp - cmvr_es::utils cmvr_es::service - cmvr_es::monitor_manager cmvr_es::hardware cmvr_es::device::canbus cmvr_es::device::ti5motor - cmvr_es::controller - cmvr_es::data_center + cmvr_es::algorithms::controller cmvr_es::ik_solver - cmvr_es::planner - cmvr_es::device::humanoid_robot + cmvr_es::base_motion cmvr_es::common - cmvr_es::applications + cmvr_es::task + cmvr_es::task_manager + ccd + fcl ) install(TARGETS cmvr_es RUNTIME DESTINATION bin) -install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/cmvr-es/common/config DESTINATION bin) +install(CODE [[ + file(REMOVE_RECURSE + "${CMAKE_INSTALL_PREFIX}/bin/config" + "${CMAKE_INSTALL_PREFIX}/bin/model") +]]) +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/cmvr-es/config DESTINATION bin) +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/model DESTINATION bin) diff --git a/cmvr-es/CMakeLists.txt b/cmvr-es/CMakeLists.txt index 7f38a336..eb16b40f 100644 --- a/cmvr-es/CMakeLists.txt +++ b/cmvr-es/CMakeLists.txt @@ -1,16 +1,12 @@ -add_subdirectory(utils) -add_subdirectory(hardware) -add_subdirectory(devices) -add_subdirectory(device_manager) -add_subdirectory(monitor) -add_subdirectory(monitor_manager) -add_subdirectory(service) -add_subdirectory(perception) -add_subdirectory(controller) -add_subdirectory(planner) +add_subdirectory(common/base/logging) +link_libraries(cmvr_es::logging) -add_subdirectory(ik_solver) -add_subdirectory(data_center) -add_subdirectory(applications) -add_subdirectory(simulate) add_subdirectory(common) +add_subdirectory(hardware) +add_subdirectory(algorithms) +add_subdirectory(devices) +add_subdirectory(manager/device_manager) +add_subdirectory(task) +add_subdirectory(manager/task_manager) +add_subdirectory(service) +add_subdirectory(simulate) diff --git a/cmvr-es/algorithms/CMakeLists.txt b/cmvr-es/algorithms/CMakeLists.txt new file mode 100644 index 00000000..53f2415c --- /dev/null +++ b/cmvr-es/algorithms/CMakeLists.txt @@ -0,0 +1,4 @@ +add_subdirectory(motion_planner) +add_subdirectory(kinematics/ik_solver) +add_subdirectory(perception) +add_subdirectory(controllers) diff --git a/cmvr-es/controller/CMakeLists.txt b/cmvr-es/algorithms/controllers/CMakeLists.txt similarity index 83% rename from cmvr-es/controller/CMakeLists.txt rename to cmvr-es/algorithms/controllers/CMakeLists.txt index 86b3027a..072d4eb4 100644 --- a/cmvr-es/controller/CMakeLists.txt +++ b/cmvr-es/algorithms/controllers/CMakeLists.txt @@ -1,7 +1,8 @@ +add_subdirectory(arm_control) + #find_package(VISP REQUIRED) - # 如果报 relocation ... can not be used when making a shared object; recompile with -fPIC ，说明SRC 中包含了test文件 ，test # 中链接 -lgtest -lgtest_main ，这是静态库,导致 libcontroller.so 被迫依赖 gtest。 # 所以 add_library(controller SHARED @@ -10,8 +11,8 @@ # ❌ 不要把 src/controller_test.cpp 放进来 #) 其他动态库类似 file(GLOB SRC - ${CMAKE_CURRENT_SOURCE_DIR}/src/pid_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/ibvs_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/pid/src/pid_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/ibvs/src/ibvs_controller.cpp ) @@ -24,7 +25,6 @@ target_link_libraries(controller PUBLIC protobuf cmvr_es::perception cmvr_es::ik_solver - cmvr_es::device::humanoid_robot gtest gtest_main pthread @@ -55,7 +55,7 @@ target_link_libraries(controller PUBLIC pinocchio_parsers ) -add_library(cmvr_es::controller ALIAS controller) +add_library(cmvr_es::algorithms::controller ALIAS controller) install(TARGETS controller LIBRARY DESTINATION lib) @@ -65,18 +65,17 @@ install(TARGETS controller LIBRARY DESTINATION lib) # -------------------------------------------------------- find_package(realsense2 REQUIRED) add_executable(controller_test - ${CMAKE_CURRENT_SOURCE_DIR}/src/controller_test.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/tests/src/controller_test.cpp ) target_link_libraries(controller_test PRIVATE - cmvr_es::utils cmvr_es::perception cmvr_es::ik_solver - cmvr_es::planner + cmvr_es::base_motion cmvr_es::proto cmvr_es::mujoco_viewer - cmvr_es::controller + cmvr_es::algorithms::controller cmvr_es::device::mujoco_camera gtest gtest_main diff --git a/cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt b/cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt new file mode 100644 index 00000000..120e91f9 --- /dev/null +++ b/cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt @@ -0,0 +1,13 @@ +add_library(arm_control SHARED + src/cartesian_velocity_controller.cpp +) + +target_include_directories(arm_control PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(arm_control + PUBLIC + cmvr_es::algorithms::arm_motion +) + +add_library(cmvr_es::algorithms::arm_control ALIAS arm_control) +install(TARGETS arm_control LIBRARY DESTINATION lib) diff --git a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h new file mode 100644 index 00000000..f2438dcc --- /dev/null +++ b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h @@ -0,0 +1,80 @@ +#ifndef CMVR_ES_CARTESIAN_VELOCITY_CONTROLLER_H +#define CMVR_ES_CARTESIAN_VELOCITY_CONTROLLER_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h" +#include "common/types/arm/arm_types.h" + +namespace cmvr::device { + +class CartesianVelocityController { +public: + struct Config { + double control_period_s{0.001}; + double stop_twist_norm{1e-9}; + double stop_command_velocity_norm{1e-3}; + double stop_measured_velocity_norm{1e-2}; + }; + + using ReadStateCallback = std::function& q, std::vector& qd)>; + using SendVelocityCallback = std::function; + + CartesianVelocityController(Config config, + std::shared_ptr planner, + std::size_t dof, + ReadStateCallback read_state, + SendVelocityCallback send_velocity); + ~CartesianVelocityController(); + + CartesianVelocityController(const CartesianVelocityController&) = delete; + CartesianVelocityController& operator=(const CartesianVelocityController&) = delete; + + Result speedL(const CartesianVelocity& velocity, + double acceleration, + double duration, + FrameType frame); + Result stop(double acceleration); + void shutdown(); + + bool busy() const { return busy_.load(); } + CartesianVelocity getCommandTwistBase() const; + +private: + void ensureWorkerStarted_(); + void workerLoop_(); + void sendZero_(); + + static double velocityNorm_(const std::vector& velocity); + static double twistNorm_(const CartesianVelocity& velocity); + +private: + Config config_; + std::shared_ptr planner_; + std::size_t dof_{0}; + ReadStateCallback read_state_; + SendVelocityCallback send_velocity_; + + std::unique_ptr worker_; + mutable std::mutex mutex_; + std::condition_variable cv_; + std::atomic stop_requested_{false}; + bool command_active_{false}; + CartesianVelocity target_twist_{}; + FrameType target_frame_{FrameType::Base}; + double target_acceleration_{0.25}; + std::uint64_t command_version_{0}; + std::atomic busy_{false}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_CARTESIAN_VELOCITY_CONTROLLER_H diff --git a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp new file mode 100644 index 00000000..5f19699d --- /dev/null +++ b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp @@ -0,0 +1,282 @@ +#include "algorithms/controllers/arm_control/include/cartesian_velocity_controller.h" + +#include +#include +#include +#include + +#include "common/base/logging/logger.h" + +namespace cmvr::device { + +namespace { + +CartesianVelocityController::Config normalizeConfig(CartesianVelocityController::Config config) +{ + const CartesianVelocityController::Config defaults; + if (config.control_period_s <= 0.0) { + config.control_period_s = defaults.control_period_s; + } + if (config.stop_twist_norm <= 0.0) { + config.stop_twist_norm = defaults.stop_twist_norm; + } + if (config.stop_command_velocity_norm <= 0.0) { + config.stop_command_velocity_norm = defaults.stop_command_velocity_norm; + } + if (config.stop_measured_velocity_norm <= 0.0) { + config.stop_measured_velocity_norm = defaults.stop_measured_velocity_norm; + } + return config; +} + +} // namespace + +CartesianVelocityController::CartesianVelocityController( + Config config, + std::shared_ptr planner, + const std::size_t dof, + ReadStateCallback read_state, + SendVelocityCallback send_velocity) + : config_(normalizeConfig(config)), + planner_(std::move(planner)), + dof_(dof), + read_state_(std::move(read_state)), + send_velocity_(std::move(send_velocity)) +{ +} + +CartesianVelocityController::~CartesianVelocityController() +{ + shutdown(); +} + +Result CartesianVelocityController::speedL(const CartesianVelocity& velocity, + const double acceleration, + const double duration, + const FrameType frame) +{ + if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) { + return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input"); + } + if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy"); + } + + ensureWorkerStarted_(); + + std::uint64_t command_version = 0; + { + std::lock_guard lock(mutex_); + target_twist_ = velocity; + target_acceleration_ = acceleration; + target_frame_ = frame; + command_active_ = true; + command_version = ++command_version_; + } + cv_.notify_all(); + + if (duration > 0.0) { + std::this_thread::sleep_for(std::chrono::duration(duration)); + bool should_stop = false; + { + std::lock_guard lock(mutex_); + if (command_version_ == command_version) { + target_twist_ = {}; + target_frame_ = FrameType::Base; + command_active_ = true; + ++command_version_; + should_stop = true; + } + } + if (should_stop) { + cv_.notify_all(); + } + } + return Result::success(); +} + +Result CartesianVelocityController::stop(const double acceleration) +{ + (void)acceleration; + if (!worker_ || !worker_->joinable()) { + return Result::success(); + } + { + std::lock_guard lock(mutex_); + target_twist_ = {}; + target_frame_ = FrameType::Base; + command_active_ = true; + ++command_version_; + } + cv_.notify_all(); + return Result::success(); +} + +void CartesianVelocityController::shutdown() +{ + if (!worker_ || !worker_->joinable()) { + busy_.store(false); + return; + } + { + std::lock_guard lock(mutex_); + stop_requested_.store(true); + command_active_ = false; + target_twist_ = {}; + target_frame_ = FrameType::Base; + } + cv_.notify_all(); + worker_->join(); + worker_.reset(); + stop_requested_.store(false); + busy_.store(false); +} + +CartesianVelocity CartesianVelocityController::getCommandTwistBase() const +{ + if (!planner_) { + return {}; + } + return planner_->getSpeedLCommandTwistBase(); +} + +void CartesianVelocityController::ensureWorkerStarted_() +{ + if (worker_ && worker_->joinable()) { + return; + } + stop_requested_.store(false); + worker_ = std::make_unique(&CartesianVelocityController::workerLoop_, this); +} + +void CartesianVelocityController::workerLoop_() +{ + const double dt = config_.control_period_s; + auto next_tick = std::chrono::steady_clock::now(); + + while (true) { + CartesianVelocity target_twist; + double acceleration = 0.25; + FrameType target_frame = FrameType::Base; + { + std::unique_lock lock(mutex_); + cv_.wait(lock, [&]() { + return stop_requested_.load() || command_active_; + }); + if (stop_requested_.load()) { + break; + } + target_twist = target_twist_; + acceleration = target_acceleration_; + target_frame = target_frame_; + } + + next_tick = std::chrono::steady_clock::now(); + while (true) { + { + std::lock_guard lock(mutex_); + if (stop_requested_.load()) { + sendZero_(); + busy_.store(false); + return; + } + if (!command_active_) { + break; + } + target_twist = target_twist_; + acceleration = target_acceleration_; + target_frame = target_frame_; + } + + if (!planner_->updateSpeedLAcceleration(acceleration)) { + CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration=" + << acceleration; + sendZero_(); + busy_.store(false); + return; + } + + std::vector q_now; + std::vector qd_now; + if (!read_state_(q_now, qd_now)) { + CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed"; + sendZero_(); + busy_.store(false); + return; + } + + std::vector qd_cmd; + if (!planner_->speedLStep(target_twist, dt, q_now, qd_now, qd_cmd, target_frame)) { + CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] speedLStep failed, target_twist=[" + << target_twist.vx << ", " << target_twist.vy << ", " + << target_twist.vz << ", " << target_twist.wx << ", " + << target_twist.wy << ", " << target_twist.wz + << "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base"); + sendZero_(); + busy_.store(false); + return; + } + + JointVelocityCommand velocity_command; + velocity_command.velocity = qd_cmd; + const auto send_result = send_velocity_(velocity_command, acceleration); + if (!send_result.ok()) { + CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: " + << send_result.message; + sendZero_(); + busy_.store(false); + return; + } + + if (twistNorm_(target_twist) < config_.stop_twist_norm && + velocityNorm_(qd_cmd) < config_.stop_command_velocity_norm && + velocityNorm_(qd_now) < config_.stop_measured_velocity_norm) { + { + std::lock_guard lock(mutex_); + command_active_ = false; + } + sendZero_(); + busy_.store(false); + break; + } + + next_tick += std::chrono::duration_cast( + std::chrono::duration(dt)); + std::this_thread::sleep_until(next_tick); + } + } + + sendZero_(); + busy_.store(false); +} + +void CartesianVelocityController::sendZero_() +{ + if (!send_velocity_) { + return; + } + JointVelocityCommand zero; + zero.velocity.assign(dof_, 0.0); + (void)send_velocity_(zero, 0.0); +} + +double CartesianVelocityController::velocityNorm_(const std::vector& velocity) +{ + double value = 0.0; + for (const double item : velocity) { + value += item * item; + } + return std::sqrt(value); +} + +double CartesianVelocityController::twistNorm_(const CartesianVelocity& velocity) +{ + return std::sqrt(velocity.vx * velocity.vx + + velocity.vy * velocity.vy + + velocity.vz * velocity.vz + + velocity.wx * velocity.wx + + velocity.wy * velocity.wy + + velocity.wz * velocity.wz); +} + +} // namespace cmvr::device diff --git a/cmvr-es/controller/include/ibvs_controller.h b/cmvr-es/algorithms/controllers/ibvs/include/ibvs_controller.h similarity index 95% rename from cmvr-es/controller/include/ibvs_controller.h rename to cmvr-es/algorithms/controllers/ibvs/include/ibvs_controller.h index d8afb684..40063687 100644 --- a/cmvr-es/controller/include/ibvs_controller.h +++ b/cmvr-es/algorithms/controllers/ibvs/include/ibvs_controller.h @@ -14,8 +14,8 @@ #include #include -#include "ik_solver/include/pinocchio_dls_ik_solver.h" -#include "perception/include/apriltag_perception.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" +#include "algorithms/perception/apriltag/include/apriltag_perception.h" namespace cmvr { @@ -66,16 +66,12 @@ public: IbvsController(); /** - * @brief 初始化 DLS 速度 IK 求解器（不再需要相机）。 - * @param urdf_path URDF 文件路径。 - * @param base_link IK 链基座 link 名称。 - * @param flange_link IK 链末端法兰 link 名称。 + * @brief 注入机械臂当前配置创建的 Pinocchio kinematics solver。 + * @param solver 来自 RobotArm 的 Pinocchio IK solver。 * @param camera_link URDF 中相机 link 名称，对应坐标系 `u`。 * @return 初始化成功返回 `true`。 */ - bool init(const std::string& urdf_path, - const std::string& base_link, - const std::string& flange_link, + bool init(std::shared_ptr solver, const std::string& camera_link); /** @@ -290,8 +286,8 @@ private: private: bool initialized_{false}; - std::string base_frame_name_; std::string camera_frame_name_; + std::shared_ptr solver_{nullptr}; std::shared_ptr perception_{nullptr}; double lambda_{0.7}; @@ -336,7 +332,6 @@ private: vpFeaturePoint s_star_[4]; int tracked_tag_id_{-1}; - std::unique_ptr dls_solver_{nullptr}; bool has_joint_position_limits_{false}; Eigen::VectorXd q_lower_limits_; diff --git a/cmvr-es/controller/src/ibvs_controller.cpp b/cmvr-es/algorithms/controllers/ibvs/src/ibvs_controller.cpp similarity index 84% rename from cmvr-es/controller/src/ibvs_controller.cpp rename to cmvr-es/algorithms/controllers/ibvs/src/ibvs_controller.cpp index e09773a7..5c83acab 100644 --- a/cmvr-es/controller/src/ibvs_controller.cpp +++ b/cmvr-es/algorithms/controllers/ibvs/src/ibvs_controller.cpp @@ -1,7 +1,8 @@ -#include "controller/include/ibvs_controller.h" +#include "algorithms/controllers/ibvs/include/ibvs_controller.h" -#include "common/utils/image/image_process.h" -#include "common/math/include/support_functions.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" +#include "common/vision/image_projection.h" +#include "common/math/support_functions.h" #include #include @@ -107,24 +108,20 @@ IbvsController::IbvsController() { initTask(); } -bool IbvsController::init(const std::string& urdf_path, - const std::string& base_link, - const std::string& flange_link, +bool IbvsController::init(std::shared_ptr solver, const std::string& camera_link) { - base_frame_name_ = base_link; + solver_ = std::move(solver); camera_frame_name_ = camera_link; - - dls_solver_ = std::make_unique( - urdf_path, base_link, flange_link, camera_frame_name_, 100, 1e-6, 1e-6, mu_); - - initialized_ = dls_solver_->init(); + initialized_ = solver_ != nullptr && !camera_frame_name_.empty(); if (initialized_) { - dls_solver_->setJointLimitAvoidance(limit_avoidance_enabled_, - limit_avoidance_gain_, - limit_avoidance_margin_ratio_, - limit_avoidance_max_push_); + if (auto dls_solver = std::dynamic_pointer_cast(solver_)) { + dls_solver->setJointLimitAvoidance(limit_avoidance_enabled_, + limit_avoidance_gain_, + limit_avoidance_margin_ratio_, + limit_avoidance_max_push_); + } has_joint_position_limits_ = - dls_solver_->getJointPositionLimits(q_lower_limits_, q_upper_limits_); + solver_->getJointPositionLimits(q_lower_limits_, q_upper_limits_); } else { has_joint_position_limits_ = false; q_lower_limits_.resize(0); @@ -209,11 +206,11 @@ bool IbvsController::computeQdot(const std::vector& joints_angle, } bool IbvsController::getChainJointNames(std::vector& joint_names) const { - if (!dls_solver_) { + if (!solver_) { joint_names.clear(); return false; } - return dls_solver_->getChainJointNames(joint_names); + return solver_->getChainJointNames(joint_names); } bool IbvsController::computeInternal(const std::vector& joints_angle, @@ -225,7 +222,7 @@ bool IbvsController::computeInternal(const std::vector& joints_angle, last_tag_pos_visp_.setZero(); last_v_camera_visp_.setZero(); - if (!initialized_ || !dls_solver_) { + if (!initialized_ || !solver_) { last_compute_status_ = ComputeStatus::NOT_READY; return false; } @@ -396,28 +393,56 @@ bool IbvsController::computeInternal(const std::vector& joints_angle, twist_urdf.head<3>() = R_camera_urdf_ * twist_cam.head<3>(); twist_urdf.tail<3>() = R_camera_urdf_ * twist_cam.tail<3>(); - // IK - dls_solver_->update_joints_state(joints_angle); + Eigen::MatrixXd jacobian_base; + Eigen::Matrix3d base_R_camera = Eigen::Matrix3d::Identity(); + if (!solver_->computeJacobianBaseAtQ(joints_angle, + camera_frame_name_, + jacobian_base, + base_R_camera)) { + last_compute_status_ = ComputeStatus::IK_FAILED; + return false; + } + + Eigen::Matrix twist_base; + twist_base.head<3>() = base_R_camera * twist_urdf.head<3>(); + twist_base.tail<3>() = base_R_camera * twist_urdf.tail<3>(); std::vector qdot; - const bool ok = dls_solver_->ik( - base_frame_name_, camera_frame_name_, twist_urdf, - qdot, mu_, std::numeric_limits::infinity()); + const bool ok = solver_->solveVelocityBase(jacobian_base, + twist_base, + joints_angle, + qdot, + std::numeric_limits::infinity()); if (!ok || qdot.size() != joints_angle.size()) { last_compute_status_ = ComputeStatus::IK_FAILED; return false; } - Eigen::Map q_chain(joints_angle.data(), static_cast(joints_angle.size())); - Eigen::Map qdot_vec(qdot.data(), static_cast(qdot.size())); - const Eigen::VectorXd qdot_soft_limited = dls_solver_->applyJointSoftLimitVelocity(q_chain, qdot_vec); - qdot_out.resize(qdot.size()); for (size_t i = 0; i < qdot.size(); ++i) { - qdot_out[i] = SupportFunctions::clamp(qdot_soft_limited[static_cast(i)], - -qdot_max_, - qdot_max_); + qdot_out[i] = SupportFunctions::clamp(qdot[i], -qdot_max_, qdot_max_); + + if (!has_joint_position_limits_ || + i >= static_cast(q_lower_limits_.size()) || + i >= static_cast(q_upper_limits_.size())) { + continue; + } + + const double lower = q_lower_limits_[static_cast(i)]; + const double upper = q_upper_limits_[static_cast(i)]; + if (!std::isfinite(lower) || !std::isfinite(upper) || upper <= lower) { + continue; + } + + const double span = upper - lower; + const double margin = std::max(0.02, 0.08 * span); + const double q = joints_angle[i]; + if (qdot_out[i] < 0.0 && q < lower + margin) { + qdot_out[i] *= SupportFunctions::clamp((q - lower) / margin, 0.0, 1.0); + } else if (qdot_out[i] > 0.0 && q > upper - margin) { + qdot_out[i] *= SupportFunctions::clamp((upper - q) / margin, 0.0, 1.0); + } } last_compute_status_ = ComputeStatus::OK; @@ -539,11 +564,11 @@ void IbvsController::setJointLimitAvoidance(bool enable, limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49); limit_avoidance_max_push_ = max_push; - if (dls_solver_) { - dls_solver_->setJointLimitAvoidance(limit_avoidance_enabled_, - limit_avoidance_gain_, - limit_avoidance_margin_ratio_, - limit_avoidance_max_push_); + if (auto dls_solver = std::dynamic_pointer_cast(solver_)) { + dls_solver->setJointLimitAvoidance(limit_avoidance_enabled_, + limit_avoidance_gain_, + limit_avoidance_margin_ratio_, + limit_avoidance_max_push_); } } diff --git a/cmvr-es/controller/include/pid_controller.h b/cmvr-es/algorithms/controllers/pid/include/pid_controller.h similarity index 100% rename from cmvr-es/controller/include/pid_controller.h rename to cmvr-es/algorithms/controllers/pid/include/pid_controller.h diff --git a/cmvr-es/controller/src/pid_controller.cpp b/cmvr-es/algorithms/controllers/pid/src/pid_controller.cpp similarity index 97% rename from cmvr-es/controller/src/pid_controller.cpp rename to cmvr-es/algorithms/controllers/pid/src/pid_controller.cpp index eb0cc0b0..01270f46 100644 --- a/cmvr-es/controller/src/pid_controller.cpp +++ b/cmvr-es/algorithms/controllers/pid/src/pid_controller.cpp @@ -2,7 +2,7 @@ // Created by lgv on 11/27/25. // -#include "controller/include/pid_controller.h" +#include "algorithms/controllers/pid/include/pid_controller.h" #include namespace cmvr { diff --git a/cmvr-es/controller/src/controller_test.cpp b/cmvr-es/algorithms/controllers/tests/src/controller_test.cpp similarity index 96% rename from cmvr-es/controller/src/controller_test.cpp rename to cmvr-es/algorithms/controllers/tests/src/controller_test.cpp index 61d58dd9..e3ea8010 100644 --- a/cmvr-es/controller/src/controller_test.cpp +++ b/cmvr-es/algorithms/controllers/tests/src/controller_test.cpp @@ -16,9 +16,9 @@ #include -#include "controller/include/ibvs_controller.h" +#include "algorithms/controllers/ibvs/include/ibvs_controller.h" #include "devices/camera/mujoco_camera/include/mujoco_camera.h" -#include "ik_solver/include/pinocchio_dls_ik_solver.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" using namespace cmvr; @@ -551,7 +551,23 @@ protected: ibvs_controller_->setAlignCameraToVisp(R_align); ibvs_controller_->setAlignCameraToUrdf(R_align); - if (!ibvs_controller_->init(urdf_path_for_check_, "PELVIS_S", "R_WRIST_R_S", camera_frame_name_for_check_)) { + config::PinocchioDlsIKConfig dls_cfg; + dls_cfg.set_urdf_path(urdf_path_for_check_); + dls_cfg.set_base_frame_name("PELVIS_S"); + dls_cfg.set_flange_frame_name("R_WRIST_R_S"); + dls_cfg.set_tcp_frame_name(camera_frame_name_for_check_); + dls_cfg.set_max_iters(100); + dls_cfg.set_pos_eps(1e-6); + dls_cfg.set_rot_eps(1e-6); + dls_cfg.set_damping(mu_); + auto solver = std::make_shared(dls_cfg); + if (!solver->init()) { + std::cout << "[IBVS] PinocchioDlsIKSolver init failed" << std::endl; + ready_ = false; + return; + } + + if (!ibvs_controller_->init(solver, camera_frame_name_for_check_)) { std::cout << "[IBVS] IbvsController init failed" << std::endl; ready_ = false; return; diff --git a/cmvr-es/ik_solver/CMakeLists.txt b/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt similarity index 53% rename from cmvr-es/ik_solver/CMakeLists.txt rename to cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt index 14c85883..246aa264 100644 --- a/cmvr-es/ik_solver/CMakeLists.txt +++ b/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt @@ -1,15 +1,14 @@ add_library(ik_solver SHARED - ${CMAKE_CURRENT_SOURCE_DIR}/src/ik_solver.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/ik_solver_creator.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/urdf_parser.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/pinocchio_ik_base.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/pinocchio_dls_ik_solver.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/pinocchio_qp_ik_solver.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/bias_srs_ik_slover.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/joints_limit_analyzer.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/lawba_ik_solver.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/opt_psi_selector.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/common/src/ik_solver.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/common/src/urdf_parser.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/pinocchio/src/pinocchio_ik_base.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/pinocchio/src/pinocchio_dls_ik_solver.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/pinocchio/src/pinocchio_qp_ik_solver.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/srs/src/srs_ik_solver.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/lawba/src/joints_limit_analyzer.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/lawba/src/lawba_ik_solver.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/lawba/src/opt_psi_selector.cpp ) target_include_directories(ik_solver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) @@ -20,7 +19,7 @@ target_link_libraries(ik_solver PUBLIC pinocchio_default pinocchio_parsers pinocchio_collision - cmvr_es::planner + cmvr_es::base_motion cmvr_es::common ) @@ -32,15 +31,15 @@ install(TARGETS ik_solver LIBRARY DESTINATION lib) # -------------------------------------------------------- add_executable(srs_ik_test - ${CMAKE_CURRENT_SOURCE_DIR}/src/srs_ik_test.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/tests/src/srs_ik_test.cpp ) target_link_libraries(srs_ik_test PRIVATE - cmvr_es::utils cmvr_es::ik_solver - cmvr_es::planner + cmvr_es::base_motion + cmvr_es::arm_motion cmvr_es::proto cmvr_es::mujoco_viewer gtest @@ -54,7 +53,7 @@ target_link_libraries(srs_ik_test add_executable(ik_test - ${CMAKE_CURRENT_SOURCE_DIR}/src/ik_test.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/tests/src/ik_test.cpp ) target_link_libraries(ik_test diff --git a/cmvr-es/ik_solver/include/ik_solver.h b/cmvr-es/algorithms/kinematics/ik_solver/common/include/ik_solver.h similarity index 98% rename from cmvr-es/ik_solver/include/ik_solver.h rename to cmvr-es/algorithms/kinematics/ik_solver/common/include/ik_solver.h index 27cd5e4a..bc6b0660 100644 --- a/cmvr-es/ik_solver/include/ik_solver.h +++ b/cmvr-es/algorithms/kinematics/ik_solver/common/include/ik_solver.h @@ -3,8 +3,7 @@ // #pragma once -#include -#include "ik_solver.h" +#include #include #include #include diff --git a/cmvr-es/ik_solver/include/urdf_parser.h b/cmvr-es/algorithms/kinematics/ik_solver/common/include/urdf_parser.h similarity index 100% rename from cmvr-es/ik_solver/include/urdf_parser.h rename to cmvr-es/algorithms/kinematics/ik_solver/common/include/urdf_parser.h diff --git a/cmvr-es/ik_solver/src/ik_solver.cpp b/cmvr-es/algorithms/kinematics/ik_solver/common/src/ik_solver.cpp similarity index 85% rename from cmvr-es/ik_solver/src/ik_solver.cpp rename to cmvr-es/algorithms/kinematics/ik_solver/common/src/ik_solver.cpp index 5d3ec41b..35918db2 100644 --- a/cmvr-es/ik_solver/src/ik_solver.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/common/src/ik_solver.cpp @@ -2,10 +2,9 @@ // Created by Codex on 2026/3/2. // -#include "ik_solver/include/ik_solver.h" -#include "ik_solver/include/urdf_parser.h" - -#include +#include "algorithms/kinematics/ik_solver/common/include/ik_solver.h" +#include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h" +#include "common/base/logging/logger.h" namespace cmvr { @@ -31,7 +30,7 @@ bool IKSolver::initUrdfChain(const std::string& urdf_path, auto parser = std::make_shared(); std::string err; if (!parser->loadModel(urdf_path, &err)) { - std::cerr << "[IKSolver] Failed to load URDF: " << err << "\n"; + CMVR_LOG(ERROR) << "[IKSolver] Failed to load URDF: " << err; return false; } return initUrdfChain(parser, base_frame_name, tip_frame_name); @@ -41,18 +40,18 @@ bool IKSolver::initUrdfChain(const std::shared_ptr& parser, const std::string& base_frame_name, const std::string& tip_frame_name) { if (!parser) { - std::cerr << "[IKSolver] initUrdfChain failed: parser is null\n"; + CMVR_LOG(ERROR) << "[IKSolver] initUrdfChain failed: parser is null"; return false; } if (!parser->loaded()) { - std::cerr << "[IKSolver] initUrdfChain failed: parser model not loaded\n"; + CMVR_LOG(ERROR) << "[IKSolver] initUrdfChain failed: parser model not loaded"; return false; } UrdfParser::ChainInfo chain_info; std::string err; if (!parser->extractChain(base_frame_name, tip_frame_name, chain_info, &err)) { - std::cerr << "[IKSolver] Failed to extract chain: " << err << "\n"; + CMVR_LOG(ERROR) << "[IKSolver] Failed to extract chain: " << err; return false; } diff --git a/cmvr-es/ik_solver/src/urdf_parser.cpp b/cmvr-es/algorithms/kinematics/ik_solver/common/src/urdf_parser.cpp similarity index 98% rename from cmvr-es/ik_solver/src/urdf_parser.cpp rename to cmvr-es/algorithms/kinematics/ik_solver/common/src/urdf_parser.cpp index bc87c104..41a3b494 100644 --- a/cmvr-es/ik_solver/src/urdf_parser.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/common/src/urdf_parser.cpp @@ -1,6 +1,6 @@ // Created by Codex on 2026/3/2. -#include "ik_solver/include/urdf_parser.h" +#include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h" #include diff --git a/cmvr-es/algorithms/kinematics/ik_solver/ik_solver_factory.h b/cmvr-es/algorithms/kinematics/ik_solver/ik_solver_factory.h new file mode 100644 index 00000000..58e430c5 --- /dev/null +++ b/cmvr-es/algorithms/kinematics/ik_solver/ik_solver_factory.h @@ -0,0 +1,44 @@ +#pragma once + +#include +#include + +#include "algorithms/kinematics/ik_solver/common/include/ik_solver.h" +#include "algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h" +#include "algorithms/kinematics/ik_solver/srs/include/srs_ik_solver.h" +#include "cmvr/config/arm_config/arm_config.pb.h" +#include "common/base/logging/logger.h" +#include "common/config/config_files.h" + +namespace cmvr { + +class IKSolverFactory { +public: + static std::shared_ptr create(const config::ArmKinematicsConfig& cfg) + { + switch (cfg.algorithm_case()) { + case config::ArmKinematicsConfig::kPinocchioDlsIkSolver: { + auto solver_cfg = cfg.pinocchio_dls_ik_solver(); + solver_cfg.set_urdf_path(ConfigHelper::resolveResourceFile(solver_cfg.urdf_path())); + return std::make_shared(solver_cfg); + } + case config::ArmKinematicsConfig::kPinocchioQpIkSolver: { + auto solver_cfg = cfg.pinocchio_qp_ik_solver(); + solver_cfg.set_urdf_path(ConfigHelper::resolveResourceFile(solver_cfg.urdf_path())); + return std::make_shared(solver_cfg); + } + case config::ArmKinematicsConfig::kSrsIkSolver: + return std::make_shared(cfg.srs_ik_solver()); + case config::ArmKinematicsConfig::kLawbaIkSolver: + return std::make_shared(cfg.lawba_ik_solver()); + case config::ArmKinematicsConfig::ALGORITHM_NOT_SET: + default: + CMVR_LOG(ERROR) << "[IKSolverFactory] missing arm kinematics algorithm config"; + return nullptr; + } + } +}; + +} // namespace cmvr diff --git a/cmvr-es/ik_solver/include/joints_limit_analyzer.h b/cmvr-es/algorithms/kinematics/ik_solver/lawba/include/joints_limit_analyzer.h similarity index 98% rename from cmvr-es/ik_solver/include/joints_limit_analyzer.h rename to cmvr-es/algorithms/kinematics/ik_solver/lawba/include/joints_limit_analyzer.h index 090003cc..4731fc92 100644 --- a/cmvr-es/ik_solver/include/joints_limit_analyzer.h +++ b/cmvr-es/algorithms/kinematics/ik_solver/lawba/include/joints_limit_analyzer.h @@ -7,7 +7,7 @@ #include #include -#include "common/math/include/support_functions.h" +#include "common/math/support_functions.h" namespace cmvr { class JointsLimitAnalyzer { diff --git a/cmvr-es/ik_solver/include/lawba_ik_solver.h b/cmvr-es/algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.h similarity index 79% rename from cmvr-es/ik_solver/include/lawba_ik_solver.h rename to cmvr-es/algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.h index 6fa3431a..c59bcca4 100644 --- a/cmvr-es/ik_solver/include/lawba_ik_solver.h +++ b/cmvr-es/algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.h @@ -1,8 +1,8 @@ // // Created by lgv on 11/7/25. // -#ifndef CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H -#define CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H +#ifndef CMVR_ES_LAWBA_IK_SOLVER_H +#define CMVR_ES_LAWBA_IK_SOLVER_H #include #include @@ -10,17 +10,17 @@ #include -#include "cmvr/msgs/can_card_parameter.grpc.pb.h" -#include "ik_solver/include/bias_srs_ik_slover.h" -#include "ik_solver/include/joints_limit_analyzer.h" -#include "ik_solver/include/opt_psi_selector.h" -#include "ik_solver/include/ik_solver.h" +#include "cmvr/config/lawba_ik_config.pb.h" +#include "algorithms/kinematics/ik_solver/lawba/include/joints_limit_analyzer.h" +#include "algorithms/kinematics/ik_solver/lawba/include/opt_psi_selector.h" +#include "algorithms/kinematics/ik_solver/srs/include/srs_ik_solver.h" +#include "algorithms/kinematics/ik_solver/common/include/ik_solver.h" namespace cmvr { class LawbaIKSolver : public IKSolver { public: - LawbaIKSolver(); + explicit LawbaIKSolver(const config::LawbaIKConfig& cfg); ~LawbaIKSolver() override = default; bool init() override; @@ -42,6 +42,8 @@ public: } private: + config::LawbaIKConfig config_; + // 机械臂 URDF 中末端工具坐标系相对于 MDH 中末端法兰的变换矩阵。 Eigen::Matrix4d T_tool_flange_{Eigen::Matrix4d::Identity()}; @@ -51,7 +53,7 @@ private: // 机械臂 URDF 中法兰姿态相对于 MDH 中法兰姿态的变换矩阵。 Eigen::Matrix4d T_flange_urdf_mdh_{Eigen::Matrix4d::Identity()}; - std::shared_ptr bias_srs_ik_solver_{nullptr}; + std::shared_ptr srs_ik_solver_{nullptr}; std::shared_ptr joints_limit_analyzer_{nullptr}; std::shared_ptr opt_psi_selector_{nullptr}; @@ -88,14 +90,14 @@ private: double &cost_out); // ConfigDirection ↔ 符号 - static inline int sign_from_dir(BiasSRSIkSolver::ConfigDirection d) { - return (d == BiasSRSIkSolver::ConfigDirection::OUTWARD) ? +1 : -1; + static inline int sign_from_dir(SrsIKSolver::ConfigDirection d) { + return (d == SrsIKSolver::ConfigDirection::OUTWARD) ? +1 : -1; } - static inline BiasSRSIkSolver::ConfigDirection dir_from_sign(int s) { + static inline SrsIKSolver::ConfigDirection dir_from_sign(int s) { return (s >= 0) - ? BiasSRSIkSolver::ConfigDirection::OUTWARD - : BiasSRSIkSolver::ConfigDirection::INWARD; + ? SrsIKSolver::ConfigDirection::OUTWARD + : SrsIKSolver::ConfigDirection::INWARD; } // 符号 (-1/+1) → 索引 (1/0) @@ -115,4 +117,4 @@ private: } // namespace cmvr -#endif // CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H +#endif // CMVR_ES_LAWBA_IK_SOLVER_H diff --git a/cmvr-es/ik_solver/include/opt_psi_selector.h b/cmvr-es/algorithms/kinematics/ik_solver/lawba/include/opt_psi_selector.h similarity index 95% rename from cmvr-es/ik_solver/include/opt_psi_selector.h rename to cmvr-es/algorithms/kinematics/ik_solver/lawba/include/opt_psi_selector.h index 4df61654..2fe53032 100644 --- a/cmvr-es/ik_solver/include/opt_psi_selector.h +++ b/cmvr-es/algorithms/kinematics/ik_solver/lawba/include/opt_psi_selector.h @@ -7,7 +7,7 @@ #include #include -#include "common/math/include/support_functions.h" +#include "common/math/support_functions.h" namespace cmvr { class OptPsiSelector { diff --git a/cmvr-es/ik_solver/src/joints_limit_analyzer.cpp b/cmvr-es/algorithms/kinematics/ik_solver/lawba/src/joints_limit_analyzer.cpp similarity index 99% rename from cmvr-es/ik_solver/src/joints_limit_analyzer.cpp rename to cmvr-es/algorithms/kinematics/ik_solver/lawba/src/joints_limit_analyzer.cpp index 4c3579cb..697c8ce2 100644 --- a/cmvr-es/ik_solver/src/joints_limit_analyzer.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/lawba/src/joints_limit_analyzer.cpp @@ -2,7 +2,7 @@ // Created by lgv on 2025/11/3. // -#include "ik_solver/include/joints_limit_analyzer.h" +#include "algorithms/kinematics/ik_solver/lawba/include/joints_limit_analyzer.h" #include #include diff --git a/cmvr-es/ik_solver/src/lawba_ik_solver.cpp b/cmvr-es/algorithms/kinematics/ik_solver/lawba/src/lawba_ik_solver.cpp similarity index 74% rename from cmvr-es/ik_solver/src/lawba_ik_solver.cpp rename to cmvr-es/algorithms/kinematics/ik_solver/lawba/src/lawba_ik_solver.cpp index 83b2f37d..65b57a34 100644 --- a/cmvr-es/ik_solver/src/lawba_ik_solver.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/lawba/src/lawba_ik_solver.cpp @@ -1,14 +1,17 @@ // // Created by lgv on 11/7/25. // -#include "ik_solver/include/lawba_ik_solver.h" +#include "algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.h" -#include "common/math/include/support_functions.h" +#include "common/math/support_functions.h" using namespace cmvr; -LawbaIKSolver::LawbaIKSolver() : IKSolver("", "", "") { - bias_srs_ik_solver_ = std::make_shared(); +LawbaIKSolver::LawbaIKSolver(const config::LawbaIKConfig& cfg) + : IKSolver("", "", "") + , config_(cfg) +{ + srs_ik_solver_ = std::make_shared(config_.srs_config()); joints_limit_analyzer_ = std::make_shared(); opt_psi_selector_ = std::make_shared(); @@ -16,6 +19,10 @@ LawbaIKSolver::LawbaIKSolver() : IKSolver("", "", "") { } bool LawbaIKSolver::init() { + if (!srs_ik_solver_ || !srs_ik_solver_->init()) { + return false; + } + Eigen::Matrix4d T_tool_flange, T_arm_robot; T_tool_flange << 0, 1, 0, -0.284077, @@ -36,14 +43,23 @@ bool LawbaIKSolver::init() { T_tool_flange_ = T_tool_flange; T_arm_robot_ = T_arm_robot; - // 臂角更新参数 - opt_psi_selector_->set_update_params(0.6, 5.0, -1, 1e-4); + const double opt_psi_update_alpha = + config_.opt_psi_update_alpha() > 0.0 ? config_.opt_psi_update_alpha() : 0.6; + const double opt_psi_max_delta = + config_.opt_psi_max_delta() > 0.0 ? config_.opt_psi_max_delta() : 5.0; + const double opt_psi_min = + config_.opt_psi_min() != 0.0 ? config_.opt_psi_min() : -1.0; + const double opt_psi_eps = + config_.opt_psi_eps() > 0.0 ? config_.opt_psi_eps() : 1e-4; + opt_psi_selector_->set_update_params(opt_psi_update_alpha, + opt_psi_max_delta, + opt_psi_min, + opt_psi_eps); - // 代价参数（可之后再通过 set_cost_params 调整） - set_cost_params( - 5.5, // lambda_q_ - 1e-3 // cur_branch_cost_threshold_ - ); + set_cost_params(config_.lambda_q() > 0.0 ? config_.lambda_q() : 5.5, + config_.cur_branch_cost_threshold() > 0.0 + ? config_.cur_branch_cost_threshold() + : 1e-3); return true; } @@ -51,7 +67,7 @@ bool LawbaIKSolver::init() { bool LawbaIKSolver::fk(const std::vector &joints_angle, Eigen::Matrix4d &cur_pose, bool is_tcp) { - cur_pose = T_arm_robot_ * bias_srs_ik_solver_->calc_total_transform(joints_angle); + cur_pose = T_arm_robot_ * srs_ik_solver_->calc_total_transform(joints_angle); if (is_tcp) { cur_pose = cur_pose * T_tool_flange_; } else { @@ -67,10 +83,10 @@ bool LawbaIKSolver::estimate_state_from_current_joints() { // 1) 当前位姿 & 系数矩阵 Eigen::Matrix4d cur_pose = - bias_srs_ik_solver_->calc_total_transform(cur_joints_angle_); + srs_ik_solver_->calc_total_transform(cur_joints_angle_); Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9); - bias_srs_ik_solver_->cal_coefficient_matrix(cur_pose, s_mat, w_mat); + srs_ik_solver_->cal_coefficient_matrix(cur_pose, s_mat, w_mat); // 2) 清空所有分支的缓存 for (int i = 0; i < 2; ++i) @@ -83,14 +99,14 @@ bool LawbaIKSolver::estimate_state_from_current_joints() { double best_score = std::numeric_limits::infinity(); // 3) 枚举所有 (s,e,w) 分支组合 - BiasSRSIkSolver::ConfigDirection dirs[2] = { - BiasSRSIkSolver::ConfigDirection::OUTWARD, - BiasSRSIkSolver::ConfigDirection::INWARD + SrsIKSolver::ConfigDirection dirs[2] = { + SrsIKSolver::ConfigDirection::OUTWARD, + SrsIKSolver::ConfigDirection::INWARD }; - for (BiasSRSIkSolver::ConfigDirection s_dir: dirs) { - for (BiasSRSIkSolver::ConfigDirection e_dir: dirs) { - for (BiasSRSIkSolver::ConfigDirection w_dir: dirs) { + for (SrsIKSolver::ConfigDirection s_dir: dirs) { + for (SrsIKSolver::ConfigDirection e_dir: dirs) { + for (SrsIKSolver::ConfigDirection w_dir: dirs) { int s = sign_from_dir(s_dir); int e = sign_from_dir(e_dir); int w = sign_from_dir(w_dir); @@ -131,10 +147,9 @@ bool LawbaIKSolver::estimate_state_from_current_joints() { return false; } - // 4) 把求得的“最佳分支”应用到 bias_srs_ik_solver_，保持一致 - bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(best.s_conf)); - bias_srs_ik_solver_->set_elbow_config(dir_from_sign(best.e_conf)); - bias_srs_ik_solver_->set_wrist_config(dir_from_sign(best.w_conf)); + srs_ik_solver_->set_shoulder_config(dir_from_sign(best.s_conf)); + srs_ik_solver_->set_elbow_config(dir_from_sign(best.e_conf)); + srs_ik_solver_->set_wrist_config(dir_from_sign(best.w_conf)); branch_state_ = best; return true; @@ -147,7 +162,7 @@ bool LawbaIKSolver::solve_on_branch(const Eigen::Matrix4d &target_cal_pose, std::vector &q_out, double &psi_out, double &cost_out) { - auto joints_limits = bias_srs_ik_solver_->get_joints_limits(); + auto joints_limits = srs_ik_solver_->get_joints_limits(); // 1) 这一分支下的 ψ 可行区间 auto limits = joints_limit_analyzer_->calc_arm_angle_limits( @@ -167,14 +182,13 @@ bool LawbaIKSolver::solve_on_branch(const Eigen::Matrix4d &target_cal_pose, return false; } - // 3) 把 bias_srs_ik_solver_ 的分支设置为当前 branch - bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(branch.s_conf)); - bias_srs_ik_solver_->set_elbow_config(dir_from_sign(branch.e_conf)); - bias_srs_ik_solver_->set_wrist_config(dir_from_sign(branch.w_conf)); + srs_ik_solver_->set_shoulder_config(dir_from_sign(branch.s_conf)); + srs_ik_solver_->set_elbow_config(dir_from_sign(branch.e_conf)); + srs_ik_solver_->set_wrist_config(dir_from_sign(branch.w_conf)); // 4) 解析 IK std::vector q; - if (!bias_srs_ik_solver_->inverse_kinematics(target_cal_pose, q, best_psi)) { + if (!srs_ik_solver_->ikWithPsi(target_cal_pose, q, best_psi)) { return false; } @@ -217,17 +231,17 @@ bool LawbaIKSolver::ik(const Eigen::Matrix4d &target_pose, // --- 1) 校验当前缓存的 branch_state_ 是否仍然对应 cur_joints_angle_ --- if (branch_state_.valid) { Eigen::Matrix4d cur_pose = - bias_srs_ik_solver_->calc_total_transform(cur_joints_angle_); + srs_ik_solver_->calc_total_transform(cur_joints_angle_); // 确保 solver 内部分支与 branch_state_ 一致 - bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(branch_state_.s_conf)); - bias_srs_ik_solver_->set_elbow_config(dir_from_sign(branch_state_.e_conf)); - bias_srs_ik_solver_->set_wrist_config(dir_from_sign(branch_state_.w_conf)); + srs_ik_solver_->set_shoulder_config(dir_from_sign(branch_state_.s_conf)); + srs_ik_solver_->set_elbow_config(dir_from_sign(branch_state_.e_conf)); + srs_ik_solver_->set_wrist_config(dir_from_sign(branch_state_.w_conf)); std::vector cur_joints_angle; - if (!bias_srs_ik_solver_->inverse_kinematics(cur_pose, cur_joints_angle, - branch_state_.psi)) { + if (!srs_ik_solver_->ikWithPsi(cur_pose, cur_joints_angle, + branch_state_.psi)) { branch_state_.valid = false; } else { for (int i = 0; i < static_cast(cur_joints_angle_.size()); ++i) { @@ -248,7 +262,7 @@ bool LawbaIKSolver::ik(const Eigen::Matrix4d &target_pose, // --- 3) 只算一次 target 位姿的系数矩阵 --- Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9); - bias_srs_ik_solver_->cal_coefficient_matrix(target_cal_pose, s_mat, w_mat); + srs_ik_solver_->cal_coefficient_matrix(target_cal_pose, s_mat, w_mat); // --- 4) 先在“当前分支”上试一次 --- std::vector q_cur; @@ -284,14 +298,14 @@ bool LawbaIKSolver::ik(const Eigen::Matrix4d &target_pose, best_state.valid = true; } - BiasSRSIkSolver::ConfigDirection dirs[2] = { - BiasSRSIkSolver::ConfigDirection::OUTWARD, - BiasSRSIkSolver::ConfigDirection::INWARD + SrsIKSolver::ConfigDirection dirs[2] = { + SrsIKSolver::ConfigDirection::OUTWARD, + SrsIKSolver::ConfigDirection::INWARD }; - for (BiasSRSIkSolver::ConfigDirection s_dir: dirs) { - for (BiasSRSIkSolver::ConfigDirection e_dir: dirs) { - for (BiasSRSIkSolver::ConfigDirection w_dir: dirs) { + for (SrsIKSolver::ConfigDirection s_dir: dirs) { + for (SrsIKSolver::ConfigDirection e_dir: dirs) { + for (SrsIKSolver::ConfigDirection w_dir: dirs) { int s = sign_from_dir(s_dir); int e = sign_from_dir(e_dir); int w = sign_from_dir(w_dir); @@ -353,9 +367,9 @@ bool LawbaIKSolver::ik(const Eigen::Matrix4d &target_pose, // --- 6) 用最佳分支 + ψ 更新状态并返回 --- branch_state_ = best_state; - bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(branch_state_.s_conf)); - bias_srs_ik_solver_->set_elbow_config(dir_from_sign(branch_state_.e_conf)); - bias_srs_ik_solver_->set_wrist_config(dir_from_sign(branch_state_.w_conf)); + srs_ik_solver_->set_shoulder_config(dir_from_sign(branch_state_.s_conf)); + srs_ik_solver_->set_elbow_config(dir_from_sign(branch_state_.e_conf)); + srs_ik_solver_->set_wrist_config(dir_from_sign(branch_state_.w_conf)); joints_angle = best_q; cur_joints_angle_ = joints_angle; // 作为下一次的“当前姿态” diff --git a/cmvr-es/ik_solver/src/opt_psi_selector.cpp b/cmvr-es/algorithms/kinematics/ik_solver/lawba/src/opt_psi_selector.cpp similarity index 98% rename from cmvr-es/ik_solver/src/opt_psi_selector.cpp rename to cmvr-es/algorithms/kinematics/ik_solver/lawba/src/opt_psi_selector.cpp index c4fda615..d4385ecb 100644 --- a/cmvr-es/ik_solver/src/opt_psi_selector.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/lawba/src/opt_psi_selector.cpp @@ -2,7 +2,7 @@ // Created by lgv on 11/7/25. // -#include "ik_solver/include/opt_psi_selector.h" +#include "algorithms/kinematics/ik_solver/lawba/include/opt_psi_selector.h" using namespace cmvr; diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h new file mode 100644 index 00000000..18fe94f8 --- /dev/null +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h @@ -0,0 +1,95 @@ +#pragma once + +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" + + +#include +#include +#include +#include +#include + +#include "cmvr/config/pinocchio_dls_ik_config.pb.h" + +namespace cmvr { + +/** + * @brief 基于 Pinocchio 的 DLS 逆运动学求解器。 + * + * 提供以下能力： + * - 位姿 IK（`ik`） + * - 正运动学 FK（`fk`，继承自基类） + * - 末端 twist 到关节速度的微分 IK（`ik` 重载） + * + * 求解链路由 URDF 中 `base_frame_name -> flange_frame_name` 自动提取， + * 并支持可选 TCP frame。 + */ +class PinocchioDlsIKSolver : public PinocchioIKBase { +public: + explicit PinocchioDlsIKSolver(const config::PinocchioDlsIKConfig& cfg); + + ~PinocchioDlsIKSolver() override = default; + + bool init() override; + + bool ik(const Eigen::Matrix4d &target_pose, + std::vector &joints_angle, + bool is_tcp = true) override; + + bool ik(const std::string& base_link, + const std::string& ee_link, + const Eigen::Matrix4d& target_pose, + std::vector& joints_angle); + + bool ik(const std::string& base_link, + const std::string& ee_link, + const Eigen::Matrix& target_vel, + std::vector& joints_vel, + double damping = -1.0, + double qdot_abs_max = std::numeric_limits::infinity()); + + bool solveVelocityBase(const Eigen::MatrixXd& jacobian_base, + const Eigen::Matrix& target_twist_base, + const std::vector& q_chain, + std::vector& qdot_out, + double qdot_abs_max = std::numeric_limits::infinity()) const override; + + void setJointLimitAvoidance(bool enable, + double gain = 0.2, + double margin_ratio = 0.15, + double max_push = 0.25); + + void setMaxIters(int iters) { max_iters_ = iters; } + void setDamping(double d) { damping_ = d; } + void setEps(double pos_eps, double rot_eps) { pos_eps_ = pos_eps; rot_eps_ = rot_eps; } + + double damping() const { return damping_; } + +private: + Eigen::MatrixXd dampedPseudoInverse(const Eigen::MatrixXd &J, double lambda); + + bool refreshJointLimits_(const config::PinocchioDlsIKConfig& cfg); + + Eigen::VectorXd computeJointLimitAvoidanceVelocity(const Eigen::VectorXd& q_chain) const; + + Eigen::VectorXd projectToNullspace(const Eigen::MatrixXd& J_pinv, + const Eigen::MatrixXd& J, + const Eigen::VectorXd& secondary) const; + +private: + bool limit_avoidance_enabled_{false}; + double limit_avoidance_gain_{0.2}; + double limit_avoidance_margin_ratio_{0.15}; + double limit_avoidance_max_push_{0.25}; + + bool initialized_{false}; + + int max_iters_; + double pos_eps_; + double rot_eps_; + double damping_; + config::PinocchioDlsIKConfig config_; + +}; + +} // namespace cmvr diff --git a/cmvr-es/ik_solver/include/pinocchio_ik_base.h b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h similarity index 59% rename from cmvr-es/ik_solver/include/pinocchio_ik_base.h rename to cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h index c783f783..68b6e54c 100644 --- a/cmvr-es/ik_solver/include/pinocchio_ik_base.h +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h @@ -1,8 +1,8 @@ // Created by Codex on 2026/3/3. #pragma once -#include "ik_solver/include/ik_solver.h" -#include "ik_solver/include/urdf_parser.h" +#include "algorithms/kinematics/ik_solver/common/include/ik_solver.h" +#include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h" #include #include @@ -11,6 +11,7 @@ #include #include +#include #include namespace cmvr { @@ -70,6 +71,39 @@ public: const std::vector& joints_angle, Eigen::Matrix4d& cur_pose); + int chainDof() const { return chain_q_dof_; } + int chainVelocityDof() const { return chain_v_dof_; } + + bool computeJacobianBaseAtQ(const std::vector& q_chain, + bool is_tcp, + Eigen::MatrixXd& jacobian_base, + Eigen::Matrix3d& base_R_ee); + + bool computeJacobianBaseAtQ(const std::vector& q_chain, + const std::string& ee_frame_name, + Eigen::MatrixXd& jacobian_base, + Eigen::Matrix3d& base_R_ee); + + bool computeTwistBaseAtQ(const std::vector& q_chain, + const std::vector& qdot_chain, + bool is_tcp, + Eigen::Matrix& twist_base, + Eigen::MatrixXd* jacobian_base_out = nullptr, + Eigen::Matrix3d* base_R_ee_out = nullptr); + + bool computeTwistBaseAtQ(const std::vector& q_chain, + const std::vector& qdot_chain, + const std::string& ee_frame_name, + Eigen::Matrix& twist_base, + Eigen::MatrixXd* jacobian_base_out = nullptr, + Eigen::Matrix3d* base_R_ee_out = nullptr); + + virtual bool solveVelocityBase(const Eigen::MatrixXd& jacobian_base, + const Eigen::Matrix& target_twist_base, + const std::vector& q_chain, + std::vector& qdot_out, + double qdot_abs_max = std::numeric_limits::infinity()) const; + protected: /** * @brief 从父类缓存的 URDF 链信息初始化 Pinocchio 运行时对象与 frame 索引。 @@ -96,6 +130,29 @@ protected: Eigen::VectorXd& q_full, const char* context) const; + bool buildFullQFromChain(const Eigen::VectorXd& q_chain, + Eigen::VectorXd& q_full, + const char* context) const; + + Eigen::MatrixXd extractChainJacobian( + const Eigen::Matrix& jacobian_full) const; + + const pinocchio::SE3& getBasePoseWorld() const; + + bool buildJacobianBaseAtQ(const std::vector& q_chain, + pinocchio::FrameIndex ee_id, + Eigen::MatrixXd& jacobian_base, + Eigen::Matrix3d& base_R_ee, + Eigen::VectorXd* q_full_out = nullptr); + + bool computeMeasuredTwistBase(const std::vector& q_chain, + const std::vector& qdot_chain, + pinocchio::FrameIndex ee_id, + Eigen::Matrix& twist_base, + Eigen::MatrixXd* jacobian_base_out = nullptr, + Eigen::Matrix3d* base_R_ee_out = nullptr, + Eigen::VectorXd* q_full_out = nullptr); + /** * @brief 在当前 `model_` 上更新 FK 与 frame placement。 */ @@ -129,6 +186,10 @@ protected: /** @brief 是否存在有效 TCP frame。 */ bool has_tcp_{false}; + /** @brief 配置链路 base 在 world 下的中性位姿缓存。 */ + bool base_pose_cached_{false}; + pinocchio::SE3 oM_base_cached_; + /** @brief 当前链在 full-model `q` 中的起始索引。 */ int chain_q_start_{0}; /** @brief 当前链关节位置自由度数量。 */ diff --git a/cmvr-es/ik_solver/include/pinocchio_qp_ik_solver.h b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h similarity index 51% rename from cmvr-es/ik_solver/include/pinocchio_qp_ik_solver.h rename to cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h index ef1f5b2d..471cc679 100644 --- a/cmvr-es/ik_solver/include/pinocchio_qp_ik_solver.h +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h @@ -7,41 +7,22 @@ #pragma once -#include "ik_solver/include/pinocchio_ik_base.h" -#include "common/math/include/qp_solver.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" +#include "common/math/qp_solver.h" #include +#include #include #include #include -#include "cmvr/config/pinocchio_qp_ik_solver_config.pb.h" +#include "cmvr/config/pinocchio_qp_ik_config.pb.h" namespace cmvr { class PinocchioQpIKSolver : public PinocchioIKBase { public: - /// urdf_path : URDF 路径（可以是单臂，也可以是双臂整机） - /// base_frame_name : 作为 IK 基坐标系的 frame 名（例：PELVIS_S） - /// flange_frame_name : 末端法兰 frame 名（例：L_FLANGE） - /// tcp_frame_name : TCP frame 名（可为空；为空则 is_tcp=true 时也用法兰） - /// lambda : 阻尼（cost 中的 √lambda * I） - /// w_posrot : 位置 / 姿态误差权重（0~1，越大越重姿态） - /// max_iters : 外层迭代上限 - /// tol : 误差收敛阈值（||任务误差||） - /// qp_time_limit : 每次 QP 的时间限制（秒） - PinocchioQpIKSolver(const std::string &urdf_path, - const std::string &base_frame_name, - const std::string &flange_frame_name, - const std::string &tcp_frame_name = std::string(), - double lambda = 1e-4, - double w_posrot = 0.5, - int max_iters = 100, - double tol = 1e-6, - double qp_time_limit = 1e-2); - - PinocchioQpIKSolver(); - PinocchioQpIKSolver(const config::PinocchioQpIKConfig &config); + explicit PinocchioQpIKSolver(const config::PinocchioQpIKConfig& config); ~PinocchioQpIKSolver() override = default; @@ -57,6 +38,12 @@ public: std::vector &joints_angle, bool is_tcp = true) override; + bool solveVelocityBase(const Eigen::MatrixXd& jacobian_base, + const Eigen::Matrix& target_twist_base, + const std::vector& q_chain, + std::vector& qdot_out, + double qdot_abs_max = std::numeric_limits::infinity()) const override; + /// 如你有更严格的速度 / 加速度限位，可以覆盖默认值 void setVelocityLimits(const Eigen::VectorXd &qd_max); void setAccelerationLimits(const Eigen::VectorXd &qdd_max); diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp new file mode 100644 index 00000000..45ef0096 --- /dev/null +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp @@ -0,0 +1,473 @@ +// Created by lgv on 11/28/25. + +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" +#include "common/base/logging/logger.h" +#include "common/math/joint_limits.h" +#include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h" +#include "common/config/config_files.h" + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace cmvr { + +using cmvr::common::config::positiveOr; + +Eigen::MatrixXd PinocchioDlsIKSolver::dampedPseudoInverse(const Eigen::MatrixXd &J, double lambda) { + const int m = J.rows(); + const int n = J.cols(); + const double l2 = lambda * lambda; + + if (m <= n) { + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m, m); + Eigen::MatrixXd JJt = J * J.transpose() + l2 * I; + return J.transpose() * JJt.inverse(); + } else { + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(n, n); + Eigen::MatrixXd JtJ = J.transpose() * J + l2 * I; + return JtJ.inverse() * J.transpose(); + } +} + +PinocchioDlsIKSolver::PinocchioDlsIKSolver(const config::PinocchioDlsIKConfig& cfg) + : PinocchioIKBase(cfg.urdf_path(), + cfg.base_frame_name(), + cfg.flange_frame_name(), + cfg.tcp_frame_name()) + , max_iters_(cfg.max_iters() > 0 ? cfg.max_iters() : 100) + , pos_eps_(cfg.pos_eps() > 0.0 ? cfg.pos_eps() : 1e-6) + , rot_eps_(cfg.rot_eps() > 0.0 ? cfg.rot_eps() : 1e-6) + , damping_(cfg.damping() > 0.0 ? cfg.damping() : 1e-4) + , config_(cfg) +{ + if (cfg.has_joint_limit_avoidance()) { + const auto& avoidance = cfg.joint_limit_avoidance(); + setJointLimitAvoidance( + avoidance.enable(), + positiveOr(avoidance.gain(), limit_avoidance_gain_), + positiveOr(avoidance.margin_ratio(), limit_avoidance_margin_ratio_), + positiveOr(avoidance.max_push(), limit_avoidance_max_push_)); + } +} + +bool PinocchioDlsIKSolver::refreshJointLimits_(const config::PinocchioDlsIKConfig& cfg) { + const auto source = cfg.has_joint_limits() + ? cfg.joint_limits().source() + : config::JOINT_LIMIT_SOURCE_URDF; + if (source == config::JOINT_LIMIT_SOURCE_UNKNOWN || + source == config::JOINT_LIMIT_SOURCE_URDF) { + return true; + } + + if (source != config::JOINT_LIMIT_SOURCE_CUSTOM) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] unsupported joint limit source"; + return false; + } + + std::vector joint_names; + if (!getChainJointNames(joint_names) || joint_names.empty()) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] failed to get chain joint names for custom limits"; + return false; + } + + std::unordered_map custom_limits; + if (cfg.has_joint_limits()) { + custom_limits.reserve(static_cast(cfg.joint_limits().joints_size())); + for (const auto& item : cfg.joint_limits().joints()) { + if (!item.joint_name().empty()) { + custom_limits[item.joint_name()] = item; + } + } + } + + const auto dof = static_cast(joint_names.size()); + joint_pos_lower_limits_.resize(dof); + joint_pos_upper_limits_.resize(dof); + joint_vel_limits_.resize(dof); + for (Eigen::Index i = 0; i < dof; ++i) { + const auto& joint_name = joint_names[static_cast(i)]; + const auto it = custom_limits.find(joint_name); + if (it == custom_limits.end()) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] missing custom joint limit for " << joint_name; + return false; + } + const auto& limit = it->second; + if (!std::isfinite(limit.lower()) || !std::isfinite(limit.upper()) || + !std::isfinite(limit.velocity()) || limit.upper() <= limit.lower() || + limit.velocity() <= 0.0) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] invalid custom joint limit for " + << limit.joint_name(); + return false; + } + joint_pos_lower_limits_[i] = limit.lower(); + joint_pos_upper_limits_[i] = limit.upper(); + joint_vel_limits_[i] = std::abs(limit.velocity()); + } + return true; +} + +Eigen::VectorXd PinocchioDlsIKSolver::computeJointLimitAvoidanceVelocity( + const Eigen::VectorXd& q_chain) const { + if (!limit_avoidance_enabled_ || + limit_avoidance_gain_ <= 0.0 || + chain_v_dof_ != chain_q_dof_ || + q_chain.size() != chain_q_dof_ || + joint_pos_lower_limits_.size() != chain_q_dof_ || + joint_pos_upper_limits_.size() != chain_q_dof_) { + return Eigen::VectorXd::Zero(chain_v_dof_); + } + + return cmvr::kinematics::computeJointLimitAvoidanceVelocity( + q_chain, + joint_pos_lower_limits_, + joint_pos_upper_limits_, + limit_avoidance_enabled_, + limit_avoidance_gain_, + limit_avoidance_margin_ratio_, + limit_avoidance_max_push_); +} + +Eigen::VectorXd PinocchioDlsIKSolver::projectToNullspace(const Eigen::MatrixXd& J_pinv, + const Eigen::MatrixXd& J, + const Eigen::VectorXd& secondary) const { + if (secondary.size() != J.cols()) { + return Eigen::VectorXd::Zero(J.cols()); + } + const Eigen::MatrixXd N = + Eigen::MatrixXd::Identity(J.cols(), J.cols()) - J_pinv * J; + return N * secondary; +} + +bool PinocchioDlsIKSolver::init() { + UrdfParser::ChainInfo chain_info; + std::string err; + if (!initPinocchioFromUrdfChain(&chain_info, &err)) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] Failed to init pinocchio base: " << err; + return false; + } + if (!refreshJointLimits_(config_)) { + return false; + } + + { + Eigen::VectorXd q0 = pinocchio::neutral(model_); + updateKinematics(q0); + oM_base_cached_ = data_->oMf[base_frame_id_]; + base_pose_cached_ = true; + } + + cur_joints_angle_.assign(chain_q_dof_, 0.0); + + initialized_ = true; + CMVR_LOG(INFO) << "[PinocchioDlsIKSolver] Chain '" << chain_base_frame_name_ << "' -> '" << chain_tip_frame_name_ + << "': q_start=" << chain_q_start_ << " q_dof=" << chain_q_dof_ + << ", v_start=" << chain_v_start_ << " v_dof=" << chain_v_dof_ + << ", nq=" << model_.nq << " nv=" << model_.nv; + CMVR_LOG(INFO) << "[PinocchioDlsIKSolver] Chain joint position limits (rad):"; + for (const auto& seg : chain_info.joints) { + const int q_idx = seg.q_index; + const int nq = seg.nq; + const std::string& jname = seg.name; + if (nq <= 0) continue; + + for (int k = 0; k < nq; ++k) { + const int qi = q_idx - chain_q_start_ + k; + if (qi < 0 || qi >= chain_q_dof_) continue; + if (nq == 1) { + CMVR_LOG(INFO) << " - " << jname + << ": [" << joint_pos_lower_limits_[qi] << ", " << joint_pos_upper_limits_[qi] << "]"; + } else { + CMVR_LOG(INFO) << " - " << jname << "[" << k << "]" + << ": [" << joint_pos_lower_limits_[qi] << ", " << joint_pos_upper_limits_[qi] << "]"; + } + } + } + CMVR_LOG(INFO) << "[PinocchioDlsIKSolver] base pose cached (constant)"; + return true; +} + +bool PinocchioDlsIKSolver::ik(const Eigen::Matrix4d &target_pose_base, + std::vector &joints_angle, + bool is_tcp) +{ + const std::string& ee_link = (is_tcp && has_tcp_) ? tcp_frame_name_ : chain_tip_frame_name_; + return ik(chain_base_frame_name_, ee_link, target_pose_base, joints_angle); +} + +bool PinocchioDlsIKSolver::ik(const std::string& base_link, + const std::string& ee_link, + const Eigen::Matrix4d& target_pose, + std::vector& joints_angle) { + if (!initialized_) return false; + if ((int)cur_joints_angle_.size() != chain_q_dof_) return false; + if (!model_.existFrame(base_link)) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] base frame not found: " << base_link; + return false; + } + if (!model_.existFrame(ee_link)) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] ee frame not found: " << ee_link; + return false; + } + const pinocchio::FrameIndex base_frame_id = model_.getFrameId(base_link); + const pinocchio::FrameIndex ee_frame_id = model_.getFrameId(ee_link); + + const pinocchio::JointIndex chain_base_joint = model_.frames[base_frame_id_].parent; + const pinocchio::JointIndex base_joint = model_.frames[base_frame_id].parent; + const pinocchio::JointIndex flange_joint = model_.frames[flange_frame_id_].parent; + const pinocchio::JointIndex ee_joint = model_.frames[ee_frame_id].parent; + + auto jointOnParentPath = [this](pinocchio::JointIndex from, + pinocchio::JointIndex target) { + if (target == 0) return true; + pinocchio::JointIndex j = from; + while (j != 0) { + if (j == target) return true; + j = model_.parents[j]; + } + return false; + }; + + auto jointOnConfiguredBranch = [&](pinocchio::JointIndex j) { + return jointOnParentPath(flange_joint, j) && jointOnParentPath(j, chain_base_joint); + }; + + const bool base_on_branch = jointOnConfiguredBranch(base_joint); + const bool ee_on_branch = jointOnConfiguredBranch(ee_joint); + const bool base_is_ancestor_of_ee = + jointOnParentPath(ee_joint, base_joint); + if (!base_on_branch || !ee_on_branch || !base_is_ancestor_of_ee) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] base/ee must be on configured single chain and base must be ancestor of ee"; + return false; + } + + const pinocchio::SE3 base_M_target = matrix4ToSE3(target_pose); + + Eigen::VectorXd q_chain = Eigen::Map(cur_joints_angle_.data(), chain_q_dof_); + + bool success = false; + for (int iter = 0; iter < max_iters_; ++iter) { + Eigen::VectorXd q_full; + if (!buildFullQFromChain(q_chain, q_full, "ik(base,ee)")) { + return false; + } + updateKinematics(q_full); + + const pinocchio::SE3& oM_base = + (base_frame_id == base_frame_id_) ? getBasePoseWorld() : data_->oMf[base_frame_id]; + const pinocchio::SE3 oM_target = oM_base * base_M_target; + const pinocchio::SE3 &oM_cur = data_->oMf[ee_frame_id]; + pinocchio::SE3 dM = oM_cur.inverse() * oM_target; + + Eigen::Matrix err = pinocchio::log6(dM).toVector(); + if (err.head<3>().norm() < pos_eps_ && err.tail<3>().norm() < rot_eps_) { + success = true; + break; + } + + Eigen::Matrix J_full(6, model_.nv); + pinocchio::computeFrameJacobian(model_, *data_, q_full, + ee_frame_id, + pinocchio::ReferenceFrame::LOCAL, + J_full); + Eigen::MatrixXd J = extractChainJacobian(J_full); + Eigen::MatrixXd J_pinv = dampedPseudoInverse(J, damping_); + + Eigen::VectorXd dq = J_pinv * err; + q_chain += dq; + q_chain = cmvr::kinematics::clampToJointPositionLimits( + q_chain, + joint_pos_lower_limits_, + joint_pos_upper_limits_); + } + + if (!success) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] IK solve failed"; + return false; + } + + cur_joints_angle_.assign(q_chain.data(), q_chain.data() + q_chain.size()); + joints_angle = cur_joints_angle_; + return true; +} + +void PinocchioDlsIKSolver::setJointLimitAvoidance(bool enable, + double gain, + double margin_ratio, + double max_push) { + limit_avoidance_enabled_ = enable; + limit_avoidance_gain_ = std::max(0.0, gain); + limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49); + limit_avoidance_max_push_ = max_push; +} + +bool PinocchioDlsIKSolver::ik(const std::string& base_link, + const std::string& ee_link, + const Eigen::Matrix& target_vel, + std::vector& joints_vel, + double damping, + double qdot_abs_max) +{ + if (!initialized_) return false; + if (!model_.existFrame(base_link)) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] base frame not found: " << base_link; + return false; + } + if (ee_link.empty()) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] ee_frame_name is empty"; + return false; + } + if (!model_.existFrame(ee_link)) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] frame not found: " << ee_link; + return false; + } + + if (chain_v_dof_ <= 0) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] invalid chain_v_dof"; + return false; + } + + if (static_cast(cur_joints_angle_.size()) != chain_q_dof_) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] ik(velocity) current joint state not initialized"; + return false; + } + + const pinocchio::FrameIndex base_id = model_.getFrameId(base_link); + const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link); + + const pinocchio::JointIndex chain_base_joint = model_.frames[base_frame_id_].parent; + const pinocchio::JointIndex base_joint = model_.frames[base_id].parent; + const pinocchio::JointIndex flange_joint = model_.frames[flange_frame_id_].parent; + const pinocchio::JointIndex ee_joint = model_.frames[ee_id].parent; + + auto jointOnParentPath = [this](pinocchio::JointIndex from, + pinocchio::JointIndex target) { + if (target == 0) return true; + pinocchio::JointIndex j = from; + while (j != 0) { + if (j == target) return true; + j = model_.parents[j]; + } + return false; + }; + auto jointOnConfiguredBranch = [&](pinocchio::JointIndex j) { + return jointOnParentPath(flange_joint, j) && jointOnParentPath(j, chain_base_joint); + }; + const bool base_on_branch = jointOnConfiguredBranch(base_joint); + const bool ee_on_branch = jointOnConfiguredBranch(ee_joint); + const bool base_is_ancestor_of_ee = jointOnParentPath(ee_joint, base_joint); + if (!base_on_branch || !ee_on_branch || !base_is_ancestor_of_ee) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] base/ee must be on configured single chain and base must be ancestor of ee"; + return false; + } + + const Eigen::Map q_chain(cur_joints_angle_.data(), chain_q_dof_); + + Eigen::VectorXd q_full; + if (!buildFullQFromChain(q_chain, q_full, "ik(velocity)")) { + return false; + } + + updateKinematics(q_full); + + const pinocchio::SE3 &oM_base = + (base_id == base_frame_id_) ? getBasePoseWorld() : data_->oMf[base_id]; + const pinocchio::SE3 &oM_ee = data_->oMf[ee_id]; + + const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee; + const Eigen::Matrix3d R_be = base_M_ee.rotation(); + + Eigen::Matrix twist_base; + twist_base.head<3>() = R_be * target_vel.head<3>(); + twist_base.tail<3>() = R_be * target_vel.tail<3>(); + + Eigen::Matrix J_world(6, model_.nv); + pinocchio::computeFrameJacobian(model_, *data_, q_full, + ee_id, + pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, + J_world); + Eigen::MatrixXd J = extractChainJacobian(J_world); + const Eigen::Matrix3d R_bo = oM_base.rotation().transpose(); + J.topRows(3) = R_bo * J.topRows(3); + J.bottomRows(3) = R_bo * J.bottomRows(3); + + const double lambda = (damping > 0.0) ? damping : damping_; + Eigen::Matrix A = J * J.transpose(); + A.diagonal().array() += (lambda * lambda); + Eigen::LDLT> ldlt(A); + if (ldlt.info() != Eigen::Success) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] ik(velocity) LDLT failed"; + return false; + } + Eigen::VectorXd qdot = J.transpose() * ldlt.solve(twist_base); + + const Eigen::VectorXd qdot_avoid = computeJointLimitAvoidanceVelocity(q_chain); + if (qdot_avoid.size() == chain_v_dof_ && qdot_avoid.squaredNorm() > 1e-16) { + const Eigen::Matrix A_inv = + ldlt.solve(Eigen::Matrix::Identity()); + const Eigen::MatrixXd J_pinv = J.transpose() * A_inv; + qdot += projectToNullspace(J_pinv, J, qdot_avoid); + } + + joints_vel.resize(chain_v_dof_); + const Eigen::VectorXd qdot_limited = + cmvr::kinematics::scaleToVelocityLimits(qdot, joint_vel_limits_, qdot_abs_max); + for (int i = 0; i < chain_v_dof_; ++i) { + joints_vel[i] = qdot_limited[i]; + } + + return true; +} + +bool PinocchioDlsIKSolver::solveVelocityBase(const Eigen::MatrixXd& jacobian_base, + const Eigen::Matrix& target_twist_base, + const std::vector& q_chain_std, + std::vector& qdot_out, + const double qdot_abs_max) const +{ + if (jacobian_base.rows() != 6 || jacobian_base.cols() != chain_v_dof_) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] solveVelocityBase failed: jacobian size mismatch"; + return false; + } + if (static_cast(q_chain_std.size()) != chain_q_dof_) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] solveVelocityBase failed: q size mismatch"; + return false; + } + + Eigen::Matrix A = jacobian_base * jacobian_base.transpose(); + A.diagonal().array() += (damping_ * damping_); + + Eigen::LDLT> ldlt(A); + if (ldlt.info() != Eigen::Success) { + CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] solveVelocityBase failed: LDLT failed"; + return false; + } + + Eigen::VectorXd qdot = jacobian_base.transpose() * ldlt.solve(target_twist_base); + + const Eigen::Map q_chain(q_chain_std.data(), chain_q_dof_); + const Eigen::VectorXd qdot_avoid = computeJointLimitAvoidanceVelocity(q_chain); + if (qdot_avoid.size() == chain_v_dof_ && qdot_avoid.squaredNorm() > 1e-16) { + const Eigen::Matrix A_inv = + ldlt.solve(Eigen::Matrix::Identity()); + const Eigen::MatrixXd J_pinv = jacobian_base.transpose() * A_inv; + qdot += projectToNullspace(J_pinv, jacobian_base, qdot_avoid); + } + + qdot = cmvr::kinematics::scaleToVelocityLimits(qdot, joint_vel_limits_, qdot_abs_max); + qdot_out.resize(chain_v_dof_); + for (int i = 0; i < chain_v_dof_; ++i) { + qdot_out[i] = qdot[i]; + } + return true; +} + +} // namespace cmvr diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp new file mode 100644 index 00000000..8b4311c4 --- /dev/null +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp @@ -0,0 +1,330 @@ +// Created by Codex on 2026/3/3. + +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" +#include "common/base/logging/logger.h" + +#include +#include +#include + +namespace cmvr { + +PinocchioIKBase::PinocchioIKBase(const std::string& urdf_path, + const std::string& base_frame_name, + const std::string& flange_frame_name, + const std::string& tcp_frame_name) + : IKSolver(urdf_path, base_frame_name, flange_frame_name) + , tcp_frame_name_(tcp_frame_name) { +} + +PinocchioIKBase::PinocchioIKBase(std::shared_ptr parser, + const std::string& base_frame_name, + const std::string& flange_frame_name, + const std::string& tcp_frame_name) + : IKSolver(parser, base_frame_name, flange_frame_name) + , tcp_frame_name_(tcp_frame_name) { +} + +bool PinocchioIKBase::initPinocchioFromUrdfChain(UrdfParser::ChainInfo* chain_info_out, + std::string* error) { + if (!urdf_parser_ || !urdf_parser_->loaded()) { + if (error) *error = "urdf parser not initialized"; + return false; + } + if (chain_base_frame_name_.empty() || chain_tip_frame_name_.empty()) { + if (error) *error = "chain base/tip frame name is empty"; + return false; + } + if (!urdf_chain_cached_) { + if (!initUrdfChain(urdf_parser_, chain_base_frame_name_, chain_tip_frame_name_)) { + if (error) *error = "initUrdfChain failed"; + return false; + } + } + + model_ = urdf_parser_->model(); + data_ = std::make_unique(model_); + + UrdfParser::ChainInfo chain_info; + std::string chain_err; + if (!urdf_parser_->extractChain(chain_base_frame_name_, chain_tip_frame_name_, chain_info, &chain_err)) { + if (error) *error = chain_err; + return false; + } + + base_frame_id_ = chain_info.base_frame_id; + flange_frame_id_ = chain_info.tip_frame_id; + chain_q_start_ = chain_info.q_start; + chain_q_dof_ = chain_info.q_dof; + chain_v_start_ = chain_info.v_start; + chain_v_dof_ = chain_info.v_dof; + + has_tcp_ = false; + tcp_frame_id_ = (pinocchio::FrameIndex)(-1); + if (!tcp_frame_name_.empty() && model_.existFrame(tcp_frame_name_)) { + tcp_frame_id_ = model_.getFrameId(tcp_frame_name_); + has_tcp_ = true; + } + + if (chain_info_out != nullptr) { + *chain_info_out = chain_info; + } + return true; +} + +bool PinocchioIKBase::buildFullQFromInput(const std::vector& joints, + Eigen::VectorXd& q_full, + const char* context) const { + const int size = static_cast(joints.size()); + if (size != chain_q_dof_ && size != model_.nq) { + if (context != nullptr) { + CMVR_LOG(ERROR) << "[PinocchioIKBase] " << context << " joints size mismatch"; + } + return false; + } + + q_full = pinocchio::neutral(model_); + if (size == model_.nq) { + q_full = Eigen::Map(joints.data(), model_.nq); + } else { + Eigen::Map q_chain(joints.data(), chain_q_dof_); + q_full.segment(chain_q_start_, chain_q_dof_) = q_chain; + } + return true; +} + +bool PinocchioIKBase::buildFullQFromChain(const Eigen::VectorXd& q_chain, + Eigen::VectorXd& q_full, + const char* context) const { + if (q_chain.size() != chain_q_dof_) { + if (context != nullptr) { + CMVR_LOG(ERROR) << "[PinocchioIKBase] " << context << " chain q size mismatch"; + } + return false; + } + q_full = pinocchio::neutral(model_); + q_full.segment(chain_q_start_, chain_q_dof_) = q_chain; + return true; +} + +Eigen::MatrixXd PinocchioIKBase::extractChainJacobian( + const Eigen::Matrix& jacobian_full) const { + return jacobian_full.middleCols(chain_v_start_, chain_v_dof_); +} + +const pinocchio::SE3& PinocchioIKBase::getBasePoseWorld() const { + return base_pose_cached_ ? oM_base_cached_ : data_->oMf[base_frame_id_]; +} + +void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) { + pinocchio::forwardKinematics(model_, *data_, q_full); + pinocchio::updateFramePlacements(model_, *data_); +} + +bool PinocchioIKBase::buildJacobianBaseAtQ(const std::vector& q_chain_std, + const pinocchio::FrameIndex ee_id, + Eigen::MatrixXd& jacobian_base, + Eigen::Matrix3d& base_R_ee, + Eigen::VectorXd* q_full_out) +{ + if (static_cast(q_chain_std.size()) != chain_q_dof_) { + CMVR_LOG(ERROR) << "[PinocchioIKBase] buildJacobianBaseAtQ: q size mismatch"; + return false; + } + + const Eigen::Map q_chain(q_chain_std.data(), chain_q_dof_); + + Eigen::VectorXd q_full; + if (!buildFullQFromChain(q_chain, q_full, "buildJacobianBaseAtQ")) { + return false; + } + + updateKinematics(q_full); + + const pinocchio::SE3& oM_base = getBasePoseWorld(); + const pinocchio::SE3& oM_ee = data_->oMf[ee_id]; + const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee; + base_R_ee = base_M_ee.rotation(); + + Eigen::Matrix jacobian_world(6, model_.nv); + pinocchio::computeFrameJacobian(model_, *data_, q_full, + ee_id, + pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, + jacobian_world); + + jacobian_base = extractChainJacobian(jacobian_world); + + const Eigen::Matrix3d R_bo = oM_base.rotation().transpose(); + jacobian_base.topRows(3) = R_bo * jacobian_base.topRows(3); + jacobian_base.bottomRows(3) = R_bo * jacobian_base.bottomRows(3); + + if (q_full_out != nullptr) { + *q_full_out = q_full; + } + + return true; +} + +bool PinocchioIKBase::computeJacobianBaseAtQ(const std::vector& q_chain, + const bool is_tcp, + Eigen::MatrixXd& jacobian_base, + Eigen::Matrix3d& base_R_ee) +{ + const pinocchio::FrameIndex ee_id = (is_tcp && has_tcp_) ? tcp_frame_id_ : flange_frame_id_; + return buildJacobianBaseAtQ(q_chain, ee_id, jacobian_base, base_R_ee, nullptr); +} + +bool PinocchioIKBase::computeJacobianBaseAtQ(const std::vector& q_chain, + const std::string& ee_frame_name, + Eigen::MatrixXd& jacobian_base, + Eigen::Matrix3d& base_R_ee) +{ + if (!model_.existFrame(ee_frame_name)) { + CMVR_LOG(ERROR) << "[PinocchioIKBase] ee frame not found: " << ee_frame_name; + return false; + } + return buildJacobianBaseAtQ(q_chain, + model_.getFrameId(ee_frame_name), + jacobian_base, + base_R_ee, + nullptr); +} + +bool PinocchioIKBase::computeMeasuredTwistBase(const std::vector& q_chain, + const std::vector& qdot_chain, + const pinocchio::FrameIndex ee_id, + Eigen::Matrix& twist_base, + Eigen::MatrixXd* jacobian_base_out, + Eigen::Matrix3d* base_R_ee_out, + Eigen::VectorXd* q_full_out) +{ + if (static_cast(qdot_chain.size()) != chain_v_dof_) { + CMVR_LOG(ERROR) << "[PinocchioIKBase] computeMeasuredTwistBase: qdot size mismatch"; + return false; + } + + Eigen::MatrixXd jacobian_base; + Eigen::Matrix3d base_R_ee = Eigen::Matrix3d::Identity(); + if (!buildJacobianBaseAtQ(q_chain, ee_id, jacobian_base, base_R_ee, q_full_out)) { + return false; + } + + const Eigen::Map qdot(qdot_chain.data(), chain_v_dof_); + twist_base = jacobian_base * qdot; + + if (jacobian_base_out != nullptr) { + *jacobian_base_out = jacobian_base; + } + if (base_R_ee_out != nullptr) { + *base_R_ee_out = base_R_ee; + } + + return true; +} + +bool PinocchioIKBase::computeTwistBaseAtQ(const std::vector& q_chain, + const std::vector& qdot_chain, + const bool is_tcp, + Eigen::Matrix& twist_base, + Eigen::MatrixXd* jacobian_base_out, + Eigen::Matrix3d* base_R_ee_out) +{ + const pinocchio::FrameIndex ee_id = (is_tcp && has_tcp_) ? tcp_frame_id_ : flange_frame_id_; + return computeMeasuredTwistBase(q_chain, + qdot_chain, + ee_id, + twist_base, + jacobian_base_out, + base_R_ee_out, + nullptr); +} + +bool PinocchioIKBase::computeTwistBaseAtQ(const std::vector& q_chain, + const std::vector& qdot_chain, + const std::string& ee_frame_name, + Eigen::Matrix& twist_base, + Eigen::MatrixXd* jacobian_base_out, + Eigen::Matrix3d* base_R_ee_out) +{ + if (!model_.existFrame(ee_frame_name)) { + CMVR_LOG(ERROR) << "[PinocchioIKBase] ee frame not found: " << ee_frame_name; + return false; + } + return computeMeasuredTwistBase(q_chain, + qdot_chain, + model_.getFrameId(ee_frame_name), + twist_base, + jacobian_base_out, + base_R_ee_out, + nullptr); +} + +bool PinocchioIKBase::solveVelocityBase(const Eigen::MatrixXd& jacobian_base, + const Eigen::Matrix& target_twist_base, + const std::vector& q_chain, + std::vector& qdot_out, + const double qdot_abs_max) const +{ + (void)jacobian_base; + (void)target_twist_base; + (void)q_chain; + (void)qdot_out; + (void)qdot_abs_max; + CMVR_LOG(ERROR) << "[PinocchioIKBase] solveVelocityBase is not implemented by this solver"; + return false; +} + +bool PinocchioIKBase::fk(const std::vector& joints_angle, + Eigen::Matrix4d& cur_pose, + bool is_tcp) { + const std::string& ee_link = (is_tcp && has_tcp_) ? tcp_frame_name_ : chain_tip_frame_name_; + return fk(chain_base_frame_name_, ee_link, joints_angle, cur_pose); +} + +bool PinocchioIKBase::fk(const std::string& base_link, + const std::string& ee_link, + const std::vector& joints_angle, + Eigen::Matrix4d& cur_pose) { + if (!data_) { + CMVR_LOG(ERROR) << "[PinocchioIKBase] fk called before pinocchio init"; + return false; + } + if (!model_.existFrame(base_link)) { + CMVR_LOG(ERROR) << "[PinocchioIKBase] base frame not found: " << base_link; + return false; + } + if (!model_.existFrame(ee_link)) { + CMVR_LOG(ERROR) << "[PinocchioIKBase] ee frame not found: " << ee_link; + return false; + } + + Eigen::VectorXd q_full; + if (!buildFullQFromInput(joints_angle, q_full, "fk")) { + return false; + } + updateKinematics(q_full); + + const pinocchio::FrameIndex base_id = model_.getFrameId(base_link); + const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link); + const pinocchio::SE3& oM_base = data_->oMf[base_id]; + const pinocchio::SE3& oM_ee = data_->oMf[ee_id]; + const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee; + cur_pose = se3ToMatrix4(base_M_ee); + return true; +} + +pinocchio::SE3 PinocchioIKBase::matrix4ToSE3(const Eigen::Matrix4d& T) { + pinocchio::SE3 M; + M.rotation() = T.block<3,3>(0,0); + M.translation() = T.block<3,1>(0,3); + return M; +} + +Eigen::Matrix4d PinocchioIKBase::se3ToMatrix4(const pinocchio::SE3& M) { + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + T.block<3,3>(0,0) = M.rotation(); + T.block<3,1>(0,3) = M.translation(); + return T; +} + +} // namespace cmvr diff --git a/cmvr-es/ik_solver/src/pinocchio_qp_ik_solver.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp similarity index 61% rename from cmvr-es/ik_solver/src/pinocchio_qp_ik_solver.cpp rename to cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp index 5d8eb866..3174b44e 100644 --- a/cmvr-es/ik_solver/src/pinocchio_qp_ik_solver.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp @@ -2,79 +2,40 @@ // Created by lgv on 12/2/25. // -#include "ik_solver/include/pinocchio_qp_ik_solver.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h" +#include "common/base/logging/logger.h" +#include "common/math/joint_limits.h" +#include "common/config/config_files.h" #include #include #include #include -#include #include // std::clamp, std::max, std::min #include // std::sqrt -#include "common/utils/config_helper/include/config_helper.h" namespace cmvr { using Eigen::Matrix4d; using Eigen::VectorXd; using Eigen::MatrixXd; - - PinocchioQpIKSolver::PinocchioQpIKSolver(const std::string &urdf_path, - const std::string &base_frame_name, - const std::string &flange_frame_name, - const std::string &tcp_frame_name, - double lambda, - double w_posrot, - int max_iters, - double tol, - double qp_time_limit) - : PinocchioIKBase(urdf_path, base_frame_name, flange_frame_name, tcp_frame_name) - , urdf_path_(urdf_path) - , lambda_(lambda) - , w_posrot_(w_posrot) - , max_iters_(max_iters) - , tol_(tol) - , qp_time_limit_(qp_time_limit) - , solver_() { - } - - PinocchioQpIKSolver::PinocchioQpIKSolver() - : PinocchioIKBase("", "", "", "") - , solver_() { - config::PinocchioQpIKConfig config; - if (ConfigHelper::getPinocchioQpIkSolverConfig(config)) - { - urdf_path_ = config.urdf_path(); - chain_base_frame_name_ = config.base_frame_name(); - chain_tip_frame_name_ = config.flange_frame_name(); - tcp_frame_name_ = config.tcp_frame_name(); - lambda_ = config.lambda(); - w_posrot_ = config.w_posrot(); - max_iters_ = config.max_iters(); - tol_ = config.tol(); - qp_time_limit_ = config.qp_time_limit(); - if (!urdf_path_.empty() && - !chain_base_frame_name_.empty() && - !chain_tip_frame_name_.empty()) { - initUrdfChain(urdf_path_, chain_base_frame_name_, chain_tip_frame_name_); - } - } - } + using cmvr::common::config::positiveIntOr; + using cmvr::common::config::positiveOr; PinocchioQpIKSolver::PinocchioQpIKSolver(const config::PinocchioQpIKConfig& config) : PinocchioIKBase(config.urdf_path(), config.base_frame_name(), config.flange_frame_name(), config.tcp_frame_name()) - , config_(config) - , solver_() + , config_(config) + , urdf_path_(config.urdf_path()) + , lambda_(positiveOr(config.lambda(), 1e-4)) + , w_posrot_(positiveOr(config.w_posrot(), 0.5)) + , max_iters_(positiveIntOr(config.max_iters(), 100)) + , tol_(positiveOr(config.tol(), 1e-6)) + , qp_time_limit_(positiveOr(config.qp_time_limit(), 1e-2)) + , solver_() { - urdf_path_ = config.urdf_path(); - lambda_ = config.lambda(); - w_posrot_ = config.w_posrot(); - max_iters_ = config.max_iters(); - tol_ = config.tol(); - qp_time_limit_ = config.qp_time_limit(); } bool PinocchioQpIKSolver::init() { @@ -82,11 +43,11 @@ namespace cmvr { if (urdf_path_.empty() || chain_base_frame_name_.empty() || chain_tip_frame_name_.empty()) { - std::cerr << "[PinocchioQpIKSolver] missing urdf/base/flange config before init().\n"; + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] missing urdf/base/flange config before init()."; return false; } if (!initUrdfChain(urdf_path_, chain_base_frame_name_, chain_tip_frame_name_)) { - std::cerr << "[PinocchioQpIKSolver] Failed to cache URDF chain metadata in IKSolver.\n"; + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] Failed to cache URDF chain metadata in IKSolver."; return false; } } @@ -94,18 +55,18 @@ namespace cmvr { UrdfParser::ChainInfo chain_info; std::string err; if (!initPinocchioFromUrdfChain(&chain_info, &err)) { - std::cerr << "[PinocchioQpIKSolver] Failed to init pinocchio base: " << err << std::endl; + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] Failed to init pinocchio base: " << err; return false; } if (chain_q_dof_ <= 0 || chain_v_dof_ <= 0) { - std::cerr << "[PinocchioQpIKSolver] Invalid chain dof: q=" << chain_q_dof_ - << " v=" << chain_v_dof_ << std::endl; + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] Invalid chain dof: q=" << chain_q_dof_ + << " v=" << chain_v_dof_; return false; } if (chain_q_dof_ != chain_v_dof_) { - std::cerr << "[PinocchioQpIKSolver] requires q_dof == v_dof, got q=" - << chain_q_dof_ << " v=" << chain_v_dof_ << std::endl; + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] requires q_dof == v_dof, got q=" + << chain_q_dof_ << " v=" << chain_v_dof_; return false; } @@ -114,14 +75,14 @@ namespace cmvr { int expected_v = chain_v_start_; for (const auto& seg : chain_info.joints) { if (seg.nq != 1 || seg.nv != 1) { - std::cerr << "[PinocchioQpIKSolver] only supports nq=1,nv=1 joints. joint=" - << seg.name << " nq=" << seg.nq << " nv=" << seg.nv << std::endl; + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] only supports nq=1,nv=1 joints. joint=" + << seg.name << " nq=" << seg.nq << " nv=" << seg.nv; return false; } if (seg.q_index != expected_q || seg.v_index != expected_v) { - std::cerr << "[PinocchioQpIKSolver] chain q/v index must be contiguous. joint=" + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] chain q/v index must be contiguous. joint=" << seg.name << " q_index=" << seg.q_index << " expect_q=" << expected_q - << " v_index=" << seg.v_index << " expect_v=" << expected_v << std::endl; + << " v_index=" << seg.v_index << " expect_v=" << expected_v; return false; } expected_q += seg.nq; @@ -130,7 +91,7 @@ namespace cmvr { if (joint_pos_lower_limits_.size() != chain_q_dof_ || joint_pos_upper_limits_.size() != chain_q_dof_) { - std::cerr << "[PinocchioQpIKSolver] position limits size mismatch with q_dof." << std::endl; + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] position limits size mismatch with q_dof."; return false; } @@ -148,13 +109,13 @@ namespace cmvr { } initialized_ = true; - std::cout << "[PinocchioQpIKSolver] Init OK. full nq = " << model_.nq + CMVR_LOG(INFO) << "[PinocchioQpIKSolver] Init OK. full nq = " << model_.nq << ", active dof = " << chain_q_dof_ << ", q_start=" << chain_q_start_ << ", v_start=" << chain_v_start_ << ", base frame = " << chain_base_frame_name_ << ", flange frame = " << chain_tip_frame_name_ << ", tcp frame = " << (has_tcp_ ? tcp_frame_name_ : "") - << std::endl; + ; return true; } @@ -162,8 +123,8 @@ namespace cmvr { if (qd_max.size() == chain_v_dof_) { joint_vel_limits_ = qd_max; } else { - std::cerr << "[PinocchioQpIKSolver] setVelocityLimits size mismatch. got " - << qd_max.size() << ", expect " << chain_v_dof_ << std::endl; + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] setVelocityLimits size mismatch. got " + << qd_max.size() << ", expect " << chain_v_dof_; } } @@ -171,8 +132,8 @@ namespace cmvr { if (qdd_max.size() == chain_q_dof_) { qdd_max_global_ = qdd_max; } else { - std::cerr << "[PinocchioQpIKSolver] setAccelerationLimits size mismatch. got " - << qdd_max.size() << ", expect " << chain_q_dof_ << std::endl; + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] setAccelerationLimits size mismatch. got " + << qdd_max.size() << ", expect " << chain_q_dof_; } } @@ -180,12 +141,12 @@ namespace cmvr { std::vector &joints_angle, bool is_tcp) { if (!initialized_) { - std::cerr << "[PinocchioQpIKSolver] IK called before init()." << std::endl; + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] IK called before init()."; return false; } if (cur_joints_angle_.size() != static_cast(chain_q_dof_)) { - std::cerr << "[PinocchioQpIKSolver] cur_joints_angle_ size mismatch: " - << cur_joints_angle_.size() << " vs dof = " << chain_q_dof_ << std::endl; + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] cur_joints_angle_ size mismatch: " + << cur_joints_angle_.size() << " vs dof = " << chain_q_dof_; return false; } @@ -195,9 +156,9 @@ namespace cmvr { target_frame_id = tcp_frame_id_; } else { if (is_tcp && !has_tcp_) { - std::cerr << "[PinocchioQpIKSolver] is_tcp=true but no TCP frame, " + CMVR_LOG(WARNING) << "[PinocchioQpIKSolver] is_tcp=true but no TCP frame, " "fallback to flange." - << std::endl; + ; } target_frame_id = flange_frame_id_; } @@ -301,19 +262,17 @@ namespace cmvr { try { dq_local = solver_.Solve(); } catch (const QPSolverException &e) { - std::cerr << "[PinocchioQpIKSolver] QP failed: " << e.what() - << " (code=" << e.code() << ")" << std::endl; + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] QP failed: " << e.what() + << " (code=" << e.code() << ")"; return false; } catch (const std::exception &e) { - std::cerr << "[PinocchioQpIKSolver] QP failed: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] QP failed: " << e.what(); return false; } // 5) 更新 q_local 并写回 q_full q_local += dq_local; - for (int i = 0; i < chain_q_dof_; ++i) { - q_local(i) = std::clamp(q_local(i), q_min(i), q_max(i)); - } + q_local = cmvr::kinematics::clampToJointPositionLimits(q_local, q_min, q_max); q_full.segment(chain_q_start_, chain_q_dof_) = q_local; // 6) 收敛检查 @@ -336,5 +295,96 @@ namespace cmvr { return false; } + bool PinocchioQpIKSolver::solveVelocityBase( + const MatrixXd& jacobian_base, + const Eigen::Matrix& target_twist_base, + const std::vector& q_chain_std, + std::vector& qdot_out, + const double qdot_abs_max) const + { + if (!initialized_) { + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] solveVelocityBase called before init()."; + return false; + } + if (jacobian_base.rows() != 6 || jacobian_base.cols() != chain_v_dof_) { + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] solveVelocityBase jacobian size mismatch."; + return false; + } + if (static_cast(q_chain_std.size()) != chain_q_dof_ || chain_q_dof_ != chain_v_dof_) { + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] solveVelocityBase q size mismatch."; + return false; + } + + const int dof = chain_v_dof_; + MatrixXd cost(6 + dof, dof); + VectorXd target(6 + dof); + cost.setZero(); + target.setZero(); + cost.topRows(6) = jacobian_base; + target.head(6) = target_twist_base; + cost.bottomRows(dof) = std::sqrt(lambda_) * MatrixXd::Identity(dof, dof); + + VectorXd lower(dof); + VectorXd upper(dof); + const Eigen::Map q_chain(q_chain_std.data(), dof); + for (int i = 0; i < dof; ++i) { + double limit = std::numeric_limits::infinity(); + if (joint_vel_limits_.size() == dof) { + const double joint_limit = std::abs(joint_vel_limits_[i]); + if (std::isfinite(joint_limit) && joint_limit > 0.0) { + limit = std::min(limit, joint_limit); + } + } + if (std::isfinite(qdot_abs_max) && qdot_abs_max > 0.0) { + limit = std::min(limit, qdot_abs_max); + } + if (!std::isfinite(limit) || limit <= 0.0) { + limit = 1e6; + } + + lower[i] = -limit; + upper[i] = limit; + + if (joint_pos_lower_limits_.size() == dof && + joint_pos_upper_limits_.size() == dof) { + const double q_min = joint_pos_lower_limits_[i]; + const double q_max = joint_pos_upper_limits_[i]; + if (std::isfinite(q_min) && std::isfinite(q_max) && q_max > q_min) { + constexpr double kLimitEps = 1e-6; + if (q_chain[i] <= q_min + kLimitEps) { + lower[i] = std::max(0.0, lower[i]); + } + if (q_chain[i] >= q_max - kLimitEps) { + upper[i] = std::min(0.0, upper[i]); + } + } + } + } + + QPSolver solver; + solver.Setup(dof, dof, qp_time_limit_); + solver.SetCostFunction(cost, target); + solver.SetConstraintsFunction(MatrixXd::Identity(dof, dof), lower, upper); + + VectorXd qdot; + try { + qdot = solver.Solve(); + } catch (const QPSolverException& e) { + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] solveVelocityBase QP failed: " + << e.what() << " (code=" << e.code() << ")"; + return false; + } catch (const std::exception& e) { + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] solveVelocityBase QP failed: " + << e.what(); + return false; + } + + if (qdot.size() != dof) { + return false; + } + qdot_out.assign(qdot.data(), qdot.data() + qdot.size()); + return true; + } + } // namespace cmvr diff --git a/cmvr-es/algorithms/kinematics/ik_solver/srs/include/srs_ik_solver.h b/cmvr-es/algorithms/kinematics/ik_solver/srs/include/srs_ik_solver.h new file mode 100644 index 00000000..54a20f2d --- /dev/null +++ b/cmvr-es/algorithms/kinematics/ik_solver/srs/include/srs_ik_solver.h @@ -0,0 +1,84 @@ +#pragma once + +#include +#include + +#include + +#include "cmvr/config/srs_ik_config.pb.h" +#include "algorithms/kinematics/ik_solver/common/include/ik_solver.h" + +namespace cmvr { + +class SrsIKSolver : public IKSolver { +public: + enum ConfigDirection { + OUTWARD = 1, + INWARD = -1 + }; + + explicit SrsIKSolver(const config::SrsIKConfig& cfg); + ~SrsIKSolver() override = default; + + bool init() override; + + bool ik(const Eigen::Matrix4d& target_pose, + std::vector& joints_angle, + bool is_tcp = true) override; + + bool fk(const std::vector& joints_angle, + Eigen::Matrix4d& cur_pose, + bool is_tcp = true) override; + + bool ikWithPsi(const Eigen::Matrix4d& pose, + std::vector& joints, + double psi); + + Eigen::Matrix4d calc_total_transform(const std::vector& joint_angles); + + bool cal_coefficient_matrix(const Eigen::Matrix4d& pose, + Eigen::MatrixXd& s_mat, + Eigen::MatrixXd& w_mat); + + void setPsi(double psi) { psi_ = psi; } + double psi() const { return psi_; } + + void set_shoulder_config(ConfigDirection value) { shoulder_config_ = value; } + void set_elbow_config(ConfigDirection value) { elbow_config_ = value; } + void set_wrist_config(ConfigDirection value) { wrist_config_ = value; } + + int get_shoulder_config() const { return static_cast(shoulder_config_); } + int get_elbow_config() const { return static_cast(elbow_config_); } + int get_wrist_config() const { return static_cast(wrist_config_); } + + std::vector> get_joints_limits() const { return joints_limits_; } + +private: + static ConfigDirection toConfigDirection_(int value); + + Eigen::Matrix3d reference_plane(const Eigen::Vector3d& S, + const Eigen::Vector3d& W); + Eigen::Matrix3d calc_rotation_matrix(const Eigen::Vector3d& rotation_axis, + double rotation_angle); + Eigen::Matrix4d calc_dh(double d, double alpha, double a, double theta); + +private: + config::SrsIKConfig cfg_; + double psi_{0.0}; + + ConfigDirection shoulder_config_{OUTWARD}; + ConfigDirection elbow_config_{OUTWARD}; + ConfigDirection wrist_config_{OUTWARD}; + + Eigen::VectorXd link_lengths_; + Eigen::MatrixXd dh_params_; + + double d_bs_{0.0}; + double d_se_{0.0}; + double d_ew_{0.0}; + double d_wt_{0.0}; + + std::vector> joints_limits_{}; +}; + +} // namespace cmvr diff --git a/cmvr-es/algorithms/kinematics/ik_solver/srs/src/srs_ik_solver.cpp b/cmvr-es/algorithms/kinematics/ik_solver/srs/src/srs_ik_solver.cpp new file mode 100644 index 00000000..3732c2ae --- /dev/null +++ b/cmvr-es/algorithms/kinematics/ik_solver/srs/src/srs_ik_solver.cpp @@ -0,0 +1,339 @@ +#include "algorithms/kinematics/ik_solver/srs/include/srs_ik_solver.h" + +#include +#include +#include "common/base/logging/logger.h" + +#include "common/base/constants.h" +#include "common/math/support_functions.h" + +namespace cmvr { + +SrsIKSolver::SrsIKSolver(const config::SrsIKConfig& cfg) + : IKSolver("", "", "") + , cfg_(cfg) +{ +} + +bool SrsIKSolver::init() +{ + psi_ = cfg_.initial_psi(); + shoulder_config_ = toConfigDirection_(cfg_.shoulder_config()); + elbow_config_ = toConfigDirection_(cfg_.elbow_config()); + wrist_config_ = toConfigDirection_(cfg_.wrist_config()); + + link_lengths_ = Eigen::VectorXd(4); + if (cfg_.link_lengths_size() == 4) { + for (int i = 0; i < 4; ++i) { + link_lengths_[i] = cfg_.link_lengths(i); + } + } else { + link_lengths_ << 0.0945 + 0.0765, 0.1475 + 0.1025, 0.0965 + 0.1525, 0.03; + } + + const double half_pi = M_PI / 2.0; + dh_params_ = Eigen::MatrixXd(7, 4); + dh_params_ << link_lengths_[0], -half_pi, 0.0, 0.0, + 0.0, half_pi, 0.0, 0.0, + link_lengths_[1], -half_pi, 0.0, 0.0, + 0.0, half_pi, 0.0, 0.0, + link_lengths_[2], -half_pi, 0.0, half_pi, + 0.0, half_pi, 0.0, half_pi, + link_lengths_[3], 0.0, 0.0, 0.0; + + d_bs_ = link_lengths_[0]; + d_se_ = link_lengths_[1]; + d_ew_ = link_lengths_[2]; + d_wt_ = link_lengths_[3]; + + joints_limits_.clear(); + if (cfg_.joint_lower_limits_size() == 7 && cfg_.joint_upper_limits_size() == 7) { + joints_limits_.reserve(7); + for (int i = 0; i < 7; ++i) { + joints_limits_.emplace_back(cfg_.joint_lower_limits(i), cfg_.joint_upper_limits(i)); + } + } else { + joints_limits_ = { + {-M_PI, M_PI}, + {-0.78, 1.57}, + {-M_PI, M_PI}, + {0.0, 2.05}, + {-M_PI, M_PI}, + {-0.78, 0.78}, + {-0.26, 1.57}, + }; + } + + return true; +} + +bool SrsIKSolver::ik(const Eigen::Matrix4d& target_pose, + std::vector& joints_angle, + bool is_tcp) +{ + (void)is_tcp; + return ikWithPsi(target_pose, joints_angle, psi_); +} + +bool SrsIKSolver::fk(const std::vector& joints_angle, + Eigen::Matrix4d& cur_pose, + bool is_tcp) +{ + (void)is_tcp; + if (joints_angle.size() < 7) { + return false; + } + cur_pose = calc_total_transform(joints_angle); + return true; +} + +SrsIKSolver::ConfigDirection SrsIKSolver::toConfigDirection_(const int value) +{ + return value < 0 ? INWARD : OUTWARD; +} + +Eigen::Matrix3d SrsIKSolver::reference_plane(const Eigen::Vector3d& S, + const Eigen::Vector3d& W) +{ + const double d_sw = (W - S).norm(); + const Eigen::Vector3d v_sw = (W - S).normalized(); + + const double x = (d_sw * d_sw + d_se_ * d_se_ - d_ew_ * d_ew_) / (2.0 * d_sw); + const double r = std::sqrt(std::max(d_se_ * d_se_ - x * x, 0.0)); + const Eigen::Vector3d F = S + x * v_sw; + + Eigen::Vector3d FE; + if (v_sw.head<2>().cwiseAbs().maxCoeff() <= 1e-6) { + FE = Eigen::Vector3d(-1.0, 0.0, 0.0); + } else { + FE(0) = -v_sw(0) * v_sw(2) / (v_sw(0) * v_sw(0) + v_sw(1) * v_sw(1)); + FE(1) = -v_sw(1) * v_sw(2) / (v_sw(0) * v_sw(0) + v_sw(1) * v_sw(1)); + FE(2) = 1.0; + } + + const Eigen::Vector3d E = F + elbow_config_ * r * FE.normalized(); + const Eigen::Vector3d v_es = (S - E).normalized(); + const Eigen::Vector3d v_ew = (W - E).normalized(); + + const Eigen::Vector3d R30_y = v_es; + Eigen::Vector3d R30_z = v_ew.cross(v_es); + + if (elbow_config_ == INWARD) { + R30_z = -R30_z; + } + + const double nz = R30_z.norm(); + if (nz > 1e-12) { + R30_z /= nz; + } else { + R30_z = Eigen::Vector3d(-0.0, 1.0, 0.0); + } + + const Eigen::Vector3d R30_x = R30_y.cross(R30_z); + + Eigen::Matrix3d R30; + R30.col(0) = R30_x; + R30.col(1) = R30_y; + R30.col(2) = R30_z; + + return R30; +} + +bool SrsIKSolver::ikWithPsi(const Eigen::Matrix4d& pose, + std::vector& joints, + double psi) +{ + joints.resize(7, 0.0); + + const Eigen::Vector3d P_target = pose.block<3, 1>(0, 3); + const Eigen::Vector3d S(0.0, 0.0, d_bs_); + const Eigen::Vector3d P67(0.0, 0.0, d_wt_); + const Eigen::Vector3d W = P_target - pose.block<3, 3>(0, 0) * P67; + + const double d_sw = (W - S).norm(); + const double r_max = d_se_ + d_ew_; + const double r_min = std::abs(d_se_ - d_ew_); + const double diff_max = d_sw - r_max; + const double diff_min = r_min - d_sw; + if (diff_max > EPS || diff_min > EPS) { + CMVR_LOG(ERROR) << "[SrsIKSolver] pose outside reachable workspace, IK solve failed"; + return false; + } + + double cos_elbow = (d_se_ * d_se_ + d_ew_ * d_ew_ - d_sw * d_sw) / (2.0 * d_se_ * d_ew_); + cos_elbow = std::clamp(cos_elbow, -1.0, 1.0); + joints[3] = elbow_config_ * (M_PI - std::acos(cos_elbow)); + + const Eigen::Matrix3d R30 = reference_plane(S, W); + const Eigen::Matrix3d R_axis = calc_rotation_matrix((W - S).normalized(), psi); + const Eigen::Matrix3d R3 = R_axis * R30; + + double k = shoulder_config_; + const double c2 = std::clamp(-R3(2, 1), -1.0, 1.0); + constexpr double eps = 1e-8; + + if (std::fabs(c2 - 1.0) < eps) { + joints[1] = 0.0 * k; + joints[0] = 0.0; + joints[2] = std::atan2(k * R3(1, 0), k * R3(0, 0)); + } else if (std::fabs(c2 + 1.0) < eps) { + joints[1] = k * M_PI; + joints[2] = std::atan2(k * R3(1, 0), k * R3(1, 2)); + joints[0] = 0.0; + } else { + joints[0] = std::atan2(-k * R3(1, 1), -k * R3(0, 1)); + joints[1] = k * std::acos(c2); + joints[2] = std::atan2(k * R3(2, 2), -k * R3(2, 0)); + } + + Eigen::Matrix3d R04 = Eigen::Matrix3d::Identity(); + for (int i = 0; i < 4; ++i) { + const Eigen::Vector4d dh = dh_params_.row(i); + R04 = R04 * calc_dh(dh[0], dh[1], dh[2], dh[3] + joints[i]).block<3, 3>(0, 0); + } + + const Eigen::Matrix3d R47 = R04.transpose() * pose.block<3, 3>(0, 0); + k = wrist_config_; + + const double c = std::clamp(R47(2, 2), -1.0, 1.0); + double theta_y = k * std::acos(c); + double phi_z = std::atan2(k * R47(1, 2), k * R47(0, 2)); + double psi_z = std::atan2(k * R47(2, 1), -k * R47(2, 0)); + + if (std::fabs(c - 1.0) < eps) { + theta_y = 0.0; + phi_z = std::atan2(k * R47(1, 0), k * R47(0, 0)); + psi_z = 0.0; + } else if (std::fabs(c + 1.0) < eps) { + theta_y = k * M_PI; + phi_z = std::atan2(-k * R47(1, 0), -k * R47(0, 0)); + psi_z = 0.0; + } + + joints[4] = SupportFunctions::normalize_angle(phi_z - M_PI / 2.0); + joints[5] = SupportFunctions::normalize_angle(theta_y - M_PI / 2.0); + joints[6] = SupportFunctions::normalize_angle(psi_z); + + psi_ = psi; + return true; +} + +Eigen::Matrix3d SrsIKSolver::calc_rotation_matrix(const Eigen::Vector3d& rotation_axis, + double rotation_angle) +{ + const Eigen::Vector3d normalized_axis = rotation_axis.normalized(); + const double ux = normalized_axis[0]; + const double uy = normalized_axis[1]; + const double uz = normalized_axis[2]; + + Eigen::Matrix3d u_hat; + u_hat << 0.0, -uz, uy, + uz, 0.0, -ux, + -uy, ux, 0.0; + + return Eigen::Matrix3d::Identity() + + std::sin(rotation_angle) * u_hat + + (1.0 - std::cos(rotation_angle)) * (u_hat * u_hat); +} + +Eigen::Matrix4d SrsIKSolver::calc_dh(double d, double alpha, double a, double theta) +{ + const double ca = std::cos(alpha); + const double sa = std::sin(alpha); + const double ct = std::cos(theta); + const double st = std::sin(theta); + + Eigen::Matrix4d T; + T << ct, -st * ca, st * sa, a * ct, + st, ct * ca, -ct * sa, a * st, + 0.0, sa, ca, d, + 0.0, 0.0, 0.0, 1.0; + + return T; +} + +Eigen::Matrix4d SrsIKSolver::calc_total_transform(const std::vector& joint_angles) +{ + Eigen::Matrix4d T_total = Eigen::Matrix4d::Identity(); + if (joint_angles.size() < static_cast(dh_params_.rows())) { + return T_total; + } + + for (int i = 0; i < dh_params_.rows(); ++i) { + const double d = dh_params_(i, 0); + const double alpha = dh_params_(i, 1); + const double a = dh_params_(i, 2); + const double theta0 = dh_params_(i, 3); + T_total = T_total * calc_dh(d, alpha, a, theta0 + joint_angles[i]); + } + + return T_total; +} + +bool SrsIKSolver::cal_coefficient_matrix(const Eigen::Matrix4d& pose, + Eigen::MatrixXd& s_mat, + Eigen::MatrixXd& w_mat) +{ + if (s_mat.rows() != 3 || s_mat.cols() != 9) { + s_mat.setZero(3, 9); + } + if (w_mat.rows() != 3 || w_mat.cols() != 9) { + w_mat.setZero(3, 9); + } + + std::vector joints(7, 0.0); + const Eigen::Vector3d P_target = pose.block<3, 1>(0, 3); + const Eigen::Vector3d S(0.0, 0.0, d_bs_); + const Eigen::Vector3d P67(0.0, 0.0, d_wt_); + const Eigen::Vector3d W = P_target - pose.block<3, 3>(0, 0) * P67; + + const double d_sw = (W - S).norm(); + const double r_max = d_se_ + d_ew_; + const double r_min = std::abs(d_se_ - d_ew_); + const double diff_max = d_sw - r_max; + const double diff_min = r_min - d_sw; + if (diff_max > EPS || diff_min > EPS) { + CMVR_LOG(ERROR) << "[SrsIKSolver] pose outside reachable workspace, IK solve failed"; + return false; + } + + double cos_elbow = (d_se_ * d_se_ + d_ew_ * d_ew_ - d_sw * d_sw) / (2.0 * d_se_ * d_ew_); + cos_elbow = std::clamp(cos_elbow, -1.0, 1.0); + joints[3] = elbow_config_ * (M_PI - std::acos(cos_elbow)); + + const Eigen::Matrix3d R30 = reference_plane(S, W); + const Eigen::Vector3d normalized_axis = (W - S).normalized(); + + const double ux = normalized_axis[0]; + const double uy = normalized_axis[1]; + const double uz = normalized_axis[2]; + Eigen::Matrix3d u_hat; + u_hat << 0.0, -uz, uy, + uz, 0.0, -ux, + -uy, ux, 0.0; + + const Eigen::MatrixXd A_s = u_hat * R30; + const Eigen::MatrixXd B_s = -u_hat * u_hat * R30; + const Eigen::MatrixXd C_s = (Eigen::MatrixXd::Identity(3, 3) + u_hat * u_hat) * R30; + + const Eigen::MatrixXd T34 = calc_dh(dh_params_(3, 0), + dh_params_(3, 1), + dh_params_(3, 2), + dh_params_(3, 3) + joints[3]); + const Eigen::MatrixXd R34 = T34.block(0, 0, 3, 3); + const Eigen::MatrixXd A_w = R34.transpose() * A_s.transpose() * pose.block(0, 0, 3, 3); + const Eigen::MatrixXd B_w = R34.transpose() * B_s.transpose() * pose.block(0, 0, 3, 3); + const Eigen::MatrixXd C_w = R34.transpose() * C_s.transpose() * pose.block(0, 0, 3, 3); + + s_mat.block<3, 3>(0, 0) = A_s; + s_mat.block<3, 3>(0, 3) = B_s; + s_mat.block<3, 3>(0, 6) = C_s; + + w_mat.block<3, 3>(0, 0) = A_w; + w_mat.block<3, 3>(0, 3) = B_w; + w_mat.block<3, 3>(0, 6) = C_w; + + return true; +} + +} // namespace cmvr diff --git a/cmvr-es/ik_solver/src/ik_test.cpp b/cmvr-es/algorithms/kinematics/ik_solver/tests/src/ik_test.cpp similarity index 87% rename from cmvr-es/ik_solver/src/ik_test.cpp rename to cmvr-es/algorithms/kinematics/ik_solver/tests/src/ik_test.cpp index 85eb5b57..000f9daf 100644 --- a/cmvr-es/ik_solver/src/ik_test.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/tests/src/ik_test.cpp @@ -13,12 +13,51 @@ #include "pinocchio/multibody/sample-models.hpp" #include #include "gtest/gtest.h" -#include "ik_solver/include/pinocchio_dls_ik_solver.h" -#include "ik_solver/include/pinocchio_qp_ik_solver.h" -#include "ik_solver/include/lawba_ik_solver.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h" +#include "algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.h" #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" using namespace cmvr; +namespace { + +constexpr const char* kDefaultUrdf = + "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf"; +constexpr const char* kDefaultBaseFrame = "PELVIS_S"; +constexpr const char* kDefaultFlangeFrame = "R_WRIST_R_S"; +constexpr const char* kDefaultTcpFrame = "R_FINGER_TIP_FIXED"; + +config::PinocchioDlsIKConfig makeDlsConfig() +{ + config::PinocchioDlsIKConfig cfg; + cfg.set_urdf_path(kDefaultUrdf); + cfg.set_base_frame_name(kDefaultBaseFrame); + cfg.set_flange_frame_name(kDefaultFlangeFrame); + cfg.set_tcp_frame_name(kDefaultTcpFrame); + cfg.set_max_iters(100); + cfg.set_pos_eps(1e-6); + cfg.set_rot_eps(1e-6); + cfg.set_damping(1e-4); + return cfg; +} + +config::PinocchioQpIKConfig makeQpConfig() +{ + config::PinocchioQpIKConfig cfg; + cfg.set_urdf_path(kDefaultUrdf); + cfg.set_base_frame_name(kDefaultBaseFrame); + cfg.set_flange_frame_name(kDefaultFlangeFrame); + cfg.set_tcp_frame_name(kDefaultTcpFrame); + cfg.set_lambda(1e-4); + cfg.set_w_posrot(0.5); + cfg.set_max_iters(100); + cfg.set_tol(1e-6); + cfg.set_qp_time_limit(1e-2); + return cfg; +} + +} // namespace + class DualArmViewer : public MuJocoViewer { public: @@ -185,28 +224,18 @@ void benchmarkIkSolversRandomJoints(DualArmViewer &viewer) {-0.26, 1.57}, }}; - constexpr int N_SAMPLES = 10; // 样本数：1000 组随机关节角 + constexpr int N_SAMPLES = 10; // ========== 2. 创建三个求解器实例 ========== // 数值优化类 QP IK - PinocchioQpIKSolver qp_solver( - "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf", - "PELVIS_S", - "R_WRIST_R_S", // 法兰 frame - "R_FINGER_TIP_FIXED" // TCP frame - ); + PinocchioQpIKSolver qp_solver(makeQpConfig()); // 基于广义逆雅可比矩阵的数值增量 IK - PinocchioDlsIKSolver pinv_solver( - "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf", - "PELVIS_S", - "R_WRIST_R_S", // 法兰 frame - "R_FINGER_TIP_FIXED" // TCP frame - ); + PinocchioDlsIKSolver pinv_solver(makeDlsConfig()); // 你的专利方法：臂角 ψ + 可行域 + 势场 - LawbaIKSolver psi_solver; + LawbaIKSolver psi_solver(config::LawbaIKConfig{}); // 统一容器，方便 for 循环 std::vector solvers = { @@ -236,6 +265,8 @@ void benchmarkIkSolversRandomJoints(DualArmViewer &viewer) // ========== 5. 主循环：随机关节角 → FK → 三种 IK ========== // 这里用 PinocchioDlsIKSolver (pinv_solver) 的 FK 作为“真值” for (int i = 0; i < N_SAMPLES; ++i) { + std::cout << "[ik_compare] sample " << (i + 1) << "/" << N_SAMPLES << std::endl; + // 5.1 随机生成一组 q_true std::vector q_true(7); for (int j = 0; j < 7; ++j) { @@ -308,7 +339,7 @@ void benchmarkIkSolversRandomJoints(DualArmViewer &viewer) } viewer.moveJ(q_sol); - std::this_thread::sleep_for(std::chrono::duration(1)); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); double dt_ms = std::chrono::duration_cast< std::chrono::microseconds>(t1 - t0).count() / 1000.0; @@ -395,34 +426,24 @@ void benchmarkIkSolversRandomJoints() }}; - constexpr int N_SAMPLES = 100; // 样本数：1000 组随机关节角 + constexpr int N_SAMPLES = 10; // ========== 2. 创建三个求解器实例 ========== // 数值优化类 QP IK - PinocchioQpIKSolver qp_solver( - "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf", - "PELVIS_S", - "R_WRIST_R_S", // 法兰 frame - "R_FINGER_TIP_FIXED" // TCP frame - ); + PinocchioQpIKSolver qp_solver(makeQpConfig()); // 基于广义逆雅可比矩阵的数值增量 IK - PinocchioDlsIKSolver pinv_solver( - "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf", - "PELVIS_S", - "R_WRIST_R_S", // 法兰 frame - "R_FINGER_TIP_FIXED" // TCP frame - ); + PinocchioDlsIKSolver pinv_solver(makeDlsConfig()); // 你的专利方法：臂角 ψ + 可行域 + 势场 - LawbaIKSolver psi_solver; + LawbaIKSolver psi_solver(config::LawbaIKConfig{}); // 统一容器，方便 for 循环 std::vector solvers = { - // &psi_solver, + &psi_solver, &pinv_solver, - // &qp_solver + &qp_solver }; @@ -446,6 +467,8 @@ void benchmarkIkSolversRandomJoints() // ========== 5. 主循环：随机关节角 → FK → 三种 IK ========== // 这里用 PinocchioDlsIKSolver (pinv_solver) 的 FK 作为“真值” for (int i = 0; i < N_SAMPLES; ++i) { + std::cout << "[ik_compare] sample " << (i + 1) << "/" << N_SAMPLES << std::endl; + // 5.1 随机生成一组 q_true std::vector q_true(7); for (int j = 0; j < 7; ++j) { @@ -586,30 +609,23 @@ void benchmarkIkSolversRandomJoints() } TEST(ik_test,pinocchio_lib_test) { - // PinocchioQpIKSolver solver( - // "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf", - // "PELVIS_S", - // "R_WRIST_R_S", // 法兰 frame - // "R_FINGER_TIP_FIXED" // TCP frame - // ); - - LawbaIKSolver solver; - solver.init(); + PinocchioQpIKSolver solver(makeQpConfig()); + // PinocchioDlsIKSolver solver(makeDlsConfig()); + ASSERT_TRUE(solver.init()); // 真实关节角（比如从控制器读回来） std::vector q_cur(7, 0.0); - solver.update_joints_state(q_cur); - q_cur[5] = 0.236; q_cur[4] = -0.236; q_cur[3] = 0.156; q_cur[2] = 0.036; q_cur[1] = 0.236; + solver.update_joints_state(q_cur); // 1) 先求当前 TCP 位姿 - Eigen::Matrix4d cur_tcp_pose; - solver.fk(q_cur, cur_tcp_pose, false); // is_tcp = true + Eigen::Matrix4d cur_tcp_pose = Eigen::Matrix4d::Identity(); + ASSERT_TRUE(solver.fk(q_cur, cur_tcp_pose, false)); std::cout << cur_tcp_pose<< std::endl; @@ -617,45 +633,29 @@ TEST(ik_test,pinocchio_lib_test) { std::vector q_target; - bool ok = solver.ik(target_tcp_pose, q_target, false); // is_tcp = true + ASSERT_TRUE(solver.ik(target_tcp_pose, q_target, false)); for (double q: q_target) { std::cout << q << std::endl; } - Eigen::Matrix4d cur_flange_pose; - solver.fk(q_target, cur_flange_pose, false); // is_tcp = false + Eigen::Matrix4d cur_flange_pose = Eigen::Matrix4d::Identity(); + ASSERT_TRUE(solver.fk(q_target, cur_flange_pose, false)); std::cout << cur_flange_pose<< std::endl; } TEST(ik_test,ik_compare) { - // benchmarkIkSolversRandomJoints(); - - - - const char *model_path = - "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml"; + constexpr const char* model_path = + "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml"; DualArmViewer viewer(model_path); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - // int viewer = 0; - // 把所有 IK 运算 + moveJ 循环放到控制线程里 - std::thread ctrl_thread([&viewer]() { + std::thread benchmark_thread([&viewer]() { + // viewer.run() 在主线程创建仿真和渲染资源，稍后再开始发送关节目标。 + std::this_thread::sleep_for(std::chrono::seconds(1)); benchmarkIkSolversRandomJoints(viewer); - - // - // // 可视化：右臂关节位置控制 - // viewer.moveJ(q); // 更新目标角 - // std::this_thread::sleep_for(std::chrono::duration(0.01)); + std::cout << "[ik_compare] benchmark finished; close the viewer to exit." << std::endl; }); - - viewer.run(); // 阻塞，直到你关掉窗口 - ctrl_thread.join(); // 控制线程结束 + viewer.run(); + benchmark_thread.join(); } - - - - - diff --git a/cmvr-es/ik_solver/src/srs_ik_test.cpp b/cmvr-es/algorithms/kinematics/ik_solver/tests/src/srs_ik_test.cpp similarity index 91% rename from cmvr-es/ik_solver/src/srs_ik_test.cpp rename to cmvr-es/algorithms/kinematics/ik_solver/tests/src/srs_ik_test.cpp index 854d46b3..4abc96e7 100644 --- a/cmvr-es/ik_solver/src/srs_ik_test.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/tests/src/srs_ik_test.cpp @@ -2,11 +2,11 @@ // Created by lgv on 2025/11/3. // -#include "ik_solver/include/pinocchio_dls_ik_solver.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" #include "gtest/gtest.h" -#include "ik_solver/include/bias_srs_ik_slover.h" -#include "ik_solver/include/joints_limit_analyzer.h" -#include "ik_solver/include/opt_psi_selector.h" +#include "algorithms/kinematics/ik_solver/srs/include/srs_ik_solver.h" +#include "algorithms/kinematics/ik_solver/lawba/include/joints_limit_analyzer.h" +#include "algorithms/kinematics/ik_solver/lawba/include/opt_psi_selector.h" #include #include #include @@ -16,19 +16,60 @@ #include #include #include +#include #include #include #include -#include "ik_solver/include/lawba_ik_solver.h" +#include "algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.h" +#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h" +#include "common/math/transform_math.h" #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" -#include "planner/joint_space_planner/include/toppra_bspline.h" -#include "planner/joint_space_planner/include/joint_space_planner_creator.h" -#include "common/math/include/support_functions.h" -#include "ik_solver/include/pinocchio_qp_ik_solver.h" +#include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h" +#include "common/math/support_functions.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h" #include using namespace cmvr; +namespace { + +constexpr const char* kDefaultUrdf = + "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf"; +constexpr const char* kDefaultBaseFrame = "PELVIS_S"; +constexpr const char* kDefaultFlangeFrame = "R_WRIST_R_S"; +constexpr const char* kDefaultTcpFrame = "R_FINGER_TIP_FIXED"; + +config::PinocchioDlsIKConfig makeDlsConfig() +{ + config::PinocchioDlsIKConfig cfg; + cfg.set_urdf_path(kDefaultUrdf); + cfg.set_base_frame_name(kDefaultBaseFrame); + cfg.set_flange_frame_name(kDefaultFlangeFrame); + cfg.set_tcp_frame_name(kDefaultTcpFrame); + cfg.set_max_iters(100); + cfg.set_pos_eps(1e-6); + cfg.set_rot_eps(1e-6); + cfg.set_damping(1e-4); + return cfg; +} + +config::PinocchioQpIKConfig makeQpConfig() +{ + config::PinocchioQpIKConfig cfg; + cfg.set_urdf_path(kDefaultUrdf); + cfg.set_base_frame_name(kDefaultBaseFrame); + cfg.set_flange_frame_name(kDefaultFlangeFrame); + cfg.set_tcp_frame_name(kDefaultTcpFrame); + cfg.set_lambda(1e-4); + cfg.set_w_posrot(0.5); + cfg.set_max_iters(100); + cfg.set_tol(1e-6); + cfg.set_qp_time_limit(1e-2); + return cfg; +} + +} // namespace + struct IkSample { double psi; @@ -347,11 +388,11 @@ private: // int viewer = 0; // // 把所有 IK 运算 + moveJ 循环放到控制线程里 // std::thread ctrl_thread([&viewer]() { -// BiasSRSIkSolver slover; +// SrsIKSolver slover(config::SrsIKConfig{}); // std::vector samples; // samples.reserve(4096); // -// slover.set_shoulder_config(BiasSRSIkSolver::INWARD); +// slover.set_shoulder_config(SrsIKSolver::INWARD); // // std::vector joint_angles(7, 0); // // joint_angles = {0.875, 0.22, 0.2644, M_PI / 2, 1.8, 1.99, 1.56}; @@ -431,7 +472,7 @@ private: // // double psi = psi_vals[idx]; // -// auto q = slover.inverse_kinematics(target_pose, psi); +// auto q = slover.ikWithPsi(target_pose, psi); // if (q.size() != 7 || std::any_of(q.begin(), q.end(), // [](double v) { return !std::isfinite(v); })) { // ++bad; @@ -522,7 +563,7 @@ private: // // // std::cout << std::fixed << std::setprecision(7); // -// BiasSRSIkSolver slover; +// SrsIKSolver slover(config::SrsIKConfig{}); // std::vector samples; // samples.reserve(4096); // @@ -572,7 +613,7 @@ private: // }; // // // IK 解 -// auto q = slover.inverse_kinematics(target_pose, best_psi); +// auto q = slover.ikWithPsi(target_pose, best_psi); // // for (double q1: q) { // std::cout << q1 << " , "; @@ -836,7 +877,7 @@ TEST(SRS_IK_TEST, MOVE_L_PLANNER_TEST) { // 等 MuJoCo / OpenGL 初始化好 std::this_thread::sleep_for(3s); - LawbaIKSolver solver; + LawbaIKSolver solver(config::LawbaIKConfig{}); // =============== 1) 设置初始关节状态 =============== std::vector joint_angles(7, 0.0); @@ -937,8 +978,8 @@ TEST(SRS_IK_TEST, MOVE_L_PLANNER_TEST) { return; } - // =============== 5) 使用 JointSpacePlanner 对 IK 路点做时间参数化 =============== - auto planner = JointSpacePlannerCreator::create(JointSpacePlannerType::TOPPRA_BSPLINE); + // =============== 5) 使用 JointTrajectoryPlanner 对 IK 路点做时间参数化 =============== + auto planner = std::make_shared(); planner->setPathType(PathType::Natural); // 按自己实际的关节约束改 @@ -993,7 +1034,7 @@ TEST(SRS_IK_TEST, TR_TEST) { using std::endl; - PinocchioQpIKSolver solver; + PinocchioDlsIKSolver solver(makeDlsConfig()); solver.init(); Eigen::Matrix4d T; T << 9.99998311e-01, -1.78940420e-03, 4.20611000e-04, 3.83367005e-02, @@ -1235,15 +1276,8 @@ TEST(SRS_IK_TEST, MOVEL_S_CURVE_LOCAL_RUN_MUJOCO) { using namespace std::chrono_literals; std::this_thread::sleep_for(3s); - cmvr::PinocchioDlsIKSolver solver( - "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf", - "PELVIS_S", - "R_WRIST_R_S", - "R_FINGER_TIP_FIXED" - ); - - LawbaIKSolver solver_1; - if (!solver.init()) return; + auto solver = std::make_shared(makeDlsConfig()); + if (!solver->init()) return; // 起点关节 // std::vector q_start = {-0.424743, 0.759386, 1.80129, 2.03728, -1.34668, 0.0560845, -0.26}; @@ -1254,14 +1288,14 @@ TEST(SRS_IK_TEST, MOVEL_S_CURVE_LOCAL_RUN_MUJOCO) { // 起点位姿（base下） Eigen::Matrix4d T0; - if (!solver_1.fk(q_start, T0, true)) return; + if (!solver->fk(q_start, T0, true)) return; std::cout << T0 << std::endl; // 目标位姿：base X 方向走 0.25m，姿态保持起点 Eigen::Matrix4d Tg = T0; // Tg(0,3) += 0.2; - Tg(2,3) += 0.2; + Tg(1,3) -= 0.2; Eigen::Vector3d dp_check = Tg.block<3,1>(0,3) - T0.block<3,1>(0,3); std::cerr << "dp(base)=" << dp_check.transpose() << "\n"; @@ -1272,15 +1306,29 @@ TEST(SRS_IK_TEST, MOVEL_S_CURVE_LOCAL_RUN_MUJOCO) { const double j_tcp = 100.00; std::vector qd_max(7, 3.0); - std::vector> q_traj; - std::vector t_traj; - - - if (!solver.moveL_SCurveLocal(Tg, q_start, q_traj, t_traj, - dt_gen, v_tcp, a_tcp, j_tcp, qd_max, true)) { - std::cerr << "moveL_SCurveLocal failed\n"; + cmvr::device::PinocchioDlsCartesianMotionPlanner planner(solver); + cmvr::config::MoveLPlannerConfig move_l_config; + move_l_config.set_sample_period_s(dt_gen); + move_l_config.set_position_gain(4.0); + move_l_config.set_rotation_gain(4.0); + if (!planner.configureMoveL(move_l_config)) { + std::cerr << "configureMoveL failed\n"; return; } + cmvr::device::CartesianJointTrajectory trajectory; + if (!planner.planMoveL(cmvr::common::math::matrixToPose(Tg), + q_start, + qd_max, + v_tcp, + a_tcp, + j_tcp, + cmvr::device::FrameType::Base, + trajectory)) { + std::cerr << "planMoveL failed\n"; + return; + } + const auto& q_traj = trajectory.position; + const auto& t_traj = trajectory.time; std::cerr << "traj gen samples=" << q_traj.size() << " T=" << (t_traj.empty()?0.0:t_traj.back()) << "s\n"; @@ -1288,7 +1336,7 @@ TEST(SRS_IK_TEST, MOVEL_S_CURVE_LOCAL_RUN_MUJOCO) { write_qtraj_csv(csv_path, t_traj, q_traj); std::vector p, v, a; - if (build_tcp_pva(solver, t_traj, q_traj, p, v, a, true)) { + if (build_tcp_pva(*solver, t_traj, q_traj, p, v, a, true)) { write_tcp_pva_csv(tcp_csv_path, t_traj, p, v, a); std::cerr << "saved tcp csv: " << tcp_csv_path << "\n"; } else { @@ -1297,7 +1345,7 @@ TEST(SRS_IK_TEST, MOVEL_S_CURVE_LOCAL_RUN_MUJOCO) { std::cerr << "saved csv: " << csv_path << "\n"; // 正确的 TCP 速度统计（用 t_traj） - print_tcp_speed_stats(solver, t_traj, q_traj, true); + print_tcp_speed_stats(*solver, t_traj, q_traj, true); // 直接按采样时刻逐点下发位置目标，不在 viewer 内做轨迹缓存和插值。 const auto t0 = std::chrono::steady_clock::now(); @@ -1425,13 +1473,10 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) { std::cerr << msg << "\n"; }; - cmvr::PinocchioDlsIKSolver solver( - urdf_path, - "PELVIS_S", - "R_WRIST_R_S", - "R_FINGER_TIP_FIXED" - ); - if (!solver.init()) { + auto dls_cfg = makeDlsConfig(); + dls_cfg.set_urdf_path(urdf_path); + auto solver = std::make_shared(dls_cfg); + if (!solver->init()) { fail("speedL test: solver.init() failed"); return; } @@ -1449,23 +1494,25 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) { std::this_thread::sleep_for(1s); const std::vector q_init = viewer.getQ(); - solver.update_joints_state(q_init); + solver->update_joints_state(q_init); std::vector q_ref = q_init; - cmvr::PinocchioDlsIKSolver::SpeedLConfig speedl_config; - speedl_config.linear_velocity_max = 0.55; - speedl_config.linear_acceleration_max = 0.80; - speedl_config.linear_jerk_max = 3.30; - speedl_config.angular_velocity_max = 1.00; - speedl_config.angular_acceleration_max = 3.00; - speedl_config.angular_jerk_max = 12.0; - speedl_config.joint_acceleration_max = std::vector(7, 8.0); - speedl_config.linear_target_replan_threshold = 1e-4; - speedl_config.angular_target_replan_threshold = 1e-4; - speedl_config.linear_reverse_cos_threshold = -0.8660254037844386; - speedl_config.linear_reverse_switch_speed_threshold = 1e-3; - // speedl_config.qdot_measurement_lowpass_alpha = 0.5; - if (!solver.configureSpeedL(speedl_config)) { + cmvr::config::SpeedLPlannerConfig speedl_config; + speedl_config.set_linear_velocity_max(0.55); + speedl_config.set_linear_acceleration_max(0.80); + speedl_config.set_linear_jerk_max(3.30); + speedl_config.set_angular_velocity_max(1.00); + speedl_config.set_angular_acceleration_max(3.00); + speedl_config.set_angular_jerk_max(12.0); + for (int i = 0; i < 7; ++i) { + speedl_config.add_joint_acceleration_max(8.0); + } + speedl_config.set_linear_target_replan_threshold(1e-4); + speedl_config.set_angular_target_replan_threshold(1e-4); + speedl_config.set_linear_reverse_cos_threshold(-0.8660254037844386); + speedl_config.set_linear_reverse_switch_speed_threshold(1e-3); + cmvr::device::PinocchioDlsCartesianMotionPlanner planner(solver); + if (!planner.configureSpeedL(speedl_config, q_init.size())) { fail("speedL test: configureSpeedL() failed"); return; } @@ -1487,11 +1534,17 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) { tcp_omega_traj.reserve(reserve_count); auto record_sample = [&](double t_sample, - const std::vector& q_sample) -> bool { + const std::vector& q_sample, + const std::vector& qd_sample) -> bool { q_traj.push_back(q_sample); t_traj.push_back(t_sample); - twist_cmd_traj.push_back(solver.getSpeedLCommandTwistBase()); - const auto twist_exec = solver.getSpeedLExecutedTwistBase(); + twist_cmd_traj.push_back( + cmvr::common::math::velocityToVector(planner.getSpeedLCommandTwistBase())); + Eigen::Matrix twist_exec = Eigen::Matrix::Zero(); + if (!solver->computeTwistBaseAtQ(q_sample, qd_sample, true, twist_exec)) { + fail("speedL test: failed to compute executed TCP twist"); + return false; + } Eigen::Vector3d tcp_pos = Eigen::Vector3d::Zero(); Eigen::Vector3d tcp_vel = Eigen::Vector3d::Zero(); @@ -1535,21 +1588,21 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) { Eigen::Matrix::Zero(); if (t < segment_time) { target_twist[1] = linear_speed_cmd; - target_twist[2] = 0.1; + target_twist[2] = 0; } else if (t < 2.0 * segment_time) { target_twist[1] = -linear_speed_cmd; - target_twist[2] = -0.1; + target_twist[2] = 0; } else { target_twist.setZero(); } std::vector qd_cmd; - if (!solver.speedLStep(target_twist, - dt, - q_meas, - qd_cmd, - cmvr::CartesianFrame::Base, - true)) { + if (!planner.speedLStep(cmvr::common::math::vectorToVelocity(target_twist), + dt, + q_meas, + qd_meas, + qd_cmd, + cmvr::device::FrameType::Base)) { viewer.moveJ(q_meas); fail("speedL test: speedLStep() failed"); return; @@ -1564,7 +1617,7 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) { std::min(q_ref[j], q_meas[j] + max_q_ref_tracking_error)); } viewer.moveJ(q_ref); - if (!record_sample(t, q_meas)) { + if (!record_sample(t, q_meas, qd_meas)) { return; } } @@ -1583,12 +1636,12 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) { Eigen::Matrix::Zero(); std::vector qd_cmd; - if (!solver.speedLStep(target_twist, - dt, - q_meas, - qd_cmd, - cmvr::CartesianFrame::Base, - true)) { + if (!planner.speedLStep(cmvr::common::math::vectorToVelocity(target_twist), + dt, + q_meas, + qd_meas, + qd_cmd, + cmvr::device::FrameType::Base)) { viewer.moveJ(q_meas); fail("speedL test: speedLStep() failed during stop phase"); return; @@ -1603,7 +1656,7 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) { std::min(q_ref[j], q_meas[j] + max_q_ref_tracking_error)); } viewer.moveJ(q_ref); - if (!record_sample(t, q_meas)) { + if (!record_sample(t, q_meas, qd_meas)) { return; } } diff --git a/cmvr-es/algorithms/motion_planner/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/CMakeLists.txt new file mode 100644 index 00000000..aadc080e --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/CMakeLists.txt @@ -0,0 +1,2 @@ +add_subdirectory(base_motion) +add_subdirectory(arm_motion) diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt new file mode 100644 index 00000000..8136c07e --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt @@ -0,0 +1,17 @@ +add_library(arm_motion SHARED + cartesian_motion/pinocchio_dls/src/pinocchio_dls_cartesian_motion_planner.cpp + cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp + joint_motion/toppra/src/toppra_joint_motion_planner.cpp +) + +target_include_directories(arm_motion PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(arm_motion + PUBLIC + cmvr_es::ik_solver + cmvr_es::base_motion +) + +add_library(cmvr_es::arm_motion ALIAS arm_motion) +add_library(cmvr_es::algorithms::arm_motion ALIAS arm_motion) +install(TARGETS arm_motion LIBRARY DESTINATION lib) diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h new file mode 100644 index 00000000..29153eea --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h @@ -0,0 +1,47 @@ +#ifndef CMVR_ES_CARTESIAN_MOTION_PLANNER_H +#define CMVR_ES_CARTESIAN_MOTION_PLANNER_H + +#include + +#include "cmvr/config/arm_config/arm_config.pb.h" +#include "common/types/arm/arm_types.h" + +namespace cmvr::device { + +struct CartesianJointTrajectory { + std::vector> position; + std::vector> velocity; + std::vector time; +}; + +class CartesianMotionPlanner { +public: + virtual ~CartesianMotionPlanner() = default; + + virtual bool configureSpeedL(const config::SpeedLPlannerConfig& config, + std::size_t dof) = 0; + virtual bool configureMoveL(const config::MoveLPlannerConfig& config) = 0; + + virtual bool planMoveL(const CartesianPose& target, + const std::vector& q_start, + const std::vector& qd_max, + double velocity, + double acceleration, + double jerk, + FrameType frame, + CartesianJointTrajectory& trajectory) = 0; + + virtual bool speedLStep(const CartesianVelocity& target_velocity, + double dt, + const std::vector& q_measured, + const std::vector& qd_measured, + std::vector& qd_command, + FrameType frame) = 0; + + virtual bool updateSpeedLAcceleration(double acceleration) = 0; + virtual CartesianVelocity getSpeedLCommandTwistBase() const = 0; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_CARTESIAN_MOTION_PLANNER_H diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h new file mode 100644 index 00000000..0c4eb769 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h @@ -0,0 +1,78 @@ +#ifndef CMVR_ES_CARTESIAN_MOTION_PLANNER_FACTORY_H +#define CMVR_ES_CARTESIAN_MOTION_PLANNER_FACTORY_H + +#include + +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" +#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h" +#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h" +#include "algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h" +#include "cmvr/config/arm_config/arm_config.pb.h" + +namespace cmvr::device { + +class CartesianMotionPlannerFactory { +public: + static std::shared_ptr create( + const config::MoveLConfig& move_l, + const config::SpeedLConfig& speed_l, + const std::shared_ptr& solver) + { + if (!solver) { + return nullptr; + } + switch (move_l.algorithm_case()) { + case config::MoveLConfig::kPinocchioQpCartesianMotionPlanner: + if (speed_l.algorithm_case() != + config::SpeedLConfig::kPinocchioQpCartesianMotionPlanner) { + return nullptr; + } + return std::make_shared(solver); + case config::MoveLConfig::kPinocchioDlsCartesianMotionPlanner: + if (speed_l.algorithm_case() != + config::SpeedLConfig::kPinocchioDlsCartesianMotionPlanner) { + return nullptr; + } + if (auto dls_solver = std::dynamic_pointer_cast(solver)) { + return std::make_shared(dls_solver); + } + return nullptr; + case config::MoveLConfig::ALGORITHM_NOT_SET: + default: + return nullptr; + } + } + + static const config::SpeedLPlannerConfig* speedLConfig( + const config::SpeedLConfig& cfg) + { + switch (cfg.algorithm_case()) { + case config::SpeedLConfig::kPinocchioQpCartesianMotionPlanner: + return &cfg.pinocchio_qp_cartesian_motion_planner(); + case config::SpeedLConfig::kPinocchioDlsCartesianMotionPlanner: + return &cfg.pinocchio_dls_cartesian_motion_planner(); + case config::SpeedLConfig::ALGORITHM_NOT_SET: + default: + return nullptr; + } + } + + static const config::MoveLPlannerConfig* moveLConfig( + const config::MoveLConfig& cfg) + { + switch (cfg.algorithm_case()) { + case config::MoveLConfig::kPinocchioQpCartesianMotionPlanner: + return &cfg.pinocchio_qp_cartesian_motion_planner(); + case config::MoveLConfig::kPinocchioDlsCartesianMotionPlanner: + return &cfg.pinocchio_dls_cartesian_motion_planner(); + case config::MoveLConfig::ALGORITHM_NOT_SET: + default: + return nullptr; + } + } +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_CARTESIAN_MOTION_PLANNER_FACTORY_H diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h new file mode 100644 index 00000000..87730db6 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h @@ -0,0 +1,66 @@ +#ifndef CMVR_ES_PINOCCHIO_DLS_CARTESIAN_MOTION_PLANNER_H +#define CMVR_ES_PINOCCHIO_DLS_CARTESIAN_MOTION_PLANNER_H + +#include + +#include +#include + +#include "../../cartesian_motion_planner.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" +#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" + +namespace cmvr::device { + +class PinocchioDlsCartesianMotionPlanner final : public CartesianMotionPlanner { +public: + explicit PinocchioDlsCartesianMotionPlanner(std::shared_ptr solver); + + bool configureSpeedL(const config::SpeedLPlannerConfig& config, + std::size_t dof) override; + bool configureMoveL(const config::MoveLPlannerConfig& config) override; + + bool planMoveL(const CartesianPose& target, + const std::vector& q_start, + const std::vector& qd_max, + double velocity, + double acceleration, + double jerk, + FrameType frame, + CartesianJointTrajectory& trajectory) override; + + bool speedLStep(const CartesianVelocity& target_velocity, + double dt, + const std::vector& q_measured, + const std::vector& qd_measured, + std::vector& qd_command, + FrameType frame) override; + + bool updateSpeedLAcceleration(double acceleration) override; + CartesianVelocity getSpeedLCommandTwistBase() const override; + +private: + bool refreshJointLimits_(); + Eigen::VectorXd applyJointVelocityLimits_(const Eigen::VectorXd& qdot) const; + Eigen::VectorXd applyJointSoftLimits_(const Eigen::VectorXd& q, + const Eigen::VectorXd& qdot); + Eigen::VectorXd applyJointAccelerationLimits_(const Eigen::VectorXd& qdot, + const Eigen::VectorXd& reference, + double dt) const; + + std::shared_ptr solver_{nullptr}; + config::MoveLPlannerConfig movel_config_{}; + config::SpeedLPlannerConfig speedl_config_{}; + cmvr::CartesianTwistLimiter twist_limiter_{}; + Eigen::VectorXd joint_lower_limits_; + Eigen::VectorXd joint_upper_limits_; + Eigen::VectorXd joint_velocity_limits_; + std::vector prev_qdot_command_; + Eigen::Matrix speedl_command_twist_base_{Eigen::Matrix::Zero()}; + double speedl_applied_acceleration_{0.25}; + bool speedl_configured_{false}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_PINOCCHIO_DLS_CARTESIAN_MOTION_PLANNER_H diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/src/pinocchio_dls_cartesian_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/src/pinocchio_dls_cartesian_motion_planner.cpp new file mode 100644 index 00000000..12a1c988 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/src/pinocchio_dls_cartesian_motion_planner.cpp @@ -0,0 +1,408 @@ +#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h" + +#include + +#include +#include +#include +#include + +#include "algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h" +#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve.h" +#include "common/math/cartesian_motion_math.h" +#include "common/math/joint_limits.h" +#include "common/config/config_files.h" +#include "common/math/transform_math.h" + +namespace cmvr::device { + +using cmvr::common::config::positiveOr; +using cmvr::device::cartesian_motion::clamp; +using cmvr::device::cartesian_motion::directionDeviationDeg; +using cmvr::device::cartesian_motion::rotationVector; +using cmvr::device::cartesian_motion::toEigenVector; +using cmvr::device::cartesian_motion::toStdVector; + +PinocchioDlsCartesianMotionPlanner::PinocchioDlsCartesianMotionPlanner( + std::shared_ptr solver) + : solver_(std::move(solver)) +{ +} + +bool PinocchioDlsCartesianMotionPlanner::refreshJointLimits_() +{ + if (!solver_) { + return false; + } + if (!solver_->getJointPositionLimits(joint_lower_limits_, joint_upper_limits_)) { + return false; + } + if (!solver_->getJointVelocityLimits(joint_velocity_limits_)) { + return false; + } + return true; +} + +Eigen::VectorXd PinocchioDlsCartesianMotionPlanner::applyJointVelocityLimits_( + const Eigen::VectorXd& qdot) const +{ + return cmvr::kinematics::scaleToVelocityLimits(qdot, joint_velocity_limits_); +} + +Eigen::VectorXd PinocchioDlsCartesianMotionPlanner::applyJointSoftLimits_( + const Eigen::VectorXd& q, + const Eigen::VectorXd& qdot) +{ + if (joint_lower_limits_.size() != q.size() || + joint_upper_limits_.size() != q.size() || + qdot.size() != q.size()) { + return qdot; + } + + Eigen::VectorXd limited = qdot; + for (Eigen::Index i = 0; i < q.size(); ++i) { + const double lower = joint_lower_limits_[i]; + const double upper = joint_upper_limits_[i]; + if (!std::isfinite(lower) || !std::isfinite(upper) || upper <= lower) { + continue; + } + + const double span = upper - lower; + const double margin = std::max(0.02, 0.08 * span); + if (limited[i] < 0.0 && q[i] < lower + margin) { + const double ratio = clamp((q[i] - lower) / margin, 0.0, 1.0); + limited[i] *= ratio; + if (q[i] <= lower) { + limited[i] = std::max(0.0, limited[i]); + } + } else if (limited[i] > 0.0 && q[i] > upper - margin) { + const double ratio = clamp((upper - q[i]) / margin, 0.0, 1.0); + limited[i] *= ratio; + if (q[i] >= upper) { + limited[i] = std::min(0.0, limited[i]); + } + } + } + return limited; +} + +Eigen::VectorXd PinocchioDlsCartesianMotionPlanner::applyJointAccelerationLimits_( + const Eigen::VectorXd& qdot, + const Eigen::VectorXd& reference, + const double dt) const +{ + if (reference.size() != qdot.size() || dt <= 0.0) { + return qdot; + } + + Eigen::VectorXd limited = qdot; + for (Eigen::Index i = 0; i < qdot.size(); ++i) { + double acc_limit = 8.0; + if (i < speedl_config_.joint_acceleration_max_size() && + speedl_config_.joint_acceleration_max(static_cast(i)) > 0.0) { + acc_limit = speedl_config_.joint_acceleration_max(static_cast(i)); + } + + const double delta_max = acc_limit * dt; + const double delta = clamp(qdot[i] - reference[i], -delta_max, delta_max); + limited[i] = reference[i] + delta; + } + return limited; +} + +bool PinocchioDlsCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlannerConfig& config, + const std::size_t dof) +{ + if (!solver_ || dof == 0) { + return false; + } + + const auto solver_dof = static_cast(std::max(0, solver_->chainVelocityDof())); + if (solver_dof != 0 && solver_dof != dof) { + return false; + } + + speedl_config_ = config; + + if (!refreshJointLimits_()) { + return false; + } + + cartesian_motion::configureTwistLimiterFromSpeedLConfig(twist_limiter_, speedl_config_); + + prev_qdot_command_.assign(dof, 0.0); + speedl_command_twist_base_.setZero(); + speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0); + speedl_configured_ = true; + return true; +} + +bool PinocchioDlsCartesianMotionPlanner::configureMoveL( + const config::MoveLPlannerConfig& config) +{ + if (!std::isfinite(config.sample_period_s()) || + !std::isfinite(config.position_gain()) || + !std::isfinite(config.rotation_gain())) { + return false; + } + movel_config_ = config; + return true; +} + +bool PinocchioDlsCartesianMotionPlanner::planMoveL(const CartesianPose& target, + const std::vector& q_start, + const std::vector& qd_max, + const double velocity, + const double acceleration, + const double jerk, + const FrameType frame, + CartesianJointTrajectory& trajectory) +{ + trajectory = {}; + const double dt = positiveOr(movel_config_.sample_period_s(), 0.001); + if (!solver_ || q_start.empty() || + velocity <= 0.0 || acceleration <= 0.0 || jerk <= 0.0) { + return false; + } + if (static_cast(q_start.size()) != solver_->chainDof()) { + return false; + } + if (!qd_max.empty() && qd_max.size() != q_start.size()) { + return false; + } + if (!refreshJointLimits_()) { + return false; + } + + Eigen::Matrix4d start_pose_base = Eigen::Matrix4d::Identity(); + if (!solver_->fk(q_start, start_pose_base, true)) { + return false; + } + + const Eigen::Matrix4d target_pose_input = common::math::poseToMatrix(target); + const Eigen::Matrix4d target_pose_base = + frame == FrameType::Tool ? start_pose_base * target_pose_input : target_pose_input; + + const Eigen::Vector3d p_start = start_pose_base.block<3, 1>(0, 3); + const Eigen::Vector3d p_target = target_pose_base.block<3, 1>(0, 3); + const Eigen::Vector3d dp = p_target - p_start; + const double linear_distance = dp.norm(); + + const Eigen::Matrix3d R_start = start_pose_base.block<3, 3>(0, 0); + const Eigen::Matrix3d R_target = target_pose_base.block<3, 3>(0, 0); + const Eigen::Vector3d total_rotation_vector = rotationVector(R_target * R_start.transpose()); + const double angular_distance = total_rotation_vector.norm(); + + const double path_length = linear_distance > 1e-9 ? linear_distance : angular_distance; + trajectory.position.push_back(q_start); + trajectory.velocity.push_back(std::vector(q_start.size(), 0.0)); + trajectory.time.push_back(0.0); + if (path_length <= 1e-9) { + return true; + } + + cmvr::SCurve curve(velocity, acceleration, jerk); + const cmvr::SCurveProfile profile = curve.calculateProfile(0.0, path_length, 0.0, 0.0); + if (profile.total_time <= 0.0) { + return false; + } + + Eigen::VectorXd q_current = toEigenVector(q_start); + Eigen::Vector3d linear_direction = Eigen::Vector3d::Zero(); + if (linear_distance > 1e-9) { + linear_direction = dp / linear_distance; + } + const Eigen::Quaterniond q_start_rot(R_start); + const Eigen::Quaterniond q_target_rot(R_target); + const double position_gain = positiveOr(movel_config_.position_gain(), 4.0); + const double rotation_gain = positiveOr(movel_config_.rotation_gain(), 4.0); + + double previous_time = 0.0; + for (double t = std::min(dt, profile.total_time); + t <= profile.total_time + 1e-9; + t = std::min(t + dt, profile.total_time)) { + const double step_dt = std::max(1e-6, t - previous_time); + previous_time = t; + + const double s = clamp(curve.getPositionAtTime(profile, t), 0.0, path_length); + const double sd = std::max(0.0, curve.getVelocityAtTime(profile, t)); + const double ratio = clamp(s / path_length, 0.0, 1.0); + + const std::vector q_std = toStdVector(q_current); + Eigen::Matrix4d current_pose_base = Eigen::Matrix4d::Identity(); + if (!solver_->fk(q_std, current_pose_base, true)) { + return false; + } + + const Eigen::Vector3d p_current = current_pose_base.block<3, 1>(0, 3); + const Eigen::Vector3d p_desired = p_start + ratio * dp; + Eigen::Matrix target_twist_base = Eigen::Matrix::Zero(); + target_twist_base.head<3>() = + linear_direction * sd + position_gain * (p_desired - p_current); + + if (angular_distance > 1e-9) { + const Eigen::Matrix3d R_current = current_pose_base.block<3, 3>(0, 0); + const Eigen::Matrix3d R_desired = + q_start_rot.slerp(ratio, q_target_rot).toRotationMatrix(); + const Eigen::Vector3d rotation_error = rotationVector(R_desired * R_current.transpose()); + target_twist_base.tail<3>() = + (total_rotation_vector / path_length) * sd + rotation_gain * rotation_error; + } + + Eigen::MatrixXd jacobian_base; + Eigen::Matrix3d base_R_tool; + if (!solver_->computeJacobianBaseAtQ(q_std, true, jacobian_base, base_R_tool)) { + return false; + } + + std::vector qdot_std; + if (!solver_->solveVelocityBase(jacobian_base, + target_twist_base, + q_std, + qdot_std, + std::numeric_limits::infinity())) { + return false; + } + + Eigen::VectorXd qdot = applyJointVelocityLimits_(toEigenVector(qdot_std)); + if (!qd_max.empty()) { + double scale = 1.0; + for (Eigen::Index i = 0; i < qdot.size(); ++i) { + const double limit = std::abs(qd_max[static_cast(i)]); + if (limit <= 0.0 || !std::isfinite(limit)) { + continue; + } + const double value = std::abs(qdot[i]); + if (value > limit) { + scale = std::min(scale, limit / value); + } + } + qdot *= scale; + } + qdot = applyJointSoftLimits_(q_current, qdot); + q_current += qdot * step_dt; + + if (joint_lower_limits_.size() == q_current.size() && + joint_upper_limits_.size() == q_current.size()) { + q_current = q_current.cwiseMax(joint_lower_limits_).cwiseMin(joint_upper_limits_); + } + + trajectory.position.push_back(toStdVector(q_current)); + trajectory.velocity.push_back(toStdVector(qdot)); + trajectory.time.push_back(t); + + if (t >= profile.total_time - 1e-9) { + break; + } + } + + return true; +} + +bool PinocchioDlsCartesianMotionPlanner::speedLStep(const CartesianVelocity& target_velocity, + const double dt, + const std::vector& q_measured, + const std::vector& qd_measured, + std::vector& qd_command, + const FrameType frame) +{ + qd_command.clear(); + if (!solver_ || !speedl_configured_ || dt <= 0.0) { + return false; + } + if (static_cast(q_measured.size()) != solver_->chainDof() || + static_cast(qd_measured.size()) != solver_->chainVelocityDof()) { + return false; + } + + Eigen::Matrix measured_twist_base = Eigen::Matrix::Zero(); + Eigen::MatrixXd jacobian_base; + Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); + if (!solver_->computeTwistBaseAtQ(q_measured, + qd_measured, + true, + measured_twist_base, + &jacobian_base, + &base_R_tool)) { + return false; + } + + const Eigen::Matrix target_twist = common::math::velocityToVector(target_velocity); + if (target_twist.squaredNorm() <= 1e-12) { + twist_limiter_.synchronize(measured_twist_base, dt, true); + } else if (speedl_command_twist_base_.squaredNorm() <= 1e-12) { + twist_limiter_.initialize(Eigen::Matrix::Zero()); + } + twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame)); + speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool); + + std::vector qdot_std; + if (!solver_->solveVelocityBase(jacobian_base, + speedl_command_twist_base_, + q_measured, + qdot_std, + std::numeric_limits::infinity())) { + return false; + } + + Eigen::VectorXd qdot = applyJointVelocityLimits_(toEigenVector(qdot_std)); + qdot = applyJointSoftLimits_(toEigenVector(q_measured), qdot); + + Eigen::VectorXd reference = toEigenVector(qd_measured); + if (prev_qdot_command_.size() == qdot.size()) { + reference = toEigenVector(prev_qdot_command_); + } + qdot = applyJointAccelerationLimits_(qdot, reference, dt); + + const Eigen::Matrix achieved_twist_base = jacobian_base * qdot; + const Eigen::Vector3d desired_linear = speedl_command_twist_base_.head<3>(); + const Eigen::Vector3d achieved_linear = achieved_twist_base.head<3>(); + const double desired_linear_norm = desired_linear.norm(); + const double achieved_linear_norm = achieved_linear.norm(); + const double direction_check_min_speed = + std::max(1e-4, positiveOr(speedl_config_.linear_reverse_switch_speed_threshold(), 1e-3)); + if (desired_linear_norm > direction_check_min_speed) { + const double linear_min_speed_ratio = + clamp(positiveOr(speedl_config_.linear_min_speed_ratio(), 0.2), 0.0, 1.0); + const double speed_ratio = achieved_linear_norm / desired_linear_norm; + if (speed_ratio < linear_min_speed_ratio) { + return false; + } + if (achieved_linear_norm > direction_check_min_speed) { + const double deviation_deg = directionDeviationDeg(desired_linear, achieved_linear); + const double severe_direction_deviation_deg = + positiveOr(speedl_config_.severe_direction_deviation_deg(), 45.0); + if (deviation_deg >= severe_direction_deviation_deg) { + return false; + } + } + } + + qd_command = toStdVector(qdot); + prev_qdot_command_ = qd_command; + return true; +} + +bool PinocchioDlsCartesianMotionPlanner::updateSpeedLAcceleration(const double acceleration) +{ + if (!speedl_configured_ || acceleration <= 0.0) { + return false; + } + if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9) { + return true; + } + + cartesian_motion::updateTwistLimiterAcceleration( + twist_limiter_, + speedl_config_, + acceleration); + speedl_applied_acceleration_ = acceleration; + return true; +} + +CartesianVelocity PinocchioDlsCartesianMotionPlanner::getSpeedLCommandTwistBase() const +{ + return common::math::vectorToVelocity(speedl_command_twist_base_); +} + +} // namespace cmvr::device diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h new file mode 100644 index 00000000..1d0d2d6e --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h @@ -0,0 +1,76 @@ +#ifndef CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H +#define CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H + +#include + +#include +#include + +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" +#include "../../cartesian_motion_planner.h" +#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" +#include "common/math/qp_solver.h" + +namespace cmvr::device { + +class PinocchioQpCartesianMotionPlanner final : public CartesianMotionPlanner { +public: + explicit PinocchioQpCartesianMotionPlanner(std::shared_ptr solver); + + bool configureSpeedL(const config::SpeedLPlannerConfig& config, + std::size_t dof) override; + bool configureMoveL(const config::MoveLPlannerConfig& config) override; + + bool planMoveL(const CartesianPose& target, + const std::vector& q_start, + const std::vector& qd_max, + double velocity, + double acceleration, + double jerk, + FrameType frame, + CartesianJointTrajectory& trajectory) override; + + bool speedLStep(const CartesianVelocity& target_velocity, + double dt, + const std::vector& q_measured, + const std::vector& qd_measured, + std::vector& qd_command, + FrameType frame) override; + + bool updateSpeedLAcceleration(double acceleration) override; + CartesianVelocity getSpeedLCommandTwistBase() const override; + +private: + bool refreshJointLimits_(const config::CartesianVelocityQpConfig& config); + bool configureQpSolver_(Eigen::Index dof, double solver_eps); + bool solveVelocityQp_(const Eigen::MatrixXd& jacobian_base, + const Eigen::Matrix& target_twist_base, + const Eigen::VectorXd& q_measured, + const Eigen::VectorXd& qd_reference, + double dt, + const std::vector& qd_max, + bool enforce_acceleration_limits, + const config::CartesianVelocityQpConfig& qp_config, + Eigen::VectorXd& qdot); + bool validateAchievedLinearTwist_(const Eigen::MatrixXd& jacobian_base, + const Eigen::VectorXd& qdot) const; + + std::shared_ptr solver_{nullptr}; + config::MoveLPlannerConfig movel_config_{}; + config::SpeedLPlannerConfig speedl_config_{}; + cmvr::CartesianTwistLimiter twist_limiter_{}; + cmvr::QPSolver qp_solver_; + int qp_solver_dof_{0}; + double qp_solver_eps_{0.0}; + Eigen::VectorXd joint_lower_limits_; + Eigen::VectorXd joint_upper_limits_; + Eigen::VectorXd joint_velocity_limits_; + std::vector prev_qdot_command_; + Eigen::Matrix speedl_command_twist_base_{Eigen::Matrix::Zero()}; + double speedl_applied_acceleration_{0.25}; + bool speedl_configured_{false}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp new file mode 100644 index 00000000..651525d8 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp @@ -0,0 +1,590 @@ +#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h" + +#include +#include +#include +#include +#include +#include + +#include "algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h" +#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve.h" +#include "common/base/logging/logger.h" +#include "common/math/cartesian_motion_math.h" +#include "common/math/joint_limits.h" +#include "common/config/config_files.h" +#include "common/math/proto_geometry.h" +#include "common/math/transform_math.h" + +namespace cmvr::device { + +using cmvr::common::config::positiveOr; +using cmvr::device::cartesian_motion::clamp; +using cmvr::device::cartesian_motion::directionDeviationDeg; +using cmvr::device::cartesian_motion::rotationVector; +using cmvr::device::cartesian_motion::toEigenVector; +using cmvr::device::cartesian_motion::toStdVector; + +namespace { + +const config::CartesianVelocityQpConfig& qpConfigOrDefault( + const config::CartesianVelocityQpConfig& config) +{ + static const config::CartesianVelocityQpConfig defaults; + return config.ByteSizeLong() > 0 ? config : defaults; +} + +Eigen::Matrix twistTrackingWeightOrDefault( + const cmvr::common::Vec6& value) +{ + Eigen::Matrix defaults; + defaults << 1.0, 1.0, 1.0, 0.5, 0.5, 0.5; + Eigen::Matrix weight = + cmvr::common::math::toEigenVec6(value, defaults); + for (int i = 0; i < weight.size(); ++i) { + if (!std::isfinite(weight[i]) || weight[i] <= 0.0) { + weight[i] = defaults[i]; + } + } + return weight; +} + +} // namespace + +PinocchioQpCartesianMotionPlanner::PinocchioQpCartesianMotionPlanner( + std::shared_ptr solver) + : solver_(std::move(solver)) +{ +} + +bool PinocchioQpCartesianMotionPlanner::refreshJointLimits_( + const config::CartesianVelocityQpConfig& config) +{ + if (!solver_) { + return false; + } + const auto source = config.has_joint_limits() + ? config.joint_limits().source() + : config::JOINT_LIMIT_SOURCE_URDF; + if (source == config::JOINT_LIMIT_SOURCE_CUSTOM) { + std::vector joint_names; + if (!solver_->getChainJointNames(joint_names) || joint_names.empty()) { + return false; + } + std::unordered_map custom_limits; + if (config.has_joint_limits()) { + custom_limits.reserve( + static_cast(config.joint_limits().joints_size())); + for (const auto& item : config.joint_limits().joints()) { + if (!item.joint_name().empty()) { + custom_limits[item.joint_name()] = item; + } + } + } + + const auto dof = static_cast(joint_names.size()); + joint_lower_limits_.resize(dof); + joint_upper_limits_.resize(dof); + joint_velocity_limits_.resize(dof); + for (Eigen::Index i = 0; i < dof; ++i) { + const auto it = custom_limits.find(joint_names[static_cast(i)]); + if (it == custom_limits.end()) { + CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] missing custom joint limit for " + << joint_names[static_cast(i)]; + return false; + } + const auto& limit = it->second; + if (!std::isfinite(limit.lower()) || !std::isfinite(limit.upper()) || + !std::isfinite(limit.velocity()) || limit.upper() <= limit.lower() || + limit.velocity() <= 0.0) { + CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] invalid custom joint limit for " + << limit.joint_name(); + return false; + } + joint_lower_limits_[i] = limit.lower(); + joint_upper_limits_[i] = limit.upper(); + joint_velocity_limits_[i] = std::abs(limit.velocity()); + } + return true; + } + + if (!solver_->getJointPositionLimits(joint_lower_limits_, joint_upper_limits_)) { + return false; + } + if (!solver_->getJointVelocityLimits(joint_velocity_limits_)) { + return false; + } + return true; +} + +bool PinocchioQpCartesianMotionPlanner::configureQpSolver_(const Eigen::Index dof, + const double solver_eps) +{ + if (dof <= 0) { + return false; + } + const double eps = solver_eps > 0.0 ? solver_eps : 1e-3; + if (qp_solver_dof_ != static_cast(dof) || std::abs(qp_solver_eps_ - eps) > 1e-12) { + qp_solver_.Setup(static_cast(dof), static_cast(dof), eps); + qp_solver_.ResetIsFirst(); + qp_solver_dof_ = static_cast(dof); + qp_solver_eps_ = eps; + } + return true; +} + +bool PinocchioQpCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlannerConfig& config, + const std::size_t dof) +{ + if (!solver_ || dof == 0) { + return false; + } + const auto solver_dof = static_cast(std::max(0, solver_->chainVelocityDof())); + if (solver_dof != 0 && solver_dof != dof) { + return false; + } + speedl_config_ = config; + + const auto& qp_config = qpConfigOrDefault(speedl_config_.qp()); + if (!refreshJointLimits_(qp_config)) { + return false; + } + + cartesian_motion::configureTwistLimiterFromSpeedLConfig(twist_limiter_, speedl_config_); + + prev_qdot_command_.assign(dof, 0.0); + speedl_command_twist_base_.setZero(); + speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0); + if (!configureQpSolver_(static_cast(dof), + positiveOr(qp_config.solver_eps(), 1e-3))) { + return false; + } + speedl_configured_ = true; + return true; +} + +bool PinocchioQpCartesianMotionPlanner::configureMoveL( + const config::MoveLPlannerConfig& config) +{ + if (!std::isfinite(config.sample_period_s()) || + !std::isfinite(config.position_gain()) || + !std::isfinite(config.rotation_gain())) { + return false; + } + movel_config_ = config; + return true; +} + +bool PinocchioQpCartesianMotionPlanner::planMoveL(const CartesianPose& target, + const std::vector& q_start, + const std::vector& qd_max, + const double velocity, + const double acceleration, + const double jerk, + const FrameType frame, + CartesianJointTrajectory& trajectory) +{ + trajectory = {}; + const double dt = positiveOr(movel_config_.sample_period_s(), 0.001); + if (!solver_ || q_start.empty() || + velocity <= 0.0 || acceleration <= 0.0 || jerk <= 0.0) { + return false; + } + if (static_cast(q_start.size()) != solver_->chainDof()) { + return false; + } + if (!qd_max.empty() && qd_max.size() != q_start.size()) { + return false; + } + const auto& qp_config = qpConfigOrDefault(movel_config_.qp()); + if (!refreshJointLimits_(qp_config)) { + return false; + } + + Eigen::Matrix4d start_pose_base = Eigen::Matrix4d::Identity(); + if (!solver_->fk(q_start, start_pose_base, true)) { + return false; + } + + const Eigen::Matrix4d target_pose_input = common::math::poseToMatrix(target); + const Eigen::Matrix4d target_pose_base = + frame == FrameType::Tool ? start_pose_base * target_pose_input : target_pose_input; + + const Eigen::Vector3d p_start = start_pose_base.block<3, 1>(0, 3); + const Eigen::Vector3d p_target = target_pose_base.block<3, 1>(0, 3); + const Eigen::Vector3d dp = p_target - p_start; + const double linear_distance = dp.norm(); + + const Eigen::Matrix3d R_start = start_pose_base.block<3, 3>(0, 0); + const Eigen::Matrix3d R_target = target_pose_base.block<3, 3>(0, 0); + const Eigen::Vector3d total_rotation_vector = rotationVector(R_target * R_start.transpose()); + const double angular_distance = total_rotation_vector.norm(); + + const double path_length = linear_distance > 1e-9 ? linear_distance : angular_distance; + trajectory.position.push_back(q_start); + trajectory.velocity.push_back(std::vector(q_start.size(), 0.0)); + trajectory.time.push_back(0.0); + if (path_length <= 1e-9) { + return true; + } + + cmvr::SCurve curve(velocity, acceleration, jerk); + const cmvr::SCurveProfile profile = curve.calculateProfile(0.0, path_length, 0.0, 0.0); + if (profile.total_time <= 0.0) { + return false; + } + + Eigen::VectorXd q_current = toEigenVector(q_start); + Eigen::VectorXd qdot_previous = Eigen::VectorXd::Zero(static_cast(q_start.size())); + Eigen::Vector3d linear_direction = Eigen::Vector3d::Zero(); + if (linear_distance > 1e-9) { + linear_direction = dp / linear_distance; + } + const Eigen::Quaterniond q_start_rot(R_start); + const Eigen::Quaterniond q_target_rot(R_target); + const double position_gain = positiveOr(movel_config_.position_gain(), 4.0); + const double rotation_gain = positiveOr(movel_config_.rotation_gain(), 4.0); + + qp_solver_.ResetIsFirst(); + double previous_time = 0.0; + for (double t = std::min(dt, profile.total_time); + t <= profile.total_time + 1e-9; + t = std::min(t + dt, profile.total_time)) { + const double step_dt = std::max(1e-6, t - previous_time); + previous_time = t; + + const double s = clamp(curve.getPositionAtTime(profile, t), 0.0, path_length); + const double sd = std::max(0.0, curve.getVelocityAtTime(profile, t)); + const double ratio = clamp(s / path_length, 0.0, 1.0); + + const std::vector q_std = toStdVector(q_current); + Eigen::Matrix4d current_pose_base = Eigen::Matrix4d::Identity(); + if (!solver_->fk(q_std, current_pose_base, true)) { + return false; + } + + const Eigen::Vector3d p_current = current_pose_base.block<3, 1>(0, 3); + const Eigen::Vector3d p_desired = p_start + ratio * dp; + Eigen::Matrix target_twist_base = Eigen::Matrix::Zero(); + target_twist_base.head<3>() = + linear_direction * sd + position_gain * (p_desired - p_current); + + if (angular_distance > 1e-9) { + const Eigen::Matrix3d R_current = current_pose_base.block<3, 3>(0, 0); + const Eigen::Matrix3d R_desired = + q_start_rot.slerp(ratio, q_target_rot).toRotationMatrix(); + const Eigen::Vector3d rotation_error = + rotationVector(R_desired * R_current.transpose()); + target_twist_base.tail<3>() = + (total_rotation_vector / path_length) * sd + rotation_gain * rotation_error; + } + + Eigen::MatrixXd jacobian_base; + Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); + if (!solver_->computeJacobianBaseAtQ(q_std, true, jacobian_base, base_R_tool)) { + return false; + } + + Eigen::VectorXd qdot; + if (!solveVelocityQp_(jacobian_base, + target_twist_base, + q_current, + qdot_previous, + step_dt, + qd_max, + false, + qp_config, + qdot)) { + return false; + } + + q_current += qdot * step_dt; + if (joint_lower_limits_.size() == q_current.size() && + joint_upper_limits_.size() == q_current.size()) { + q_current = q_current.cwiseMax(joint_lower_limits_).cwiseMin(joint_upper_limits_); + } + + trajectory.position.push_back(toStdVector(q_current)); + trajectory.velocity.push_back(toStdVector(qdot)); + trajectory.time.push_back(t); + qdot_previous = qdot; + + if (t >= profile.total_time - 1e-9) { + break; + } + } + + return true; +} + +bool PinocchioQpCartesianMotionPlanner::solveVelocityQp_( + const Eigen::MatrixXd& jacobian_base, + const Eigen::Matrix& target_twist_base, + const Eigen::VectorXd& q_measured, + const Eigen::VectorXd& qd_reference, + const double dt, + const std::vector& qd_max, + const bool enforce_acceleration_limits, + const config::CartesianVelocityQpConfig& qp_config, + Eigen::VectorXd& qdot) +{ + const Eigen::Index dof = q_measured.size(); + if (jacobian_base.rows() != 6 || jacobian_base.cols() != dof || + qd_reference.size() != dof || dt <= 0.0) { + return false; + } + + if (!configureQpSolver_(dof, positiveOr(qp_config.solver_eps(), 1e-3))) { + return false; + } + + const Eigen::Matrix twist_weight = + twistTrackingWeightOrDefault(qp_config.twist_tracking_weight()); + Eigen::Matrix task_weight = Eigen::Matrix::Identity(); + for (int i = 0; i < 6; ++i) { + task_weight(i, i) = twist_weight[i]; + } + const double qdot_regularization = + positiveOr(qp_config.qdot_regularization(), 1e-4); + const double prev_qdot_regularization = + positiveOr(qp_config.prev_qdot_regularization(), + enforce_acceleration_limits ? 2e-2 : 1e-4); + const bool use_joint_limit_avoidance = + qp_config.has_joint_limit_avoidance() && + qp_config.joint_limit_avoidance().enable() && + qp_config.joint_limit_avoidance().weight() > 0.0; + const int avoidance_rows = use_joint_limit_avoidance ? static_cast(dof) : 0; + + Eigen::MatrixXd cost(6 + 2 * dof + avoidance_rows, dof); + Eigen::VectorXd target(6 + 2 * dof + avoidance_rows); + cost.topRows(6) = task_weight * jacobian_base; + target.head(6) = task_weight * target_twist_base; + cost.middleRows(6, dof) = std::sqrt(qdot_regularization) * Eigen::MatrixXd::Identity(dof, dof); + target.segment(6, dof).setZero(); + cost.middleRows(6 + dof, dof) = + std::sqrt(prev_qdot_regularization) * Eigen::MatrixXd::Identity(dof, dof); + target.segment(6 + dof, dof) = std::sqrt(prev_qdot_regularization) * qd_reference; + if (use_joint_limit_avoidance) { + const auto& avoidance = qp_config.joint_limit_avoidance(); + const Eigen::VectorXd qdot_avoid = + cmvr::kinematics::computeJointLimitAvoidanceVelocity( + q_measured, + joint_lower_limits_, + joint_upper_limits_, + true, + positiveOr(avoidance.gain(), 0.2), + positiveOr(avoidance.margin_ratio(), 0.15), + positiveOr(avoidance.max_push(), 0.25)); + const double sqrt_weight = std::sqrt( + positiveOr(qp_config.joint_limit_avoidance().weight(), 0.05)); + cost.middleRows(6 + 2 * dof, dof) = + sqrt_weight * Eigen::MatrixXd::Identity(dof, dof); + target.segment(6 + 2 * dof, dof) = sqrt_weight * qdot_avoid; + } + + Eigen::VectorXd lower(dof); + Eigen::VectorXd upper(dof); + for (Eigen::Index i = 0; i < dof; ++i) { + double velocity_limit = std::numeric_limits::infinity(); + if (joint_velocity_limits_.size() == dof && joint_velocity_limits_[i] > 0.0) { + velocity_limit = std::abs(joint_velocity_limits_[i]); + } + if (qd_max.size() == static_cast(dof)) { + const double requested_limit = std::abs(qd_max[static_cast(i)]); + if (std::isfinite(requested_limit) && requested_limit > 0.0) { + velocity_limit = std::min(velocity_limit, requested_limit); + } + } + + double lb = -velocity_limit; + double ub = velocity_limit; + if (enforce_acceleration_limits) { + double acc_limit = 8.0; + if (i < speedl_config_.joint_acceleration_max_size() && + speedl_config_.joint_acceleration_max(static_cast(i)) > 0.0) { + acc_limit = speedl_config_.joint_acceleration_max(static_cast(i)); + } + lb = std::max(lb, qd_reference[i] - acc_limit * dt); + ub = std::min(ub, qd_reference[i] + acc_limit * dt); + } + + if (joint_lower_limits_.size() == dof && joint_upper_limits_.size() == dof) { + lb = std::max(lb, (joint_lower_limits_[i] - q_measured[i]) / dt); + ub = std::min(ub, (joint_upper_limits_[i] - q_measured[i]) / dt); + } + + if (lb > ub) { + CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] velocity bound infeasible at joint " + << i << ": lb=" << lb << ", ub=" << ub + << ", q=" << q_measured[i] + << ", qd_ref=" << qd_reference[i] + << ", dt=" << dt; + return false; + } + lower[i] = lb; + upper[i] = ub; + } + + qp_solver_.SetCostFunction(cost, target); + qp_solver_.SetConstraintsFunction(Eigen::MatrixXd::Identity(dof, dof), lower, upper); + qp_solver_.SetPrimalVariable(qd_reference); + + try { + qdot = qp_solver_.Solve(); + } catch (const cmvr::QPSolverException& error) { + CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] QP failed: " + << error.what() << " (code=" << error.code() << ")"; + return false; + } catch (const std::exception& error) { + CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] QP failed: " + << error.what(); + return false; + } + return qdot.size() == dof; +} + +bool PinocchioQpCartesianMotionPlanner::validateAchievedLinearTwist_( + const Eigen::MatrixXd& jacobian_base, + const Eigen::VectorXd& qdot) const +{ + const Eigen::Matrix achieved_twist_base = jacobian_base * qdot; + const Eigen::Vector3d desired_linear = speedl_command_twist_base_.head<3>(); + const Eigen::Vector3d achieved_linear = achieved_twist_base.head<3>(); + const double desired_linear_norm = desired_linear.norm(); + const double achieved_linear_norm = achieved_linear.norm(); + const double direction_check_min_speed = + std::max(1e-4, positiveOr(speedl_config_.linear_reverse_switch_speed_threshold(), 1e-3)); + if (desired_linear_norm <= direction_check_min_speed) { + return true; + } + + const double linear_min_speed_ratio = + clamp(positiveOr(speedl_config_.linear_min_speed_ratio(), 0.2), 0.0, 1.0); + const double speed_ratio = achieved_linear_norm / desired_linear_norm; + if (speed_ratio < linear_min_speed_ratio) { + CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] achieved speed too low: desired_linear=[" + << desired_linear.x() << ", " << desired_linear.y() << ", " << desired_linear.z() + << "], achieved_linear=[" << achieved_linear.x() << ", " + << achieved_linear.y() << ", " << achieved_linear.z() + << "], desired_norm=" << desired_linear_norm + << ", achieved_norm=" << achieved_linear_norm + << ", speed_ratio=" << speed_ratio + << ", min_ratio=" << linear_min_speed_ratio + << ", direction_check_min_speed=" << direction_check_min_speed; + return false; + } + if (achieved_linear_norm <= direction_check_min_speed) { + return true; + } + const double deviation_deg = directionDeviationDeg(desired_linear, achieved_linear); + const double severe_direction_deviation_deg = + positiveOr(speedl_config_.severe_direction_deviation_deg(), 45.0); + if (deviation_deg >= severe_direction_deviation_deg) { + CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] direction deviation too large: desired_linear=[" + << desired_linear.x() << ", " << desired_linear.y() << ", " << desired_linear.z() + << "], achieved_linear=[" << achieved_linear.x() << ", " + << achieved_linear.y() << ", " << achieved_linear.z() + << "], deviation_deg=" << deviation_deg + << ", severe_threshold_deg=" << severe_direction_deviation_deg + << ", direction_check_min_speed=" << direction_check_min_speed; + return false; + } + return true; +} + +bool PinocchioQpCartesianMotionPlanner::speedLStep(const CartesianVelocity& target_velocity, + const double dt, + const std::vector& q_measured, + const std::vector& qd_measured, + std::vector& qd_command, + const FrameType frame) +{ + qd_command.clear(); + if (!solver_ || !speedl_configured_ || dt <= 0.0) { + CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] invalid state: solver=" + << (solver_ ? 1 : 0) + << ", configured=" << (speedl_configured_ ? 1 : 0) + << ", dt=" << dt; + return false; + } + if (static_cast(q_measured.size()) != solver_->chainDof() || + static_cast(qd_measured.size()) != solver_->chainVelocityDof()) { + CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] state size mismatch: q=" + << q_measured.size() << "/" << solver_->chainDof() + << ", qd=" << qd_measured.size() << "/" << solver_->chainVelocityDof(); + return false; + } + + Eigen::Matrix measured_twist_base = Eigen::Matrix::Zero(); + Eigen::MatrixXd jacobian_base; + Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); + if (!solver_->computeTwistBaseAtQ(q_measured, + qd_measured, + true, + measured_twist_base, + &jacobian_base, + &base_R_tool)) { + CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] computeTwistBaseAtQ failed"; + return false; + } + + const Eigen::Matrix target_twist = common::math::velocityToVector(target_velocity); + if (target_twist.squaredNorm() <= 1e-12) { + twist_limiter_.synchronize(measured_twist_base, dt, true); + } else if (speedl_command_twist_base_.squaredNorm() <= 1e-12) { + twist_limiter_.initialize(Eigen::Matrix::Zero()); + } + twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame)); + speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool); + + Eigen::VectorXd reference = toEigenVector(qd_measured); + if (prev_qdot_command_.size() == q_measured.size()) { + reference = toEigenVector(prev_qdot_command_); + } + + Eigen::VectorXd qdot; + if (!solveVelocityQp_(jacobian_base, + speedl_command_twist_base_, + toEigenVector(q_measured), + reference, + dt, + {}, + true, + qpConfigOrDefault(speedl_config_.qp()), + qdot)) { + CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] solveVelocityQp failed"; + return false; + } + if (!validateAchievedLinearTwist_(jacobian_base, qdot)) { + CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] validateAchievedLinearTwist failed"; + return false; + } + + qd_command = toStdVector(qdot); + prev_qdot_command_ = qd_command; + return true; +} + +bool PinocchioQpCartesianMotionPlanner::updateSpeedLAcceleration(const double acceleration) +{ + if (!speedl_configured_ || acceleration <= 0.0) { + return false; + } + if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9) { + return true; + } + + cartesian_motion::updateTwistLimiterAcceleration( + twist_limiter_, + speedl_config_, + acceleration); + speedl_applied_acceleration_ = acceleration; + return true; +} + +CartesianVelocity PinocchioQpCartesianMotionPlanner::getSpeedLCommandTwistBase() const +{ + return common::math::vectorToVelocity(speedl_command_twist_base_); +} + +} // namespace cmvr::device diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h b/cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h new file mode 100644 index 00000000..66ab7e88 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h @@ -0,0 +1,51 @@ +#ifndef CMVR_ES_TWIST_LIMITER_CONFIG_H +#define CMVR_ES_TWIST_LIMITER_CONFIG_H + +#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" +#include "cmvr/config/arm_config/arm_config.pb.h" +#include "common/config/config_files.h" + +namespace cmvr::device::cartesian_motion { + +inline void configureTwistLimiterFromSpeedLConfig( + cmvr::CartesianTwistLimiter& limiter, + const config::SpeedLPlannerConfig& config) +{ + using cmvr::common::config::positiveOr; + + limiter.setLinearConstraints(positiveOr(config.linear_velocity_max(), 0.55), + positiveOr(config.linear_acceleration_max(), 5.0), + positiveOr(config.linear_jerk_max(), 10.0)); + limiter.setAngularConstraints(positiveOr(config.angular_velocity_max(), 1.0), + positiveOr(config.angular_acceleration_max(), 5.0), + positiveOr(config.angular_jerk_max(), 12.0)); + limiter.setLinearTargetReplanThreshold( + positiveOr(config.linear_target_replan_threshold(), 1e-4)); + limiter.setAngularTargetReplanThreshold( + positiveOr(config.angular_target_replan_threshold(), 1e-4)); + limiter.setLinearReverseSwitchPolicy( + config.linear_reverse_cos_threshold() != 0.0 + ? config.linear_reverse_cos_threshold() + : -0.8660254037844386, + positiveOr(config.linear_reverse_switch_speed_threshold(), 1e-3)); + limiter.initialize(Eigen::Matrix::Zero()); +} + +inline void updateTwistLimiterAcceleration( + cmvr::CartesianTwistLimiter& limiter, + const config::SpeedLPlannerConfig& config, + const double acceleration) +{ + using cmvr::common::config::positiveOr; + + limiter.setLinearConstraints(positiveOr(config.linear_velocity_max(), 0.55), + acceleration, + positiveOr(config.linear_jerk_max(), 10.0)); + limiter.setAngularConstraints(positiveOr(config.angular_velocity_max(), 1.0), + acceleration, + positiveOr(config.angular_jerk_max(), 12.0)); +} + +} // namespace cmvr::device::cartesian_motion + +#endif // CMVR_ES_TWIST_LIMITER_CONFIG_H diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h new file mode 100644 index 00000000..ea500ea4 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h @@ -0,0 +1,31 @@ +#ifndef CMVR_ES_JOINT_MOTION_PLANNER_H +#define CMVR_ES_JOINT_MOTION_PLANNER_H + +#include + +#include "common/types/arm/arm_types.h" + +namespace cmvr::device { + +struct JointTrajectorySample { + double t{0.0}; + std::vector position; + std::vector velocity; +}; + +class JointMotionPlanner { +public: + virtual ~JointMotionPlanner() = default; + + virtual bool init() = 0; + + virtual bool planMoveJ(const std::vector& start, + const JointPositionCommand& target, + const MotionOptions& options, + double speed_scaling, + std::vector& samples) = 0; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_JOINT_MOTION_PLANNER_H diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner_factory.h b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner_factory.h new file mode 100644 index 00000000..c9211cd1 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner_factory.h @@ -0,0 +1,55 @@ +#ifndef CMVR_ES_JOINT_MOTION_PLANNER_FACTORY_H +#define CMVR_ES_JOINT_MOTION_PLANNER_FACTORY_H + +#include + +#include "algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h" +#include "algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h" +#include "cmvr/config/arm_config/arm_config.pb.h" + +namespace cmvr::device { + +class JointMotionPlannerFactory { +public: + static std::shared_ptr create( + const config::MoveJConfig& cfg) + { + if (!cfg.has_toppra_joint_motion_planner()) { + return nullptr; + } + + cmvr::PathType path_type; + switch (cfg.toppra_joint_motion_planner().path_type()) { + case config::TOPPRA_PATH_TYPE_LINEAR: + path_type = cmvr::PathType::Linear; + break; + case config::TOPPRA_PATH_TYPE_CUBIC_HERMITE: + path_type = cmvr::PathType::CubicHermite; + break; + case config::TOPPRA_PATH_TYPE_QUINTIC: + path_type = cmvr::PathType::Quintic; + break; + case config::TOPPRA_PATH_TYPE_NATURAL: + path_type = cmvr::PathType::Natural; + break; + case config::TOPPRA_PATH_TYPE_UNKNOWN: + default: + return nullptr; + } + + const auto& toppra = cfg.toppra_joint_motion_planner(); + auto planner = std::make_shared( + path_type, + toppra.sample_period_s(), + toppra.grid_size(), + toppra.high_grid_size()); + if (!planner->init()) { + return nullptr; + } + return planner; + } +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_JOINT_MOTION_PLANNER_FACTORY_H diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h new file mode 100644 index 00000000..8c6db577 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h @@ -0,0 +1,36 @@ +#ifndef CMVR_ES_TOPPRA_JOINT_MOTION_PLANNER_H +#define CMVR_ES_TOPPRA_JOINT_MOTION_PLANNER_H + +#include + +#include "../../joint_motion_planner.h" +#include "../../../../base_motion/joint_trajectory/joint_trajectory_planner.h" + +namespace cmvr::device { + +class ToppraJointMotionPlanner final : public JointMotionPlanner { +public: + ToppraJointMotionPlanner(cmvr::PathType path_type, + double sample_period_s, + int grid_size, + int high_grid_size); + + bool init() override; + + bool planMoveJ(const std::vector& start, + const JointPositionCommand& target, + const MotionOptions& options, + double speed_scaling, + std::vector& samples) override; + +private: + std::shared_ptr planner_; + cmvr::PathType path_type_{cmvr::PathType::Quintic}; + double sample_period_s_{0.001}; + int grid_size_{150}; + int high_grid_size_{300}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_TOPPRA_JOINT_MOTION_PLANNER_H diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp new file mode 100644 index 00000000..8ee1e8e2 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp @@ -0,0 +1,76 @@ +#include "algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h" + +#include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h" + +namespace cmvr::device { + +namespace { + +std::vector toStdVector(const Eigen::VectorXd& value) +{ + std::vector result; + result.reserve(static_cast(value.size())); + for (int i = 0; i < value.size(); ++i) { + result.push_back(value[i]); + } + return result; +} + +} // namespace + +ToppraJointMotionPlanner::ToppraJointMotionPlanner(const cmvr::PathType path_type, + const double sample_period_s, + const int grid_size, + const int high_grid_size) + : path_type_(path_type) + , sample_period_s_(sample_period_s) + , grid_size_(grid_size) + , high_grid_size_(high_grid_size) +{ +} + +bool ToppraJointMotionPlanner::init() +{ + if (sample_period_s_ <= 0.0 || grid_size_ <= 0 || high_grid_size_ < grid_size_) { + return false; + } + planner_ = std::make_shared(path_type_); + planner_->setGridSizes(grid_size_, high_grid_size_); + return true; +} + +bool ToppraJointMotionPlanner::planMoveJ(const std::vector& start, + const JointPositionCommand& target, + const MotionOptions& options, + const double speed_scaling, + std::vector& samples) +{ + samples.clear(); + if (!planner_ || start.empty() || start.size() != target.position.size() || + options.velocity <= 0.0 || options.acceleration <= 0.0) { + return false; + } + + cmvr::TrajPtr trajectory; + planner_->setPathType(path_type_); + planner_->setGridSizes(grid_size_, high_grid_size_); + planner_->setSymmetricLimits( + std::vector(start.size(), options.velocity * speed_scaling), + std::vector(start.size(), options.acceleration)); + if (!planner_->plan(start, target.position, trajectory)) { + return false; + } + + const auto raw_samples = planner_->sampleTrajectory(trajectory, sample_period_s_); + samples.reserve(raw_samples.size()); + for (const auto& sample : raw_samples) { + JointTrajectorySample dst; + dst.t = sample.t; + dst.position = toStdVector(sample.q); + dst.velocity = toStdVector(sample.qd); + samples.push_back(std::move(dst)); + } + return true; +} + +} // namespace cmvr::device diff --git a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt new file mode 100644 index 00000000..52df3e28 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt @@ -0,0 +1,65 @@ + + +add_library(base_motion SHARED + joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp + + motion_profile/s_curve/src/s_curve.cpp + motion_profile/s_curve/src/s_curve_position_planner.cpp + motion_profile/s_curve/src/s_curve_velocity_planner.cpp + cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp +) + +target_include_directories(base_motion PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(base_motion PUBLIC + OsqpEigen + tinyxml2 + fcl + toppra + cmvr_es::common +) + +add_library(cmvr_es::base_motion ALIAS base_motion) +install(TARGETS base_motion LIBRARY DESTINATION lib) + + +# -------------------------------------------------------- +# Unit test +# -------------------------------------------------------- + + + + +add_executable(toppra_joint_trajectory_planner_test + ${CMAKE_CURRENT_SOURCE_DIR}/joint_trajectory/toppra/src/toppra_joint_trajectory_planner_test.cpp +) + + +target_link_libraries(toppra_joint_trajectory_planner_test + PRIVATE + cmvr_es::base_motion + gtest + gtest_main + pthread + glog + cmvr_es::proto + ccd + fcl + OsqpEigen + +) + +add_executable(cartesian_twist_limiter_test + ${CMAKE_CURRENT_SOURCE_DIR}/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter_test.cpp +) + +target_link_libraries(cartesian_twist_limiter_test + PRIVATE + cmvr_es::base_motion + gtest + gtest_main + pthread + glog + cmvr_es::proto + matplot +) diff --git a/cmvr-es/planner/cartesian_space_planner/include/cartesian_twist_limiter.h b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h similarity index 98% rename from cmvr-es/planner/cartesian_space_planner/include/cartesian_twist_limiter.h rename to cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h index c62bdeea..220389f0 100644 --- a/cmvr-es/planner/cartesian_space_planner/include/cartesian_twist_limiter.h +++ b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h @@ -2,7 +2,7 @@ #include #include -#include "planner/s_curve_planner/include/s_curve_velocity_planner.h" +#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve_velocity_planner.h" namespace cmvr { diff --git a/cmvr-es/planner/cartesian_space_planner/src/cartesian_twist_limiter.cpp b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp similarity index 100% rename from cmvr-es/planner/cartesian_space_planner/src/cartesian_twist_limiter.cpp rename to cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp diff --git a/cmvr-es/planner/cartesian_space_planner/src/cartesian_twist_limiter_test.cpp b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter_test.cpp similarity index 99% rename from cmvr-es/planner/cartesian_space_planner/src/cartesian_twist_limiter_test.cpp rename to cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter_test.cpp index 2878a823..f40d07ca 100644 --- a/cmvr-es/planner/cartesian_space_planner/src/cartesian_twist_limiter_test.cpp +++ b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter_test.cpp @@ -7,7 +7,7 @@ #include #include -#include "planner/cartesian_space_planner/include/cartesian_twist_limiter.h" +#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" namespace cmvr { diff --git a/cmvr-es/planner/joint_space_planner/include/joint_space_planner.h b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/joint_trajectory_planner.h similarity index 78% rename from cmvr-es/planner/joint_space_planner/include/joint_space_planner.h rename to cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/joint_trajectory_planner.h index dd33b616..fcb64bb7 100644 --- a/cmvr-es/planner/joint_space_planner/include/joint_space_planner.h +++ b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/joint_trajectory_planner.h @@ -4,7 +4,11 @@ #pragma once +#include +#include #include + +#include #include #include #include @@ -33,12 +37,12 @@ namespace cmvr { Quintic, // 三次B样条 Natural }; - class JointSpacePlanner { + class JointTrajectoryPlanner { public: - explicit JointSpacePlanner(PathType p):path_type_(p){}; - JointSpacePlanner()=default; - virtual ~JointSpacePlanner() = default; + explicit JointTrajectoryPlanner(PathType p):path_type_(p){}; + JointTrajectoryPlanner()=default; + virtual ~JointTrajectoryPlanner() = default; virtual bool plan(const std::vector& start_joints, const std::vector& goal_joints, @@ -62,14 +66,20 @@ namespace cmvr { } // 对称限：[-v_max, v_max]、[-a_max, a_max] - virtual void setSymmetricLimits(const std::vector &v_max, - const std::vector &a_max); + virtual void setSymmetricLimits(const std::vector &v_max,const std::vector &a_max){ + v_max_ = v_max; + a_max_ = a_max; + } // 可调网格密度（默认 150/300） - virtual void setGridSizes(int N, int N_high); + virtual void setGridSizes(int N, int N_high) + { + N_grid_ = N; + N_grid_high_ = N_high; + } // 切换几何路径类型 - virtual void setPathType(PathType p); + virtual void setPathType(PathType p){path_type_ = p;} protected: PathType path_type_{PathType::Quintic}; int N_grid_ = 150; @@ -77,4 +87,4 @@ namespace cmvr { std::vector v_max_, a_max_; }; -} \ No newline at end of file +} diff --git a/cmvr-es/planner/joint_space_planner/include/toppra_bspline.h b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h similarity index 97% rename from cmvr-es/planner/joint_space_planner/include/toppra_bspline.h rename to cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h index 6e1fe7ab..39865020 100644 --- a/cmvr-es/planner/joint_space_planner/include/toppra_bspline.h +++ b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h @@ -4,7 +4,7 @@ #include #include -#include "planner/joint_space_planner/include/joint_space_planner.h" +#include "../../joint_trajectory_planner.h" #include #include #include @@ -47,9 +47,9 @@ namespace cmvr { }; // 具体规划器：一次/三次/五次可切换；ConstAccel 校验失败自动回退 Spline - class ToppraBSpline : public JointSpacePlanner { + class ToppraJointTrajectoryPlanner : public JointTrajectoryPlanner { public: - explicit ToppraBSpline(PathType type = PathType::Quintic); + explicit ToppraJointTrajectoryPlanner(PathType type = PathType::Quintic); // 统一入口：两点/多点皆可 diff --git a/cmvr-es/planner/joint_space_planner/src/toppra_bspline.cpp b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp similarity index 88% rename from cmvr-es/planner/joint_space_planner/src/toppra_bspline.cpp rename to cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp index 13a87ff7..e3637b8b 100644 --- a/cmvr-es/planner/joint_space_planner/src/toppra_bspline.cpp +++ b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp @@ -3,7 +3,7 @@ #include #include #include -#include "planner/joint_space_planner/include/toppra_bspline.h" +#include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h" #include #include @@ -31,19 +31,19 @@ namespace cmvr { Eigen::VectorXd SplineTraj::qd(double t) const { return impl_.eval_single(t, 1); } Eigen::VectorXd SplineTraj::qdd(double t) const { return impl_.eval_single(t, 2); } - // ===== ToppraBSpline ===== - ToppraBSpline::ToppraBSpline(PathType type) :JointSpacePlanner(type) { + // ===== ToppraJointTrajectoryPlanner ===== + ToppraJointTrajectoryPlanner::ToppraJointTrajectoryPlanner(PathType type) :JointTrajectoryPlanner(type) { } - bool ToppraBSpline::ensureLimitsSized(std::size_t DoF) { + bool ToppraJointTrajectoryPlanner::ensureLimitsSized(std::size_t DoF) { if (v_max_.size() != DoF) v_max_.assign(DoF, 1.5); if (a_max_.size() != DoF) a_max_.assign(DoF, 3.0); return true; } - void ToppraBSpline::sanitizeVsq(toppra::Vector &v) { + void ToppraJointTrajectoryPlanner::sanitizeVsq(toppra::Vector &v) { for (int i = 0; i < v.size(); ++i) if (v[i] < 0 && v[i] > -1e-10) v[i] = 0; if (v.size() > 0) { v[0] = 0; @@ -51,7 +51,7 @@ namespace cmvr { } } // ===== 统一入口：两点/多点 ===== - bool ToppraBSpline::plan(const std::vector>& waypoints, + bool ToppraJointTrajectoryPlanner::plan(const std::vector>& waypoints, TrajPtr& traj_out) { traj_out.reset(); const size_t M = waypoints.size(); @@ -115,7 +115,7 @@ namespace cmvr { } - bool ToppraBSpline::plan(const std::vector& start_joints, + bool ToppraJointTrajectoryPlanner::plan(const std::vector& start_joints, const std::vector& goal_joints, TrajPtr& traj_out) { if (start_joints.empty() || start_joints.size()!=goal_joints.size()) return false; @@ -125,7 +125,7 @@ namespace cmvr { std::shared_ptr - ToppraBSpline::buildPathUnified(const std::vector& q, + ToppraJointTrajectoryPlanner::buildPathUnified(const std::vector& q, const std::vector& S) { const size_t M = q.size(); if (M == 2) return buildTwoPointPath(q[0], q[1]); @@ -140,7 +140,7 @@ namespace cmvr { std::shared_ptr - ToppraBSpline::buildTwoPointPath(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) { + ToppraJointTrajectoryPlanner::buildTwoPointPath(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) { switch (path_type_) { case PathType::Linear: return buildLinearTwo(q0, q1); case PathType::CubicHermite: return buildCubicHermiteTwo(q0, q1); @@ -152,7 +152,7 @@ namespace cmvr { // 二点：Linear —— std::shared_ptr - ToppraBSpline::buildLinearTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) { + ToppraJointTrajectoryPlanner::buildLinearTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) { const size_t DoF = static_cast(q0.size()); toppra::Matrix seg(2, DoF); seg.row(0) = (q1 - q0).transpose(); @@ -163,7 +163,7 @@ namespace cmvr { // 二点：Cubic Hermite（端点速度 0） —— std::shared_ptr - ToppraBSpline::buildCubicHermiteTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) { + ToppraJointTrajectoryPlanner::buildCubicHermiteTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) { toppra::Vectors pos{q0, q1}; toppra::Vectors vel{Eigen::VectorXd::Zero(q0.size()), Eigen::VectorXd::Zero(q1.size())}; @@ -174,7 +174,7 @@ namespace cmvr { // 二点：Quintic rest-to-rest —— std::shared_ptr - ToppraBSpline::buildQuinticRestToRestTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) { + ToppraJointTrajectoryPlanner::buildQuinticRestToRestTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) { const size_t DoF = static_cast(q0.size()); const Eigen::VectorXd dq = q1 - q0; toppra::Matrix seg(6, DoF); // x^5..x^0 @@ -188,7 +188,7 @@ namespace cmvr { } std::shared_ptr - ToppraBSpline::buildNaturalTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) { + ToppraJointTrajectoryPlanner::buildNaturalTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) { using PWP = toppra::PiecewisePolyPath; // positions（两点） @@ -217,7 +217,7 @@ namespace cmvr { // —— 多点：Linear —— std::shared_ptr - ToppraBSpline::buildLinearMulti(const std::vector& q, + ToppraJointTrajectoryPlanner::buildLinearMulti(const std::vector& q, const std::vector& S) { const size_t M = q.size(), DoF = q[0].size(); toppra::Matrices segs; segs.reserve(M-1); @@ -234,7 +234,7 @@ namespace cmvr { // —— 多点：Cubic Hermite —— std::shared_ptr - ToppraBSpline::buildCubicHermiteMulti(const std::vector& q, + ToppraJointTrajectoryPlanner::buildCubicHermiteMulti(const std::vector& q, const std::vector& S) { auto v = estimateVelsCatmull(q, S); clampNodeVels(v, q, /*k=*/1.0); @@ -246,7 +246,7 @@ namespace cmvr { std::shared_ptr - ToppraBSpline::buildNaturalMulti(const std::vector& q, + ToppraJointTrajectoryPlanner::buildNaturalMulti(const std::vector& q, const std::vector& S) { using PWP = toppra::PiecewisePolyPath; @@ -278,7 +278,7 @@ namespace cmvr { // —— 多点：Quintic C² —— std::shared_ptr - ToppraBSpline::buildQuinticC2Multi(const std::vector& q, + ToppraJointTrajectoryPlanner::buildQuinticC2Multi(const std::vector& q, const std::vector& S) { const size_t M = q.size(), DoF = q[0].size(); auto v = estimateVelsCatmull(q, S); @@ -312,7 +312,7 @@ namespace cmvr { return std::make_shared(segs, std::vector(S.begin(), S.end())); } - std::vector ToppraBSpline::sampleTrajectory(const TrajPtr &traj, double dt) { + std::vector ToppraJointTrajectoryPlanner::sampleTrajectory(const TrajPtr &traj, double dt) { std::vector out; if (!traj) return out; @@ -357,7 +357,7 @@ namespace cmvr { } - bool ToppraBSpline::writeTrajectoryCsv(const std::string &filename, const std::vector &samples) { + bool ToppraJointTrajectoryPlanner::writeTrajectoryCsv(const std::string &filename, const std::vector &samples) { if (samples.empty()) return false; const int dof = static_cast(samples.front().q.size()); diff --git a/cmvr-es/planner/joint_space_planner/src/joint_space_planner_test.cpp b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner_test.cpp similarity index 85% rename from cmvr-es/planner/joint_space_planner/src/joint_space_planner_test.cpp rename to cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner_test.cpp index 815da9bc..91e907b6 100644 --- a/cmvr-es/planner/joint_space_planner/src/joint_space_planner_test.cpp +++ b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner_test.cpp @@ -3,12 +3,14 @@ // #include "gtest/gtest.h" -#include "planner/joint_space_planner/include/toppra_bspline.h" -#include "planner/joint_space_planner/include/joint_space_planner_creator.h" -using namespace cmvr; -TEST(JOINT_SPACE_PLANNER_TEST,TOPPRA_TEST) { +#include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h" - auto planner = JointSpacePlannerCreator::create(JointSpacePlannerType::TOPPRA_BSPLINE); +#include + +using namespace cmvr; +TEST(TOPPRA_JOINT_TRAJECTORY_PLANNER_TEST,TOPPRA_TEST) { + + auto planner = std::make_shared(); planner->setPathType(PathType::Quintic); planner->setSymmetricLimits(std::vector(7, 1.5), @@ -30,9 +32,9 @@ TEST(JOINT_SPACE_PLANNER_TEST,TOPPRA_TEST) { } -TEST(JOINT_SPACE_PLANNER_TEST, TOPPRA_WAYPOINTS_TEST) { +TEST(TOPPRA_JOINT_TRAJECTORY_PLANNER_TEST, TOPPRA_WAYPOINTS_TEST) { - auto planner = JointSpacePlannerCreator::create(JointSpacePlannerType::TOPPRA_BSPLINE); + auto planner = std::make_shared(); planner->setPathType(PathType::Quintic); // 7 自由度对称速度 / 加速度约束 @@ -83,4 +85,4 @@ TEST(JOINT_SPACE_PLANNER_TEST, TOPPRA_WAYPOINTS_TEST) { } else { std::cout << "CSV saved: traj.csv\n"; } -} \ No newline at end of file +} diff --git a/cmvr-es/common/curve/include/s_curve.h b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve.h similarity index 100% rename from cmvr-es/common/curve/include/s_curve.h rename to cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve.h diff --git a/cmvr-es/planner/s_curve_planner/include/s_curve_position_planner.h b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve_position_planner.h similarity index 100% rename from cmvr-es/planner/s_curve_planner/include/s_curve_position_planner.h rename to cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve_position_planner.h diff --git a/cmvr-es/planner/s_curve_planner/include/s_curve_velocity_planner.h b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve_velocity_planner.h similarity index 100% rename from cmvr-es/planner/s_curve_planner/include/s_curve_velocity_planner.h rename to cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve_velocity_planner.h diff --git a/cmvr-es/common/curve/src/s_curve.cpp b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve.cpp similarity index 99% rename from cmvr-es/common/curve/src/s_curve.cpp rename to cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve.cpp index ae0bed89..720becab 100644 --- a/cmvr-es/common/curve/src/s_curve.cpp +++ b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve.cpp @@ -8,7 +8,7 @@ * @brief S 曲线轨迹生成器实现 */ -#include "common/curve/include/s_curve.h" +#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve.h" #include #include #include @@ -187,7 +187,7 @@ SCurveProfile SCurve::calculateProfile(double start_position, double end_positio void SCurve::calculateShortProfile(SCurveProfile& profile) const { - // 短距离：无巡航段（t4 = 0），与 moveL_SCurveLocal 中的 SCurveProfile1D 保持一致。 + // 短距离：无巡航段（t4 = 0）。 const double j = profile.j_max; const double a = profile.a_max; const double v = profile.v_max; diff --git a/cmvr-es/planner/s_curve_planner/src/s_curve_position_planner.cpp b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_position_planner.cpp similarity index 100% rename from cmvr-es/planner/s_curve_planner/src/s_curve_position_planner.cpp rename to cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_position_planner.cpp diff --git a/cmvr-es/planner/s_curve_planner/src/s_curve_velocity_planner.cpp b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp similarity index 100% rename from cmvr-es/planner/s_curve_planner/src/s_curve_velocity_planner.cpp rename to cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp diff --git a/cmvr-es/perception/CMakeLists.txt b/cmvr-es/algorithms/perception/CMakeLists.txt similarity index 86% rename from cmvr-es/perception/CMakeLists.txt rename to cmvr-es/algorithms/perception/CMakeLists.txt index b5876789..6eef84ee 100644 --- a/cmvr-es/perception/CMakeLists.txt +++ b/cmvr-es/algorithms/perception/CMakeLists.txt @@ -2,8 +2,8 @@ find_package(OpenCV REQUIRED) add_library(perception SHARED - src/tag_relative_target_3d.cpp - src/apriltag_perception.cpp + apriltag/src/tag_relative_target_3d.cpp + apriltag/src/apriltag_perception.cpp ) target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) @@ -33,7 +33,7 @@ add_library(cmvr_es::perception ALIAS perception) install(TARGETS perception LIBRARY DESTINATION lib) add_executable(tag_relative_target_3d_test - src/tag_relative_target_3d_test.cpp + apriltag/src/tag_relative_target_3d_test.cpp ) target_link_libraries(tag_relative_target_3d_test diff --git a/cmvr-es/perception/include/apriltag_perception.h b/cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h similarity index 100% rename from cmvr-es/perception/include/apriltag_perception.h rename to cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h diff --git a/cmvr-es/perception/include/tag_relative_target_3d.h b/cmvr-es/algorithms/perception/apriltag/include/tag_relative_target_3d.h similarity index 99% rename from cmvr-es/perception/include/tag_relative_target_3d.h rename to cmvr-es/algorithms/perception/apriltag/include/tag_relative_target_3d.h index 82cb588d..fe101265 100644 --- a/cmvr-es/perception/include/tag_relative_target_3d.h +++ b/cmvr-es/algorithms/perception/apriltag/include/tag_relative_target_3d.h @@ -12,7 +12,7 @@ #include #include -#include "perception/include/apriltag_perception.h" +#include "algorithms/perception/apriltag/include/apriltag_perception.h" namespace cmvr::perception { diff --git a/cmvr-es/perception/src/apriltag_perception.cpp b/cmvr-es/algorithms/perception/apriltag/src/apriltag_perception.cpp similarity index 99% rename from cmvr-es/perception/src/apriltag_perception.cpp rename to cmvr-es/algorithms/perception/apriltag/src/apriltag_perception.cpp index a084db92..246ea759 100644 --- a/cmvr-es/perception/src/apriltag_perception.cpp +++ b/cmvr-es/algorithms/perception/apriltag/src/apriltag_perception.cpp @@ -2,7 +2,7 @@ // Created by lgv on 2026/3/5. // -#include "perception/include/apriltag_perception.h" +#include "algorithms/perception/apriltag/include/apriltag_perception.h" #include #include diff --git a/cmvr-es/perception/src/tag_relative_target_3d.cpp b/cmvr-es/algorithms/perception/apriltag/src/tag_relative_target_3d.cpp similarity index 99% rename from cmvr-es/perception/src/tag_relative_target_3d.cpp rename to cmvr-es/algorithms/perception/apriltag/src/tag_relative_target_3d.cpp index 300fe73d..4ec53e45 100644 --- a/cmvr-es/perception/src/tag_relative_target_3d.cpp +++ b/cmvr-es/algorithms/perception/apriltag/src/tag_relative_target_3d.cpp @@ -1,11 +1,11 @@ -#include "perception/include/tag_relative_target_3d.h" +#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h" #include #include #include #include -#include "common/utils/image/image_process.h" +#include "common/vision/image_projection.h" namespace cmvr::perception { namespace { diff --git a/cmvr-es/perception/src/tag_relative_target_3d_test.cpp b/cmvr-es/algorithms/perception/apriltag/src/tag_relative_target_3d_test.cpp similarity index 99% rename from cmvr-es/perception/src/tag_relative_target_3d_test.cpp rename to cmvr-es/algorithms/perception/apriltag/src/tag_relative_target_3d_test.cpp index 6d6c8f1b..d895251e 100644 --- a/cmvr-es/perception/src/tag_relative_target_3d_test.cpp +++ b/cmvr-es/algorithms/perception/apriltag/src/tag_relative_target_3d_test.cpp @@ -2,7 +2,7 @@ // Created by lgv on 2026/2/26. // -#include "perception/include/tag_relative_target_3d.h" +#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h" #include "devices/camera/realsense_camera/include/realsense_camera.h" #include @@ -301,7 +301,6 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) { cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR); cam_cfg.set_buffer_size(30); cam_cfg.set_sync(true); - cam_cfg.set_enable(true); auto camera = std::make_shared(cam_cfg); @@ -623,7 +622,6 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) { // cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR); // cam_cfg.set_buffer_size(30); // cam_cfg.set_sync(true); -// cam_cfg.set_enable(true); // // auto camera = std::make_shared(cam_cfg); // cmvr::perception::TagRelativeTarget3D tracker(camera); diff --git a/cmvr-es/applications/CMakeLists.txt b/cmvr-es/applications/CMakeLists.txt deleted file mode 100644 index 331fd67c..00000000 --- a/cmvr-es/applications/CMakeLists.txt +++ /dev/null @@ -1,31 +0,0 @@ -add_library(applications - src/touch_screen_app.cpp -) - -target_include_directories(applications PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -target_link_libraries(applications - PUBLIC - cmvr_es::controller - cmvr_es::common - PRIVATE - cmvr_es::device_manager -) - -add_library(cmvr_es::applications ALIAS applications) -install(TARGETS applications LIBRARY DESTINATION lib) - -add_executable(touch_screen_app_test - src/touch_screen_app_test.cpp -) - -target_link_libraries(touch_screen_app_test PRIVATE - cmvr_es::applications - cmvr_es::device_manager - cmvr_es::service - cmvr_es::monitor_manager - gtest - gtest_main - pthread - glog -) diff --git a/cmvr-es/applications/include/touch_screen_app.h b/cmvr-es/applications/include/touch_screen_app.h deleted file mode 100644 index c7f2ea6a..00000000 --- a/cmvr-es/applications/include/touch_screen_app.h +++ /dev/null @@ -1,329 +0,0 @@ -#pragma once - -#ifndef CMVR_ES_TOUCH_SCREEN_APP_H -#define CMVR_ES_TOUCH_SCREEN_APP_H - -#include -#include -#include -#include -#include - -#include - -#include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h" -#include "controller/include/ibvs_controller.h" -#include "devices/camera/abstract_camera.h" -#include "devices/dexhand/abstract_dexhand.h" -#include "devices/robot/abstract_robot.h" -#include "perception/include/apriltag_perception.h" -#include "perception/include/tag_relative_target_3d.h" - -namespace cmvr::app { - -class TouchScreenApp { -public: - enum class Phase { - IDLE = 0, // 空闲，尚未开始任务。 - ALIGNING, // 视觉对准阶段：持续 IBVS 对齐目标点。 - ALIGN_REACHED, // 视觉对准已达到阈值，等待进入下一阶段。 - TOUCHING, // 前进触控阶段：沿设定方向向屏幕推进。 - DWELLING, // 已检测到接触，保持当前位置短暂停留。 - RETRACTING, // 回退阶段：沿设定回退方向离开屏幕。 - DONE, // 整个流程成功完成。 - FAILED // 流程失败并已停止。 - }; - - enum class Status { - IDLE = 0, // 空闲状态。 - NOT_INITIALIZED, // 尚未调用 init() 完成初始化。 - INVALID_CONFIG, // 配置非法，无法启动或应用参数。 - CONTROL_JOINT_MISMATCH, // 控制关节顺序与 IK 链不一致。 - ALIGN_WAITING_PERCEPTION, // 对准阶段等待相机/AprilTag 感知结果。 - ALIGN_WAITING_TRACK, // 对准阶段等待目标点跟踪恢复成功。 - ALIGN_TARGET_SETUP_FAILED,// 视觉目标设置失败，setTargetFromPointInTag 失败。 - ALIGN_COMPUTE_FAILED, // 对准阶段 IBVS 或 IK 计算失败。 - ALIGN_TIMEOUT, // 对准阶段超时仍未收敛。 - ALIGNING, // 正在执行视觉对准。 - ALIGN_REACHED, // 视觉对准完成。 - TOUCHING, // 正在向前触控。 - TACTILE_UNAVAILABLE, // 触觉数据不可用。 - TOUCH_TRIGGERED, // 已检测到接触触发。 - TOUCH_FORWARD_TIMEOUT, // 前进触控时间到，但未触发接触。 - RETRACTING, // 正在回退离开屏幕。 - DONE, // 流程成功完成。 - STOPPED, // 被外部 stop() 主动停止。 - ROBOT_STATE_FAILED, // 读取机器人状态失败。 - ROBOT_COMMAND_FAILED // 向机器人下发控制命令失败。 - }; - - enum class AlignMode { - POSE_AND_POSITION = 0, // 使用配置里的固定 rx/ry/rz 与位置一起对齐。 - RX_RY_AND_POSITION, // 使用配置里的 rx/ry，保留锁定时看到的 tag 平面内 yaw，再与位置一起对齐。 - POSITION_ONLY // 保留锁定时看到的完整 tag 姿态，只按位置对齐。 - }; - - enum class TactileCriterion { - FZ = 0, - MAGNITUDE - }; - - struct Config { - // 是否在触控流程开始前先回到指定初始关节位姿。 - bool move_to_init_position_before_start{false}; - // 是否在触控流程结束（DONE/FAILED）后回到指定初始关节位姿。 - bool move_to_init_position{false}; - // 初始关节位姿目标，在前置回位或结束后回位开启时使用。 - std::vector init_joint_positions{}; - // 回到初始位姿时的 moveJ 主导速度，单位 rad/s。 - double init_movej_vel{1.0}; - // 回到初始位姿时的 moveJ 主导加速度，单位 rad/s^2。 - double init_movej_acc{2.0}; - - // IBVS / IK 初始化参数。 - // URDF 文件路径，用于初始化 IbvsController 内部 IK 求解器。 - std::string urdf_path; - // IK 链基座 link 名称。 - std::string base_link{"PELVIS_S"}; - // IK 链末端法兰 link 名称。 - std::string flange_link{"R_WRIST_R_S"}; - // URDF 中相机 link 名称。 - std::string camera_link; - - // 视觉感知参数。 - // AprilTag 实际边长，单位米。 - double tag_size_m{0.12}; - // 感知更新时如何使用深度图：不用 / 尽量用 / 必须用。 - perception::AprilTagPerception::DepthPolicy depth_policy{ - perception::AprilTagPerception::DepthPolicy::NONE}; - // 从像素恢复目标点时采用 tag 平面求交，还是深度图反投影。 - perception::TagRelativeTarget3D::TargetPointMethod target_point_method{ - perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE}; - - // 视觉阶段目标：触控点在相机坐标系中的 hover 位置。 - // 目标点在相机坐标系中的期望位置，单位米。 - Eigen::Vector3d hover_target_in_camera{0.0, 0.0, 0.40}; - // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。 - double target_rx{3.14159265358979323846}; - // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。 - double target_ry{0.0}; - // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。 - double target_rz{0.0}; - // 对齐模式：1) 固定姿态+位置；2) 固定 rx/ry + 锁定时 yaw + 位置；3) 仅位置。 - AlignMode align_mode{AlignMode::POSE_AND_POSITION}; - - // IBVS 参数。 - // 视觉伺服增益 lambda。 - double ibvs_lambda{0.6}; - // DLS IK 阻尼系数 mu。 - double ibvs_mu{0.1}; - // 单关节最大速度，单位 rad/s。 - double ibvs_qdot_max{0.15}; - // 相机 twist 六维限幅 `[vx, vy, vz, wx, wy, wz]`。 - std::array ibvs_vmax6{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}}; - // 相机 twist 六维加速度限幅 `[ax, ay, az, alphax, alphay, alphaz]`； - // 分量小于等于 0 表示该维度不启用加速度限幅。 - std::array ibvs_amax6{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; - // 相机 twist 一阶低通滤波系数；取值在 (0, 1) 时启用低通，默认 1.0 表示不过滤。 - double ibvs_twist_filter_alpha{1.0}; - // 是否启用关节限位回避。 - bool enable_joint_limit_avoidance{true}; - // 关节限位回避增益。 - double joint_limit_avoidance_gain{0.2}; - // 距离关节限位多近时开始回避，按关节范围比例计算。 - double joint_limit_avoidance_margin_ratio{0.15}; - // 单关节限位回避最大推回速度。 - double joint_limit_avoidance_max_push{0.25}; - - // `AbstractCamera` 相机坐标系到 ViSP 相机坐标系的旋转矩阵。 - Eigen::Matrix3d R_camera_to_visp{Eigen::Matrix3d::Identity()}; - // `AbstractCamera` 相机坐标系到 URDF 相机坐标系的旋转矩阵。 - Eigen::Matrix3d R_camera_to_urdf{Eigen::Matrix3d::Identity()}; - - // 关节控制链，默认右臂 7 轴。 - // 顺序必须与 IbvsController 内部 IK 链顺序一致。 - std::vector control_joint_names{ - "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", - "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"}; - - // 视觉对准收敛判据 `[x, y, z, rx, ry, rz]`。 - // 其中位置误差单位米，旋转误差单位弧度；旋转部分使用目标姿态误差 rotvec 的三个分量分别比较。 - std::array align_error_threshold6{{0.003, 0.003, 0.010, - 0.08726646259971647, - 0.08726646259971647, - 0.08726646259971647}}; - // 连续多少帧都满足阈值，才认为对准完成。 - int align_stable_frames{5}; - // 对准阶段超时时间，单位秒。 - double align_timeout_s{10.0}; - // 为 true 时对准完成后暂停，不自动进入触控阶段。 - bool pause_after_align_reached{false}; - - // 触控阶段前进方向。当前按末端 Tool 坐标系解释，字段名保留兼容。 - // 为 true 时，TOUCHING 阶段使用 speedL；为 false 时使用 moveL。 - // 当前 moveL 路径为同步前进，执行完成后直接结束流程，并按配置决定是否回初始位姿。 - bool touch_use_speedl{true}; - // 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`，单位 m/s 和 rad/s。 - // 当前机器人上 `[0, -0.08, 0, 0, 0, 0]` 表示沿 Tool -Y 方向向前触屏。 - // 当 TOUCHING 使用 moveL 时，会取其线速度方向并按 touch_forward_l 构造位移目标。 - Eigen::Matrix touch_twist_base{ - (Eigen::Matrix() << 0.0, -0.08, 0.0, 0.0, 0.0, 0.0).finished()}; - // 触控阶段 speedL 的加速度参数。 - double touch_speedl_acceleration{3.0}; - // 触控阶段使用 moveL 时，沿 touch_twist_base 线速度方向前进的距离，单位米。 - double touch_forward_l{0.08}; - // 触控阶段使用 moveL 时的末端速度，单位 m/s。 - double touch_movel_speed{0.25}; - // 触控阶段使用 moveL 时的末端加速度，单位 m/s^2。 - double touch_movel_acceleration{1.2}; - // 触控阶段使用 moveL 时的末端 jerk，单位 m/s^3。 - double touch_movel_jerk{5.0}; - // 触控阶段使用 moveL 时的关节速度上限；为空时退回 robot->moveL 默认值。 - std::vector touch_movel_qd_max{2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5}; - // 前进触控阶段的最大累计位移，单位米。 - // 该距离仅在 TOUCHING 使用 speedL 时生效，由 TouchScreenApp 根据末端相对触控起点的累计位移判断。 - // 大于 0 时，达到该距离后无论压力是否达阈值，都会立即进入回退阶段。 - // 小于等于 0 时，表示不启用这条限制。 - double touch_speedl_forward_l{0.08}; - - // 接触后停留与回退。当前按末端 Tool 坐标系解释。 - // 检测到接触后在当前位置停留的时间，单位秒。 - // 当该值小于 0 时，表示不做停留，直接把 speedL 切换为回退。 - double dwell_time_s{0.05}; - // 回退阶段的 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`。 - // 当前机器人上 `[0, +0.08, 0, 0, 0, 0]` 表示沿 Tool +Y 方向向后离屏。 - // 回退阶段统一使用 speedL。 - Eigen::Matrix retract_twist_base{ - (Eigen::Matrix() << 0.0, 0.08,0.0, 0.0, 0.0, 0.0).finished()}; - // 回退阶段 speedL 的加速度参数。 - double retract_acceleration{3.0}; - // 回退阶段 speedL 持续时间，单位秒。 - double retract_duration_s{0.8}; - - // 指尖触觉判据。 - // 使用哪根手指的触觉阵列判断是否接触。 - device::AbstractDexHand::FingerType tactile_finger{device::AbstractDexHand::FingerType::INDEX}; - // 使用该手指的哪个触觉区域。 - device::AbstractDexHand::TactileRegion tactile_region{ - device::AbstractDexHand::TactileRegion::TIP}; - // 三维合力标量化方式：直接使用法向 fz，或使用三维力模长。 - TactileCriterion tactile_criterion{TactileCriterion::FZ}; - // 触觉区域三维合力按 tactile_criterion 标量化后的阈值；超过该值认为已经接触。 - double tactile_pressure_sum_threshold{100.0}; - // 保留兼容的旧字段；当前合力判定逻辑不再使用非零点数量阈值。 - int tactile_nonzero_count_threshold{1}; - }; - - TouchScreenApp(); - ~TouchScreenApp() = default; - - bool init(); - bool init(const std::shared_ptr& robot, - const std::shared_ptr& dexhand, - const std::shared_ptr& camera); - bool init(const std::shared_ptr& robot, - const std::shared_ptr& dexhand, - const std::shared_ptr& camera, - const cmvr::config::TouchScreenAppConfig& config); - - bool setConfigFromProto(); - bool setConfigFromProto(const cmvr::config::TouchScreenAppConfig& config); - bool setTouchSpeedlForwardL(double forward_l); - - bool startFromPixel(int u, int v); - bool step(double dt); - void stop(); - - Phase phase() const { return phase_; } - Status lastStatus() const { return last_status_; } - static const char* phaseToString(Phase phase); - static const char* statusToString(Status status); - - bool isBusy() const { return phase_ == Phase::ALIGNING || phase_ == Phase::ALIGN_REACHED || - phase_ == Phase::TOUCHING || phase_ == Phase::DWELLING || - phase_ == Phase::RETRACTING; } - bool isFinished() const { return phase_ == Phase::DONE; } - bool isFailed() const { return phase_ == Phase::FAILED; } - - int targetU() const { return target_u_; } - int targetV() const { return target_v_; } - double lastTouchPressureSum() const { return last_touch_pressure_sum_; } - int lastTouchNonzeroCount() const { return last_touch_nonzero_count_; } - int lastActiveTagId() const { return last_active_tag_id_; } - const Eigen::Vector3d& lastAlignErrorCamera() const { return last_align_error_camera_; } - - const std::shared_ptr& perception() const { return perception_; } - const perception::TagRelativeTarget3D& tracker() const { return tracker_; } - const IbvsController& ibvs() const { return ibvs_; } - -private: - using Clock = std::chrono::steady_clock; - - static bool configFromProto(const cmvr::config::TouchScreenAppConfig& proto_config, - Config& config_out); - void setConfig(const Config& config); - bool applyConfig(); - bool validateControlJointNames() const; - bool stepAligning(double dt); - bool stepTouching(); - bool stepDwelling(); - bool stepRetracting(); - - bool readControlledJointPositions(std::vector& q_out) const; - bool sendJointVelocity(const std::vector& qdot) const; - bool sendZeroJointVelocity() const; - void hardStopIbvsMotion(); - bool holdCurrentControlledPosition() const; - bool moveToInitPositionIfEnabled() const; - bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const; - void logTouchingSpeedLState() const; - - bool startTouchPhase(); - bool handleTouchTriggered(bool stop_forward_motion); - bool startRetractPhase(Phase next_phase_after_retract, Status final_status_after_retract); - void enterFailed(Status status); - - bool updateTouchPressure(); - -private: - std::shared_ptr robot_{nullptr}; - std::shared_ptr dexhand_{nullptr}; - std::shared_ptr camera_{nullptr}; - - std::shared_ptr perception_{nullptr}; - perception::TagRelativeTarget3D tracker_; - IbvsController ibvs_; - Config config_{}; - - Phase phase_{Phase::IDLE}; - Phase phase_after_retract_{Phase::DONE}; - Status last_status_{Status::NOT_INITIALIZED}; - - bool initialized_{false}; - bool target_locked_{false}; - bool ibvs_target_initialized_{false}; - bool touch_command_started_{false}; - bool retract_command_started_{false}; - - int target_u_{-1}; - int target_v_{-1}; - int align_stable_count_{0}; - int last_active_tag_id_{-1}; - - double last_touch_pressure_sum_{0.0}; - int last_touch_nonzero_count_{0}; - Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()}; - bool locked_target_rotation_valid_{false}; - Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()}; - bool touch_start_position_valid_{false}; - Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()}; - - Clock::time_point phase_start_time_{}; - Status final_status_after_retract_{Status::DONE}; -}; - -using touch_screen_app = TouchScreenApp; - -} // namespace cmvr::app - -#endif // CMVR_ES_TOUCH_SCREEN_APP_H diff --git a/cmvr-es/applications/src/touch_screen_app.cpp b/cmvr-es/applications/src/touch_screen_app.cpp deleted file mode 100644 index 8aefc4ff..00000000 --- a/cmvr-es/applications/src/touch_screen_app.cpp +++ /dev/null @@ -1,1686 +0,0 @@ -#include "applications/include/touch_screen_app.h" - -#include -#include -#include -#include -#include -#include - -#include "common/utils/config_helper/include/config_helper.h" -#include "device_manager/include/device_manager.h" -#include - -namespace cmvr::app { - -namespace { - -std::vector toStdVector6(const Eigen::Matrix& twist) { - std::vector out(6, 0.0); - for (int i = 0; i < 6; ++i) { - out[static_cast(i)] = twist[i]; - } - return out; -} - -bool computeLinearMoveDeltaTool(const Eigen::Matrix& twist_base, - const double move_length, - Eigen::Vector3d& delta_out) { - if (!std::isfinite(move_length) || move_length <= 0.0) { - return false; - } - - const Eigen::Vector3d linear = twist_base.head<3>(); - if (!linear.allFinite()) { - return false; - } - - const double linear_norm = linear.norm(); - if (!std::isfinite(linear_norm) || linear_norm <= 1e-9) { - return false; - } - - delta_out = linear / linear_norm * move_length; - return delta_out.allFinite(); -} - -bool isTouchTriggered(const TouchScreenApp::Config& config, - const double resultant_force_value) { - return resultant_force_value >= config.tactile_pressure_sum_threshold; -} - -double tactileForceValue(const device::AbstractDexHand::TactilePoint& point, - const TouchScreenApp::TactileCriterion criterion) { - switch (criterion) { - case TouchScreenApp::TactileCriterion::FZ: - return static_cast(point.fz); - case TouchScreenApp::TactileCriterion::MAGNITUDE: - return point.magnitude(); - } - return static_cast(point.fz); -} - -bool requiresReinitForConfigChange(const TouchScreenApp::Config& current, - const TouchScreenApp::Config& requested) { - const auto init_joints_differ = [&]() { - if (current.init_joint_positions.size() != requested.init_joint_positions.size()) { - return true; - } - for (size_t i = 0; i < current.init_joint_positions.size(); ++i) { - if (current.init_joint_positions[i].joint_name != requested.init_joint_positions[i].joint_name || - current.init_joint_positions[i].rad != requested.init_joint_positions[i].rad) { - return true; - } - } - return false; - }; - return current.move_to_init_position_before_start != requested.move_to_init_position_before_start || - current.move_to_init_position != requested.move_to_init_position || - init_joints_differ() || - current.init_movej_vel != requested.init_movej_vel || - current.init_movej_acc != requested.init_movej_acc || - current.urdf_path != requested.urdf_path || - current.base_link != requested.base_link || - current.flange_link != requested.flange_link || - current.camera_link != requested.camera_link || - current.control_joint_names != requested.control_joint_names; -} - -Eigen::Matrix3d rotationFromTargetRotvec(double rx, double ry, double rz) { - vpRotationMatrix R_visp; - R_visp.buildFrom(rx, ry, rz); - - Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - R(r, c) = R_visp[r][c]; - } - } - return R; -} - -double rotationErrorRad(const Eigen::Matrix3d& R_current, - const Eigen::Matrix3d& R_target) { - if (!R_current.allFinite() || !R_target.allFinite()) { - return std::numeric_limits::infinity(); - } - - const Eigen::Matrix3d R_err = R_current * R_target.transpose(); - const double cos_angle = std::clamp(0.5 * (R_err.trace() - 1.0), -1.0, 1.0); - return std::acos(cos_angle); -} - -Eigen::Vector3d rotvecFromRotationMatrix(const Eigen::Matrix3d& R) { - const Eigen::AngleAxisd aa(R); - if (!std::isfinite(aa.angle()) || !aa.axis().allFinite() || std::abs(aa.angle()) <= 1e-12) { - return Eigen::Vector3d::Zero(); - } - return aa.axis() * aa.angle(); -} - -bool extractProjectedYawAboutTargetNormal(const Eigen::Matrix3d& R_target, - const Eigen::Matrix3d& R_current, - double& yaw_rad_out) { - if (!R_target.allFinite() || !R_current.allFinite()) { - return false; - } - - const Eigen::Vector3d z_ref = R_target.col(2); - const Eigen::Vector3d x_ref = R_target.col(0); - const Eigen::Vector3d y_ref = R_target.col(1); - - Eigen::Vector3d in_plane = R_current.col(0) - z_ref * z_ref.dot(R_current.col(0)); - if (in_plane.norm() <= 1e-9) { - in_plane = R_current.col(1) - z_ref * z_ref.dot(R_current.col(1)); - } - const double in_plane_norm = in_plane.norm(); - if (!std::isfinite(in_plane_norm) || in_plane_norm <= 1e-9) { - return false; - } - - in_plane /= in_plane_norm; - yaw_rad_out = std::atan2(y_ref.dot(in_plane), x_ref.dot(in_plane)); - return std::isfinite(yaw_rad_out); -} - -bool appendRequestedTactileRegions( - const device::AbstractDexHand::FingerType finger, - const device::AbstractDexHand::TactileRegion region, - std::vector& regions_out) { - using DeviceTactileRegion = device::AbstractDexHand::TactileRegion; - - switch (region) { - case device::AbstractDexHand::TactileRegion::TIP: - regions_out.emplace_back(finger, DeviceTactileRegion::TIP); - return true; - case device::AbstractDexHand::TactileRegion::FINGER: - regions_out.emplace_back(finger, DeviceTactileRegion::FINGER); - return true; - case device::AbstractDexHand::TactileRegion::PAD: - regions_out.emplace_back(finger, DeviceTactileRegion::PAD); - return true; - case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE: - if (finger != device::AbstractDexHand::FingerType::THUMB) { - return false; - } - regions_out.emplace_back(finger, DeviceTactileRegion::THUMB_MIDDLE); - return true; - } - return false; -} - -perception::AprilTagPerception::DepthPolicy toDepthPolicy( - const cmvr::config::TouchScreenDepthPolicy policy) { - switch (policy) { - case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_NONE: - return perception::AprilTagPerception::DepthPolicy::NONE; - case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_PREFER: - return perception::AprilTagPerception::DepthPolicy::PREFER; - case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_REQUIRE: - return perception::AprilTagPerception::DepthPolicy::REQUIRE; - } - return perception::AprilTagPerception::DepthPolicy::NONE; -} - -perception::TagRelativeTarget3D::TargetPointMethod toTargetPointMethod( - const cmvr::config::TouchScreenTargetPointMethod method) { - switch (method) { - case cmvr::config::TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE: - return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE; - case cmvr::config::TOUCH_SCREEN_TARGET_POINT_METHOD_DEPTH_IMAGE: - return perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE; - } - return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE; -} - -device::AbstractDexHand::FingerType toFingerType(const cmvr::config::TouchScreenFingerType finger) { - switch (finger) { - case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_PINKY: - return device::AbstractDexHand::FingerType::PINKY; - case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_RING: - return device::AbstractDexHand::FingerType::RING; - case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_MIDDLE: - return device::AbstractDexHand::FingerType::MIDDLE; - case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_INDEX: - return device::AbstractDexHand::FingerType::INDEX; - case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_THUMB: - return device::AbstractDexHand::FingerType::THUMB; - } - return device::AbstractDexHand::FingerType::INDEX; -} - -const char* fingerTypeToString(const device::AbstractDexHand::FingerType finger) { - switch (finger) { - case device::AbstractDexHand::FingerType::PINKY: return "PINKY"; - case device::AbstractDexHand::FingerType::RING: return "RING"; - case device::AbstractDexHand::FingerType::MIDDLE: return "MIDDLE"; - case device::AbstractDexHand::FingerType::INDEX: return "INDEX"; - case device::AbstractDexHand::FingerType::THUMB: return "THUMB"; - case device::AbstractDexHand::FingerType::PALM: return "PALM"; - } - return "UNKNOWN"; -} - -const char* tactileRegionToString(const device::AbstractDexHand::TactileRegion region) { - switch (region) { - case device::AbstractDexHand::TactileRegion::TIP: return "TIP"; - case device::AbstractDexHand::TactileRegion::FINGER: return "FINGER"; - case device::AbstractDexHand::TactileRegion::PAD: return "PAD"; - case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE: return "THUMB_MIDDLE"; - case device::AbstractDexHand::TactileRegion::PALM_PAD: return "PALM_PAD"; - } - return "UNKNOWN"; -} - -device::AbstractDexHand::TactileRegion toTactileRegion( - const cmvr::config::TouchScreenTactileRegion region) { - switch (region) { - case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP: - return device::AbstractDexHand::TactileRegion::TIP; - case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_FINGER: - return device::AbstractDexHand::TactileRegion::FINGER; - case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_PAD: - return device::AbstractDexHand::TactileRegion::PAD; - case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE: - return device::AbstractDexHand::TactileRegion::THUMB_MIDDLE; - } - return device::AbstractDexHand::TactileRegion::TIP; -} - -TouchScreenApp::TactileCriterion toTactileCriterion( - const cmvr::config::TouchScreenTactileCriterion criterion) { - switch (criterion) { - case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_FZ: - return TouchScreenApp::TactileCriterion::FZ; - case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_MAGNITUDE: - return TouchScreenApp::TactileCriterion::MAGNITUDE; - } - return TouchScreenApp::TactileCriterion::FZ; -} - -TouchScreenApp::AlignMode toAlignMode(const cmvr::config::TouchScreenAlignMode mode) { - switch (mode) { - case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION: - return TouchScreenApp::AlignMode::POSE_AND_POSITION; - case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION: - return TouchScreenApp::AlignMode::RX_RY_AND_POSITION; - case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY: - return TouchScreenApp::AlignMode::POSITION_ONLY; - } - return TouchScreenApp::AlignMode::POSE_AND_POSITION; -} - -void applyVec3FromConfig(const cmvr::config::TouchScreenVec3& src, - Eigen::Vector3d& dst) { - if (src.has_x()) { - dst.x() = src.x(); - } - if (src.has_y()) { - dst.y() = src.y(); - } - if (src.has_z()) { - dst.z() = src.z(); - } -} - -void applyTwist6FromConfig(const cmvr::config::TouchScreenTwist6& src, - Eigen::Matrix& dst) { - if (src.has_vx()) { - dst[0] = src.vx(); - } - if (src.has_vy()) { - dst[1] = src.vy(); - } - if (src.has_vz()) { - dst[2] = src.vz(); - } - if (src.has_wx()) { - dst[3] = src.wx(); - } - if (src.has_wy()) { - dst[4] = src.wy(); - } - if (src.has_wz()) { - dst[5] = src.wz(); - } -} - -void applyErrorThreshold6FromConfig(const cmvr::config::TouchScreenErrorThreshold6& src, - Eigen::Matrix& dst) { - if (src.has_x()) { - dst[0] = src.x(); - } - if (src.has_y()) { - dst[1] = src.y(); - } - if (src.has_z()) { - dst[2] = src.z(); - } - if (src.has_rx()) { - dst[3] = src.rx(); - } - if (src.has_ry()) { - dst[4] = src.ry(); - } - if (src.has_rz()) { - dst[5] = src.rz(); - } -} - -void applyMatrix3dFromConfig(const cmvr::config::TouchScreenMatrix3d& src, - Eigen::Matrix3d& dst) { - if (src.has_m00()) { - dst(0, 0) = src.m00(); - } - if (src.has_m01()) { - dst(0, 1) = src.m01(); - } - if (src.has_m02()) { - dst(0, 2) = src.m02(); - } - if (src.has_m10()) { - dst(1, 0) = src.m10(); - } - if (src.has_m11()) { - dst(1, 1) = src.m11(); - } - if (src.has_m12()) { - dst(1, 2) = src.m12(); - } - if (src.has_m20()) { - dst(2, 0) = src.m20(); - } - if (src.has_m21()) { - dst(2, 1) = src.m21(); - } - if (src.has_m22()) { - dst(2, 2) = src.m22(); - } -} - -} // namespace - -TouchScreenApp::TouchScreenApp() - : tracker_(nullptr) {} - -bool TouchScreenApp::init() { - cmvr::config::TouchScreenAppConfig config; - if (!cmvr::ConfigHelper::getTouchScreenAppConfig(config)) { - initialized_ = false; - last_status_ = Status::INVALID_CONFIG; - return false; - } - - if (!config.has_robot_id() || config.robot_id().empty() || - !config.has_camera_id() || config.camera_id().empty() || - !config.has_dexhand_id() || config.dexhand_id().empty()) { - initialized_ = false; - last_status_ = Status::INVALID_CONFIG; - return false; - } - - try { - auto& dm = device::DeviceManager::getInstance(); - auto robot = dm.getDevice(config.robot_id()); - auto dexhand = dm.getDevice(config.dexhand_id()); - auto camera = dm.getDevice(config.camera_id()); - camera->start(); - return init(robot, dexhand, camera, config); - } catch (...) { - initialized_ = false; - last_status_ = Status::INVALID_CONFIG; - return false; - } -} - -bool TouchScreenApp::init(const std::shared_ptr& robot, - const std::shared_ptr& dexhand, - const std::shared_ptr& camera) { - cmvr::config::TouchScreenAppConfig config; - if (!cmvr::ConfigHelper::getTouchScreenAppConfig(config)) { - initialized_ = false; - last_status_ = Status::INVALID_CONFIG; - return false; - } - return init(robot, dexhand, camera, config); -} - -bool TouchScreenApp::init(const std::shared_ptr& robot, - const std::shared_ptr& dexhand, - const std::shared_ptr& camera, - const cmvr::config::TouchScreenAppConfig& proto_config) { - Config app_config; - if (!configFromProto(proto_config, app_config)) { - initialized_ = false; - last_status_ = Status::INVALID_CONFIG; - return false; - } - - robot_ = robot; - dexhand_ = dexhand; - camera_ = camera; - config_ = app_config; - - if (!robot_ || !camera_) { - initialized_ = false; - last_status_ = Status::INVALID_CONFIG; - return false; - } - - if ((config_.move_to_init_position_before_start || config_.move_to_init_position) && - config_.init_joint_positions.empty()) { - initialized_ = false; - last_status_ = Status::INVALID_CONFIG; - return false; - } - - if (config_.move_to_init_position_before_start) { - try { - auto init_cmd = config_.init_joint_positions; - robot_->moveJ(init_cmd, config_.init_movej_vel, config_.init_movej_acc); - } catch (...) { - initialized_ = false; - last_status_ = Status::ROBOT_COMMAND_FAILED; - return false; - } - } - - if (config_.urdf_path.empty() || config_.camera_link.empty() || config_.control_joint_names.empty()) { - initialized_ = false; - last_status_ = Status::INVALID_CONFIG; - return false; - } - - perception_ = std::make_shared(camera_); - perception_->setTagSize(config_.tag_size_m); - - tracker_.setPerception(perception_); - tracker_.setTargetPointMethod(config_.target_point_method); - - if (!ibvs_.init(config_.urdf_path, - config_.base_link, - config_.flange_link, - config_.camera_link)) { - initialized_ = false; - last_status_ = Status::INVALID_CONFIG; - return false; - } - - ibvs_.setPerception(perception_); - if (!validateControlJointNames()) { - initialized_ = false; - last_status_ = Status::CONTROL_JOINT_MISMATCH; - return false; - } - - initialized_ = applyConfig(); - if (initialized_) { - tracker_.clear(); - tracker_.resetActiveTagTracking(); - ibvs_.reset(); - phase_ = Phase::IDLE; - phase_after_retract_ = Phase::DONE; - final_status_after_retract_ = Status::DONE; - target_locked_ = false; - ibvs_target_initialized_ = false; - touch_command_started_ = false; - retract_command_started_ = false; - align_stable_count_ = 0; - last_active_tag_id_ = -1; - last_touch_pressure_sum_ = 0.0; - last_touch_nonzero_count_ = 0; - last_align_error_camera_.setZero(); - last_status_ = Status::IDLE; - } else { - last_status_ = Status::INVALID_CONFIG; - } - return initialized_; -} - -void TouchScreenApp::setConfig(const Config& config) { - if (!initialized_) { - config_ = config; - return; - } - - const Config previous_config = config_; - Config applied_config = config; - if (requiresReinitForConfigChange(config_, config)) { - std::cerr << "[TouchScreenApp] setConfig() ignored IK-chain config changes after init; " - "call init() again to update device/init pose/urdf/link/joint chain\n"; - applied_config.move_to_init_position_before_start = previous_config.move_to_init_position_before_start; - applied_config.move_to_init_position = previous_config.move_to_init_position; - applied_config.init_joint_positions = previous_config.init_joint_positions; - applied_config.init_movej_vel = previous_config.init_movej_vel; - applied_config.init_movej_acc = previous_config.init_movej_acc; - applied_config.urdf_path = previous_config.urdf_path; - applied_config.base_link = previous_config.base_link; - applied_config.flange_link = previous_config.flange_link; - applied_config.camera_link = previous_config.camera_link; - applied_config.control_joint_names = previous_config.control_joint_names; - } - - config_ = applied_config; - if (!applyConfig()) { - config_ = previous_config; - applyConfig(); - last_status_ = Status::INVALID_CONFIG; - } -} - -bool TouchScreenApp::setConfigFromProto() { - cmvr::config::TouchScreenAppConfig config; - if (!cmvr::ConfigHelper::getTouchScreenAppConfig(config)) { - last_status_ = Status::INVALID_CONFIG; - return false; - } - return setConfigFromProto(config); -} - -bool TouchScreenApp::setConfigFromProto(const cmvr::config::TouchScreenAppConfig& proto_config) { - Config app_config; - if (!configFromProto(proto_config, app_config)) { - last_status_ = Status::INVALID_CONFIG; - return false; - } - setConfig(app_config); - return last_status_ != Status::INVALID_CONFIG; -} - -bool TouchScreenApp::setTouchSpeedlForwardL(double forward_l) { - if (!std::isfinite(forward_l) || forward_l < 0.0) { - last_status_ = Status::INVALID_CONFIG; - return false; - } - - Config updated_config = config_; - updated_config.touch_speedl_forward_l = forward_l; - setConfig(updated_config); - return last_status_ != Status::INVALID_CONFIG; -} - -bool TouchScreenApp::startFromPixel(int u, int v) { - if (!initialized_) { - last_status_ = Status::NOT_INITIALIZED; - return false; - } - if (u < 0 || v < 0) { - last_status_ = Status::INVALID_CONFIG; - return false; - } - - stop(); - tracker_.clear(); - tracker_.resetActiveTagTracking(); - ibvs_.reset(); - - target_u_ = u; - target_v_ = v; - target_locked_ = false; - ibvs_target_initialized_ = false; - touch_command_started_ = false; - retract_command_started_ = false; - align_stable_count_ = 0; - last_touch_pressure_sum_ = 0.0; - last_touch_nonzero_count_ = 0; - last_active_tag_id_ = -1; - last_align_error_camera_.setZero(); - locked_target_rotation_valid_ = false; - locked_target_rotation_.setIdentity(); - touch_start_position_valid_ = false; - touch_start_position_base_.setZero(); - phase_ = Phase::ALIGNING; - phase_after_retract_ = Phase::DONE; - final_status_after_retract_ = Status::DONE; - phase_start_time_ = Clock::now(); - last_status_ = Status::ALIGN_WAITING_TRACK; - return true; -} - -bool TouchScreenApp::step(const double dt) { - if (!initialized_) { - last_status_ = Status::NOT_INITIALIZED; - return false; - } - if (!std::isfinite(dt) || dt <= 0.0) { - last_status_ = Status::INVALID_CONFIG; - return false; - } - - if (dexhand_) { - const bool tactile_ok = updateTouchPressure(); - // std::cout << "[TouchScreenApp][TACTILE] finger=" - // << fingerTypeToString(config_.tactile_finger) - // << ", region=" << tactileRegionToString(config_.tactile_region) - // << ", ok=" << (tactile_ok ? 1 : 0) - // << ", nonzero_count=" << last_touch_nonzero_count_ - // << ", pressure_sum=" << last_touch_pressure_sum_ - // << std::endl; - } - - switch (phase_) { - case Phase::IDLE: - last_status_ = Status::IDLE; - return true; - case Phase::ALIGNING: - return stepAligning(dt); - case Phase::ALIGN_REACHED: - last_status_ = Status::ALIGN_REACHED; - if (config_.pause_after_align_reached) { - return true; - } - if (!startTouchPhase()) { - enterFailed(last_status_ == Status::TACTILE_UNAVAILABLE || - last_status_ == Status::INVALID_CONFIG || - last_status_ == Status::ROBOT_STATE_FAILED - ? last_status_ - : Status::ROBOT_COMMAND_FAILED); - return false; - } - return true; - case Phase::TOUCHING: - return stepTouching(); - case Phase::DWELLING: - return stepDwelling(); - case Phase::RETRACTING: - return stepRetracting(); - case Phase::DONE: - last_status_ = Status::DONE; - return true; - case Phase::FAILED: - return false; - } - last_status_ = Status::INVALID_CONFIG; - return false; -} - -void TouchScreenApp::stop() { - if (robot_) { - try { - robot_->stopSpeedL(); - } catch (...) { - } - } - - sendZeroJointVelocity(); - ibvs_.resetTwistCommandState(); - holdCurrentControlledPosition(); - - phase_ = Phase::IDLE; - phase_after_retract_ = Phase::DONE; - final_status_after_retract_ = Status::DONE; - target_locked_ = false; - ibvs_target_initialized_ = false; - touch_command_started_ = false; - retract_command_started_ = false; - align_stable_count_ = 0; - last_active_tag_id_ = -1; - last_touch_pressure_sum_ = 0.0; - last_touch_nonzero_count_ = 0; - last_align_error_camera_.setZero(); - locked_target_rotation_valid_ = false; - locked_target_rotation_.setIdentity(); - touch_start_position_valid_ = false; - touch_start_position_base_.setZero(); - last_status_ = Status::STOPPED; -} - -const char* TouchScreenApp::phaseToString(const Phase phase) { - switch (phase) { - case Phase::IDLE: return "IDLE"; - case Phase::ALIGNING: return "ALIGNING"; - case Phase::ALIGN_REACHED: return "ALIGN_REACHED"; - case Phase::TOUCHING: return "TOUCHING"; - case Phase::DWELLING: return "DWELLING"; - case Phase::RETRACTING: return "RETRACTING"; - case Phase::DONE: return "DONE"; - case Phase::FAILED: return "FAILED"; - } - return "UNKNOWN"; -} - -const char* TouchScreenApp::statusToString(const Status status) { - switch (status) { - case Status::IDLE: return "IDLE"; - case Status::NOT_INITIALIZED: return "NOT_INITIALIZED"; - case Status::INVALID_CONFIG: return "INVALID_CONFIG"; - case Status::CONTROL_JOINT_MISMATCH: return "CONTROL_JOINT_MISMATCH"; - case Status::ALIGN_WAITING_PERCEPTION: return "ALIGN_WAITING_PERCEPTION"; - case Status::ALIGN_WAITING_TRACK: return "ALIGN_WAITING_TRACK"; - case Status::ALIGN_TARGET_SETUP_FAILED: return "ALIGN_TARGET_SETUP_FAILED"; - case Status::ALIGN_COMPUTE_FAILED: return "ALIGN_COMPUTE_FAILED"; - case Status::ALIGN_TIMEOUT: return "ALIGN_TIMEOUT"; - case Status::ALIGNING: return "ALIGNING"; - case Status::ALIGN_REACHED: return "ALIGN_REACHED"; - case Status::TOUCHING: return "TOUCHING"; - case Status::TACTILE_UNAVAILABLE: return "TACTILE_UNAVAILABLE"; - case Status::TOUCH_TRIGGERED: return "TOUCH_TRIGGERED"; - case Status::TOUCH_FORWARD_TIMEOUT: return "TOUCH_FORWARD_TIMEOUT"; - case Status::RETRACTING: return "RETRACTING"; - case Status::DONE: return "DONE"; - case Status::STOPPED: return "STOPPED"; - case Status::ROBOT_STATE_FAILED: return "ROBOT_STATE_FAILED"; - case Status::ROBOT_COMMAND_FAILED: return "ROBOT_COMMAND_FAILED"; - } - return "UNKNOWN"; -} - -bool TouchScreenApp::configFromProto(const cmvr::config::TouchScreenAppConfig& config, - Config& config_out) { - Config app_config; - - if (config.has_move_to_init_position_before_start()) { - app_config.move_to_init_position_before_start = config.move_to_init_position_before_start(); - } - if (config.has_move_to_init_position()) { - app_config.move_to_init_position = config.move_to_init_position(); - } - if (config.init_joint_positions_size() > 0) { - app_config.init_joint_positions.clear(); - app_config.init_joint_positions.reserve(static_cast(config.init_joint_positions_size())); - for (const auto& joint : config.init_joint_positions()) { - app_config.init_joint_positions.emplace_back( - joint.has_joint_name() ? joint.joint_name() : std::string{}, - joint.has_rad() ? joint.rad() : 0.0); - } - } - if (config.has_init_movej_vel()) { - app_config.init_movej_vel = config.init_movej_vel(); - } - if (config.has_init_movej_acc()) { - app_config.init_movej_acc = config.init_movej_acc(); - } - - if (config.has_urdf_path()) { - app_config.urdf_path = config.urdf_path(); - } - if (config.has_base_link()) { - app_config.base_link = config.base_link(); - } - if (config.has_flange_link()) { - app_config.flange_link = config.flange_link(); - } - if (config.has_camera_link()) { - app_config.camera_link = config.camera_link(); - } - - if (config.has_tag_size_m()) { - app_config.tag_size_m = config.tag_size_m(); - } - if (config.has_depth_policy()) { - app_config.depth_policy = toDepthPolicy(config.depth_policy()); - } - if (config.has_target_point_method()) { - app_config.target_point_method = toTargetPointMethod(config.target_point_method()); - } - - if (config.has_hover_target_in_camera()) { - applyVec3FromConfig(config.hover_target_in_camera(), app_config.hover_target_in_camera); - } - if (config.has_target_rx()) { - app_config.target_rx = config.target_rx(); - } - if (config.has_target_ry()) { - app_config.target_ry = config.target_ry(); - } - if (config.has_target_rz()) { - app_config.target_rz = config.target_rz(); - } - if (config.has_align_mode()) { - app_config.align_mode = toAlignMode(config.align_mode()); - } - - if (config.has_ibvs_lambda()) { - app_config.ibvs_lambda = config.ibvs_lambda(); - } - if (config.has_ibvs_mu()) { - app_config.ibvs_mu = config.ibvs_mu(); - } - if (config.has_ibvs_qdot_max()) { - app_config.ibvs_qdot_max = config.ibvs_qdot_max(); - } - if (config.has_ibvs_vmax6()) { - Eigen::Matrix ibvs_vmax; - ibvs_vmax << app_config.ibvs_vmax6[0], app_config.ibvs_vmax6[1], app_config.ibvs_vmax6[2], - app_config.ibvs_vmax6[3], app_config.ibvs_vmax6[4], app_config.ibvs_vmax6[5]; - applyTwist6FromConfig(config.ibvs_vmax6(), ibvs_vmax); - for (int i = 0; i < 6; ++i) { - app_config.ibvs_vmax6[static_cast(i)] = ibvs_vmax[i]; - } - } - if (config.has_ibvs_amax6()) { - Eigen::Matrix ibvs_amax; - ibvs_amax << app_config.ibvs_amax6[0], app_config.ibvs_amax6[1], app_config.ibvs_amax6[2], - app_config.ibvs_amax6[3], app_config.ibvs_amax6[4], app_config.ibvs_amax6[5]; - applyTwist6FromConfig(config.ibvs_amax6(), ibvs_amax); - for (int i = 0; i < 6; ++i) { - app_config.ibvs_amax6[static_cast(i)] = ibvs_amax[i]; - } - } - if (config.has_ibvs_twist_filter_alpha()) { - app_config.ibvs_twist_filter_alpha = std::clamp(config.ibvs_twist_filter_alpha(), 0.0, 1.0); - } - if (config.has_align_error_threshold6()) { - Eigen::Matrix align_err; - align_err << app_config.align_error_threshold6[0], app_config.align_error_threshold6[1], app_config.align_error_threshold6[2], - app_config.align_error_threshold6[3], app_config.align_error_threshold6[4], app_config.align_error_threshold6[5]; - applyErrorThreshold6FromConfig(config.align_error_threshold6(), align_err); - for (int i = 0; i < 6; ++i) { - app_config.align_error_threshold6[static_cast(i)] = align_err[i]; - } - } else { - if (config.has_align_xy_threshold_m()) { - app_config.align_error_threshold6[0] = config.align_xy_threshold_m(); - app_config.align_error_threshold6[1] = config.align_xy_threshold_m(); - } - if (config.has_align_z_threshold_m()) { - app_config.align_error_threshold6[2] = config.align_z_threshold_m(); - } - if (config.has_align_rot_threshold_rad()) { - app_config.align_error_threshold6[3] = config.align_rot_threshold_rad(); - app_config.align_error_threshold6[4] = config.align_rot_threshold_rad(); - app_config.align_error_threshold6[5] = config.align_rot_threshold_rad(); - } - } - if (config.has_enable_joint_limit_avoidance()) { - app_config.enable_joint_limit_avoidance = config.enable_joint_limit_avoidance(); - } - if (config.has_joint_limit_avoidance_gain()) { - app_config.joint_limit_avoidance_gain = config.joint_limit_avoidance_gain(); - } - if (config.has_joint_limit_avoidance_margin_ratio()) { - app_config.joint_limit_avoidance_margin_ratio = config.joint_limit_avoidance_margin_ratio(); - } - if (config.has_joint_limit_avoidance_max_push()) { - app_config.joint_limit_avoidance_max_push = config.joint_limit_avoidance_max_push(); - } - - if (config.has_r_camera_to_visp()) { - applyMatrix3dFromConfig(config.r_camera_to_visp(), app_config.R_camera_to_visp); - } - if (config.has_r_camera_to_urdf()) { - applyMatrix3dFromConfig(config.r_camera_to_urdf(), app_config.R_camera_to_urdf); - } - - if (config.control_joint_names_size() > 0) { - app_config.control_joint_names.assign(config.control_joint_names().begin(), - config.control_joint_names().end()); - } - - if (config.has_align_xy_threshold_m()) { - app_config.align_error_threshold6[0] = config.align_xy_threshold_m(); - app_config.align_error_threshold6[1] = config.align_xy_threshold_m(); - } - if (config.has_align_z_threshold_m()) { - app_config.align_error_threshold6[2] = config.align_z_threshold_m(); - } - if (config.has_align_rot_threshold_rad()) { - app_config.align_error_threshold6[3] = config.align_rot_threshold_rad(); - app_config.align_error_threshold6[4] = config.align_rot_threshold_rad(); - app_config.align_error_threshold6[5] = config.align_rot_threshold_rad(); - } - if (config.has_align_stable_frames()) { - app_config.align_stable_frames = config.align_stable_frames(); - } - if (config.has_align_timeout_s()) { - app_config.align_timeout_s = config.align_timeout_s(); - } - if (config.has_pause_after_align_reached()) { - app_config.pause_after_align_reached = config.pause_after_align_reached(); - } - - if (config.has_touch_twist_base()) { - applyTwist6FromConfig(config.touch_twist_base(), app_config.touch_twist_base); - } - if (config.has_touch_use_speedl()) { - app_config.touch_use_speedl = config.touch_use_speedl(); - } - if (config.has_touch_speedl_acceleration()) { - app_config.touch_speedl_acceleration = config.touch_speedl_acceleration(); - } - if (config.has_touch_forward_l()) { - app_config.touch_forward_l = config.touch_forward_l(); - } - if (config.has_touch_movel_speed()) { - app_config.touch_movel_speed = config.touch_movel_speed(); - } - if (config.has_touch_movel_acceleration()) { - app_config.touch_movel_acceleration = config.touch_movel_acceleration(); - } - if (config.has_touch_movel_jerk()) { - app_config.touch_movel_jerk = config.touch_movel_jerk(); - } - if (config.touch_movel_qd_max_size() > 0) { - app_config.touch_movel_qd_max.assign(config.touch_movel_qd_max().begin(), - config.touch_movel_qd_max().end()); - } - if (config.has_touch_speedl_forward_l()) { - app_config.touch_speedl_forward_l = config.touch_speedl_forward_l(); - } - - if (config.has_dwell_time_s()) { - app_config.dwell_time_s = config.dwell_time_s(); - } - if (config.has_retract_twist_base()) { - applyTwist6FromConfig(config.retract_twist_base(), app_config.retract_twist_base); - } - if (config.has_retract_acceleration()) { - app_config.retract_acceleration = config.retract_acceleration(); - } - if (config.has_retract_duration_s()) { - app_config.retract_duration_s = config.retract_duration_s(); - } - if (config.has_tactile_finger()) { - app_config.tactile_finger = toFingerType(config.tactile_finger()); - } - if (config.has_tactile_region()) { - app_config.tactile_region = toTactileRegion(config.tactile_region()); - } - if (config.has_tactile_criterion()) { - app_config.tactile_criterion = toTactileCriterion(config.tactile_criterion()); - } - if (config.has_tactile_pressure_sum_threshold()) { - app_config.tactile_pressure_sum_threshold = config.tactile_pressure_sum_threshold(); - } - if (config.has_tactile_nonzero_count_threshold()) { - app_config.tactile_nonzero_count_threshold = - std::max(0, config.tactile_nonzero_count_threshold()); - } - - config_out = app_config; - return true; -} - -bool TouchScreenApp::applyConfig() { - if (!perception_) { - return false; - } - if (!config_.R_camera_to_visp.allFinite() || !config_.R_camera_to_urdf.allFinite()) { - return false; - } - if (!std::isfinite(config_.touch_speedl_forward_l) || - config_.touch_speedl_forward_l < 0.0) { - return false; - } - for (const double threshold_i : config_.align_error_threshold6) { - if (!std::isfinite(threshold_i) || threshold_i < 0.0) { - return false; - } - } - if (!config_.touch_use_speedl) { - Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero(); - if (!computeLinearMoveDeltaTool(config_.touch_twist_base, - config_.touch_forward_l, - touch_forward_delta)) { - return false; - } - if (!std::isfinite(config_.touch_movel_speed) || config_.touch_movel_speed <= 0.0 || - !std::isfinite(config_.touch_movel_acceleration) || - config_.touch_movel_acceleration <= 0.0 || - !std::isfinite(config_.touch_movel_jerk) || config_.touch_movel_jerk <= 0.0) { - return false; - } - if (!config_.touch_movel_qd_max.empty()) { - if (config_.touch_movel_qd_max.size() != config_.control_joint_names.size()) { - return false; - } - for (const double qd_max_i : config_.touch_movel_qd_max) { - if (!std::isfinite(qd_max_i) || qd_max_i <= 0.0) { - return false; - } - } - } - - } - - if (dexhand_) { - std::vector tactile_regions; - if (!appendRequestedTactileRegions(config_.tactile_finger, - config_.tactile_region, - tactile_regions)) { - return false; - } - try { - dexhand_->setTactilePollingRegions(tactile_regions); - } catch (...) { - return false; - } - } - - perception_->setTagSize(config_.tag_size_m); - tracker_.setTargetPointMethod(config_.target_point_method); - ibvs_.setLambda(config_.ibvs_lambda); - ibvs_.setMu(config_.ibvs_mu); - ibvs_.setQdotMax(config_.ibvs_qdot_max); - ibvs_.setVelocityLimit6(config_.ibvs_vmax6); - ibvs_.setAccelerationLimit6(config_.ibvs_amax6); - ibvs_.setTwistFilterAlpha(config_.ibvs_twist_filter_alpha); - ibvs_.setJointLimitAvoidance(config_.enable_joint_limit_avoidance, - config_.joint_limit_avoidance_gain, - config_.joint_limit_avoidance_margin_ratio, - config_.joint_limit_avoidance_max_push); - ibvs_.setAlignCameraToVisp(config_.R_camera_to_visp); - ibvs_.setAlignCameraToUrdf(config_.R_camera_to_urdf); - return true; -} - -bool TouchScreenApp::validateControlJointNames() const { - std::vector solver_joint_names; - if (!ibvs_.getChainJointNames(solver_joint_names)) { - return false; - } - if (solver_joint_names == config_.control_joint_names) { - return true; - } - - std::cerr << "[TouchScreenApp] control_joint_names mismatch with IbvsController IK chain\n"; - std::cerr << " app joints :"; - for (const auto& name : config_.control_joint_names) { - std::cerr << " " << name; - } - std::cerr << "\n solver joints:"; - for (const auto& name : solver_joint_names) { - std::cerr << " " << name; - } - std::cerr << '\n'; - return false; -} - -bool TouchScreenApp::stepAligning(const double dt) { - const auto now = Clock::now(); - const double elapsed = std::chrono::duration(now - phase_start_time_).count(); - if (elapsed > config_.align_timeout_s) { - enterFailed(Status::ALIGN_TIMEOUT); - return false; - } - const double ibvs_dt = std::clamp(dt, 0.005, 0.05); - - perception::AprilTagPerception::Options perception_options; - perception_options.depth_policy = config_.depth_policy; - perception_options.detect_tags = true; - perception_options.fetch_encoded = false; - if (!perception_->update(perception_options)) { - hardStopIbvsMotion(); - last_status_ = Status::ALIGN_WAITING_PERCEPTION; - return true; - } - - bool tracking_ok = false; - if (!target_locked_) { - tracking_ok = tracker_.startTrackingFromPixel(target_u_, target_v_); - if (tracking_ok) { - target_locked_ = true; - ibvs_target_initialized_ = false; - align_stable_count_ = 0; - } - } else { - tracking_ok = tracker_.track(); - } - - if (!tracking_ok) { - hardStopIbvsMotion(); - align_stable_count_ = 0; - last_status_ = Status::ALIGN_WAITING_TRACK; - return true; - } - - const int tag_id = tracker_.activeTagId(); - last_active_tag_id_ = tag_id; - if (tag_id < 0) { - hardStopIbvsMotion(); - align_stable_count_ = 0; - last_status_ = Status::ALIGN_WAITING_TRACK; - return true; - } - - Eigen::Vector3d p_t_target = Eigen::Vector3d::Zero(); - if (!tracker_.getAnchorInTag(tag_id, p_t_target)) { - hardStopIbvsMotion(); - align_stable_count_ = 0; - last_status_ = Status::ALIGN_WAITING_TRACK; - return true; - } - - const auto* current_tag = perception_->findTag(tag_id); - if (!current_tag) { - hardStopIbvsMotion(); - align_stable_count_ = 0; - last_status_ = Status::ALIGN_WAITING_TRACK; - return true; - } - - Eigen::Matrix3d R_target = - rotationFromTargetRotvec(config_.target_rx, config_.target_ry, config_.target_rz); - const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0); - switch (config_.align_mode) { - case AlignMode::POSE_AND_POSITION: - break; - case AlignMode::RX_RY_AND_POSITION: - if (!locked_target_rotation_valid_ || tracker_.lastSwitched()) { - double locked_yaw_rad = 0.0; - if (!extractProjectedYawAboutTargetNormal(R_target, R_current, locked_yaw_rad)) { - enterFailed(Status::ALIGN_TARGET_SETUP_FAILED); - return false; - } - const Eigen::Matrix3d Rz_locked = - Eigen::AngleAxisd(locked_yaw_rad, Eigen::Vector3d::UnitZ()).toRotationMatrix(); - locked_target_rotation_ = R_target * Rz_locked; - locked_target_rotation_valid_ = true; - } - R_target = locked_target_rotation_; - break; - case AlignMode::POSITION_ONLY: - // True position-only mode: keep the desired orientation equal to the - // current tag orientation every frame so IBVS does not actively try to - // correct rotational error. - R_target = R_current; - break; - } - const Eigen::Vector3d target_rotvec = rotvecFromRotationMatrix(R_target); - - ibvs_.setTrackedTagId(tag_id); - const bool refresh_target = !ibvs_target_initialized_ || tracker_.lastSwitched() || - config_.align_mode == AlignMode::POSITION_ONLY; - if (refresh_target) { - if (!ibvs_.setTargetFromPointInTag(p_t_target, - config_.hover_target_in_camera, - target_rotvec.x(), - target_rotvec.y(), - target_rotvec.z())) { - enterFailed(Status::ALIGN_TARGET_SETUP_FAILED); - return false; - } - ibvs_target_initialized_ = true; - } - - std::vector q_now; - if (!readControlledJointPositions(q_now)) { - enterFailed(Status::ROBOT_STATE_FAILED); - return false; - } - - std::vector qdot_cmd; - if (!ibvs_.computeQdot(q_now, ibvs_dt, qdot_cmd)) { - switch (ibvs_.lastComputeStatus()) { - case IbvsController::ComputeStatus::NO_NEW_FRAME: - case IbvsController::ComputeStatus::NO_TAG: - case IbvsController::ComputeStatus::TAG_MISMATCH: - case IbvsController::ComputeStatus::NO_DEPTH: - hardStopIbvsMotion(); - align_stable_count_ = 0; - last_status_ = Status::ALIGN_WAITING_TRACK; - return true; - case IbvsController::ComputeStatus::OK: - case IbvsController::ComputeStatus::NOT_READY: - case IbvsController::ComputeStatus::BAD_IMAGE: - case IbvsController::ComputeStatus::INVALID_INPUT: - case IbvsController::ComputeStatus::IK_FAILED: - default: - enterFailed(Status::ALIGN_COMPUTE_FAILED); - return false; - } - } - - if (!sendJointVelocity(qdot_cmd)) { - enterFailed(Status::ROBOT_COMMAND_FAILED); - return false; - } - - last_align_error_camera_ = tracker_.lastTargetInCamera() - config_.hover_target_in_camera; - const Eigen::Vector3d rot_error_vec = - rotvecFromRotationMatrix(R_target.transpose() * R_current); - bool align_ok = - std::abs(last_align_error_camera_.x()) <= config_.align_error_threshold6[0] && - std::abs(last_align_error_camera_.y()) <= config_.align_error_threshold6[1] && - std::abs(last_align_error_camera_.z()) <= config_.align_error_threshold6[2]; - switch (config_.align_mode) { - case AlignMode::POSE_AND_POSITION: - align_ok = align_ok && - std::abs(rot_error_vec.x()) <= config_.align_error_threshold6[3] && - std::abs(rot_error_vec.y()) <= config_.align_error_threshold6[4] && - std::abs(rot_error_vec.z()) <= config_.align_error_threshold6[5]; - break; - case AlignMode::RX_RY_AND_POSITION: - align_ok = align_ok && - std::abs(rot_error_vec.x()) <= config_.align_error_threshold6[3] && - std::abs(rot_error_vec.y()) <= config_.align_error_threshold6[4]; - break; - case AlignMode::POSITION_ONLY: - break; - } - if (align_ok) { - ++align_stable_count_; - } else { - align_stable_count_ = 0; - } - - if (align_stable_count_ >= config_.align_stable_frames) { - std::cout << "[TouchScreenApp][ALIGN_REACHED] tag_id=" << tag_id - << ", err_xyz=[" << last_align_error_camera_.x() << ", " - << last_align_error_camera_.y() << ", " - << last_align_error_camera_.z() << "]" - << ", err_rxyz=[" << rot_error_vec.x() << ", " - << rot_error_vec.y() << ", " - << rot_error_vec.z() << "]" - << std::endl; - hardStopIbvsMotion(); - phase_ = Phase::ALIGN_REACHED; - phase_start_time_ = Clock::now(); - touch_command_started_ = false; - last_status_ = Status::ALIGN_REACHED; - return true; - } - - last_status_ = Status::ALIGNING; - return true; -} - -bool TouchScreenApp::stepTouching() { - if (!touch_command_started_) { - if (!startTouchPhase()) { - enterFailed(last_status_ == Status::TACTILE_UNAVAILABLE || - last_status_ == Status::INVALID_CONFIG || - last_status_ == Status::ROBOT_STATE_FAILED - ? last_status_ - : Status::ROBOT_COMMAND_FAILED); - return false; - } - } - - if (!dexhand_) { - enterFailed(Status::TACTILE_UNAVAILABLE); - return false; - } - - if (isTouchTriggered(config_, last_touch_pressure_sum_)) { - if (config_.touch_use_speedl) { - logTouchingSpeedLState(); - } - if (!handleTouchTriggered(true)) { - enterFailed(Status::ROBOT_COMMAND_FAILED); - return false; - } - return true; - } - - if (!config_.touch_use_speedl) { - last_status_ = Status::TOUCHING; - return true; - } - - if (config_.touch_speedl_forward_l > 0.0) { - if (!touch_start_position_valid_) { - enterFailed(Status::ROBOT_STATE_FAILED); - return false; - } - - Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero(); - if (!readCurrentTouchPointPositionBase(current_position_base)) { - enterFailed(Status::ROBOT_STATE_FAILED); - return false; - } - - const double traveled_distance = - (current_position_base - touch_start_position_base_).norm(); - if (traveled_distance >= config_.touch_speedl_forward_l) { - logTouchingSpeedLState(); - if (!updateTouchPressure()) { - enterFailed(Status::TACTILE_UNAVAILABLE); - return false; - } - if (isTouchTriggered(config_, last_touch_pressure_sum_)) { - if (!handleTouchTriggered(true)) { - enterFailed(Status::ROBOT_COMMAND_FAILED); - return false; - } - return true; - } - if (!startRetractPhase(Phase::FAILED, Status::TOUCH_FORWARD_TIMEOUT)) { - enterFailed(Status::ROBOT_COMMAND_FAILED); - return false; - } - return true; - } - } - - last_status_ = Status::TOUCHING; - return true; -} - -bool TouchScreenApp::stepDwelling() { - const double elapsed = std::chrono::duration(Clock::now() - phase_start_time_).count(); - if (elapsed < config_.dwell_time_s) { - last_status_ = Status::TOUCH_TRIGGERED; - return true; - } - - if (!startRetractPhase(Phase::DONE, Status::DONE)) { - enterFailed(Status::ROBOT_COMMAND_FAILED); - return false; - } - return true; -} - -bool TouchScreenApp::stepRetracting() { - if (!retract_command_started_) { - if (!startRetractPhase(phase_after_retract_, final_status_after_retract_)) { - enterFailed(Status::ROBOT_COMMAND_FAILED); - return false; - } - } - - const double elapsed = std::chrono::duration(Clock::now() - phase_start_time_).count(); - if (elapsed < config_.retract_duration_s) { - last_status_ = Status::RETRACTING; - return true; - } - - try { - robot_->stopSpeedL(); - } catch (...) { - enterFailed(Status::ROBOT_COMMAND_FAILED); - return false; - } - holdCurrentControlledPosition(); - if ((phase_after_retract_ == Phase::DONE || phase_after_retract_ == Phase::FAILED) && - !moveToInitPositionIfEnabled()) { - phase_ = Phase::FAILED; - last_status_ = Status::ROBOT_COMMAND_FAILED; - return false; - } - phase_ = phase_after_retract_; - last_status_ = final_status_after_retract_; - return phase_ != Phase::FAILED; -} - -bool TouchScreenApp::readControlledJointPositions(std::vector& q_out) const { - if (!robot_) { - return false; - } - - std::vector states; - robot_->getJointsState(states); - std::unordered_map q_map; - q_map.reserve(states.size()); - for (const auto& state : states) { - q_map[state.name] = state.position; - } - - q_out.resize(config_.control_joint_names.size()); - for (size_t i = 0; i < config_.control_joint_names.size(); ++i) { - const auto it = q_map.find(config_.control_joint_names[i]); - if (it == q_map.end()) { - return false; - } - q_out[i] = it->second; - } - return true; -} - -bool TouchScreenApp::sendJointVelocity(const std::vector& qdot) const { - if (!robot_ || qdot.size() != config_.control_joint_names.size()) { - return false; - } - - std::vector cmd; - cmd.reserve(qdot.size()); - for (size_t i = 0; i < qdot.size(); ++i) { - cmd.push_back({config_.control_joint_names[i], qdot[i]}); - } - - try { - robot_->speedJ(cmd); - } catch (...) { - return false; - } - return true; -} - -bool TouchScreenApp::sendZeroJointVelocity() const { - std::vector zero(config_.control_joint_names.size(), 0.0); - return sendJointVelocity(zero); -} - -void TouchScreenApp::hardStopIbvsMotion() { - sendZeroJointVelocity(); - ibvs_.resetTwistCommandState(); -} - -bool TouchScreenApp::readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const { - if (!robot_) { - return false; - } - - try { - const auto pose = robot_->fk(true); - p_out << pose.position().x(), pose.position().y(), pose.position().z(); - return p_out.allFinite(); - } catch (...) { - return false; - } -} - -void TouchScreenApp::logTouchingSpeedLState() const { - if (!robot_ || !config_.touch_use_speedl || phase_ != Phase::TOUCHING) { - return; - } - - const Eigen::Matrix cmd_twist_base = robot_->getSpeedLCommandTwistBase(); - Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero(); - const bool have_current_position = readCurrentTouchPointPositionBase(current_position_base); - const bool have_delta = touch_start_position_valid_ && have_current_position; - Eigen::Vector3d cumulative_delta_base = Eigen::Vector3d::Zero(); - if (have_delta) { - cumulative_delta_base = current_position_base - touch_start_position_base_; - } - - std::cout << "[TouchScreenApp][TOUCHING] speedl_cmd_base=[" - << cmd_twist_base[0] << ", " - << cmd_twist_base[1] << ", " - << cmd_twist_base[2] << ", " - << cmd_twist_base[3] << ", " - << cmd_twist_base[4] << ", " - << cmd_twist_base[5] << "]"; - if (have_delta) { - std::cout << ", cum_tcp_delta_base=[" - << cumulative_delta_base.x() << ", " - << cumulative_delta_base.y() << ", " - << cumulative_delta_base.z() << "]" - << ", cum_tcp_dist=" << cumulative_delta_base.norm(); - } else { - std::cout << ", cum_tcp_delta_base=[unavailable]"; - } - std::cout << std::endl; -} - -bool TouchScreenApp::holdCurrentControlledPosition() const { - if (!robot_) { - return false; - } - - std::vector states; - robot_->getJointsState(states); - std::unordered_map q_map; - q_map.reserve(states.size()); - for (const auto& state : states) { - q_map[state.name] = state.position; - } - - std::vector joints; - joints.reserve(config_.control_joint_names.size()); - for (const auto& name : config_.control_joint_names) { - const auto it = q_map.find(name); - if (it == q_map.end()) { - return false; - } - joints.emplace_back(name, it->second, 0.0); - } - - try { - robot_->servoJ(joints, 0.02); - } catch (...) { - return false; - } - return true; -} - -bool TouchScreenApp::moveToInitPositionIfEnabled() const { - if (!config_.move_to_init_position) { - return true; - } - if (!robot_ || config_.init_joint_positions.empty()) { - return false; - } - - try { - auto init_cmd = config_.init_joint_positions; - robot_->moveJ(init_cmd, config_.init_movej_vel, config_.init_movej_acc); - } catch (...) { - return false; - } - return true; -} - -bool TouchScreenApp::handleTouchTriggered(const bool stop_forward_motion) { - if (config_.dwell_time_s < 0.0) { - if (!startRetractPhase(Phase::DONE, Status::DONE)) { - return false; - } - if (phase_ == Phase::RETRACTING) { - last_status_ = Status::TOUCH_TRIGGERED; - } - return true; - } - - if (stop_forward_motion) { - try { - robot_->stopSpeedL(); - } catch (...) { - return false; - } - } - - phase_ = Phase::DWELLING; - phase_start_time_ = Clock::now(); - last_status_ = Status::TOUCH_TRIGGERED; - return true; -} - -bool TouchScreenApp::startTouchPhase() { - if (!robot_) { - last_status_ = Status::ROBOT_COMMAND_FAILED; - return false; - } - - phase_ = Phase::TOUCHING; - phase_start_time_ = Clock::now(); - touch_command_started_ = true; - retract_command_started_ = false; - last_status_ = Status::TOUCHING; - - if (config_.touch_use_speedl) { - touch_start_position_valid_ = readCurrentTouchPointPositionBase(touch_start_position_base_); - if (config_.touch_speedl_forward_l > 0.0 && !touch_start_position_valid_) { - std::cerr << "[TouchScreenApp] startTouchPhase failed: cannot read touch start pose " - << "for speedL distance-based touching" << std::endl; - last_status_ = Status::ROBOT_STATE_FAILED; - return false; - } - try { - if (!robot_->speedL(toStdVector6(config_.touch_twist_base), - config_.touch_speedl_acceleration, - 0.0, - cmvr::CartesianFrame::Tool)) { - last_status_ = Status::ROBOT_COMMAND_FAILED; - return false; - } - } catch (...) { - last_status_ = Status::ROBOT_COMMAND_FAILED; - return false; - } - return true; - } - - Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero(); - if (!computeLinearMoveDeltaTool(config_.touch_twist_base, - config_.touch_forward_l, - touch_forward_delta)) { - last_status_ = Status::INVALID_CONFIG; - return false; - } - - const std::vector pose_cmd = { - touch_forward_delta.x(), - touch_forward_delta.y(), - touch_forward_delta.z(), - 0.0, - 0.0, - 0.0 - }; - - try { - if (!robot_->moveL(pose_cmd, - config_.touch_movel_speed, - config_.touch_movel_acceleration, - config_.touch_movel_jerk, - config_.touch_movel_qd_max, - false)) { - last_status_ = Status::ROBOT_COMMAND_FAILED; - return false; - } - } catch (...) { - last_status_ = Status::ROBOT_COMMAND_FAILED; - return false; - } - - if (!moveToInitPositionIfEnabled()) { - last_status_ = Status::ROBOT_COMMAND_FAILED; - return false; - } - - phase_ = Phase::DONE; - touch_command_started_ = false; - retract_command_started_ = false; - last_status_ = Status::DONE; - return true; -} - -bool TouchScreenApp::startRetractPhase(const Phase next_phase_after_retract, - const Status final_status_after_retract) { - if (!robot_) { - return false; - } - - try { - if (!robot_->speedL(toStdVector6(config_.retract_twist_base), - config_.retract_acceleration, - 0.0, - cmvr::CartesianFrame::Tool)) { - return false; - } - } catch (...) { - return false; - } - - phase_ = Phase::RETRACTING; - phase_after_retract_ = next_phase_after_retract; - final_status_after_retract_ = final_status_after_retract; - phase_start_time_ = Clock::now(); - retract_command_started_ = true; - last_status_ = Status::RETRACTING; - return true; -} - -void TouchScreenApp::enterFailed(const Status status) { - try { - if (robot_) { - robot_->stopSpeedL(); - } - } catch (...) { - } - hardStopIbvsMotion(); - holdCurrentControlledPosition(); - phase_ = Phase::FAILED; - touch_command_started_ = false; - retract_command_started_ = false; - last_status_ = moveToInitPositionIfEnabled() ? status : Status::ROBOT_COMMAND_FAILED; -} - -bool TouchScreenApp::updateTouchPressure() { - last_touch_pressure_sum_ = 0.0; - last_touch_nonzero_count_ = 0; - if (!dexhand_) { - return false; - } - - std::vector tactile_regions; - if (!appendRequestedTactileRegions(config_.tactile_finger, - config_.tactile_region, - tactile_regions)) { - return false; - } - - double resultant_value = 0.0; - double resultant_fz = 0.0; - try { - for (const auto& tactile_region : tactile_regions) { - const auto resultant_force = dexhand_->getResultantForce(tactile_region.first, tactile_region.second); - resultant_value += tactileForceValue(resultant_force, config_.tactile_criterion); - resultant_fz += static_cast(resultant_force.fz); - } - } catch (...) { - return false; - } - - last_touch_nonzero_count_ = std::abs(resultant_value) > 1e-9 ? 1 : 0; - last_touch_pressure_sum_ = resultant_value; - if (phase_ == Phase::TOUCHING) { - std::cout << "[TouchScreenApp][TOUCHING][TACTILE] fz=" << resultant_fz - << ", criterion_value=" << resultant_value - << ", threshold=" << config_.tactile_pressure_sum_threshold - << std::endl; - } - return true; -} - -} // namespace cmvr::app diff --git a/cmvr-es/applications/src/touch_screen_app_test.cpp b/cmvr-es/applications/src/touch_screen_app_test.cpp deleted file mode 100644 index b2c178fe..00000000 --- a/cmvr-es/applications/src/touch_screen_app_test.cpp +++ /dev/null @@ -1,105 +0,0 @@ -#include "gtest/gtest.h" - -#include -#include -#include -#include - -#include "applications/include/touch_screen_app.h" - -#include "include/device_manager.h" -#include "service/grpc/include/server_runner.h" -namespace { - -constexpr const char* kConfigPath = - "/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"; -constexpr int kTargetU = 1280 / 2.0; -constexpr int kTargetV = 720 / 2.0; - - -void run_touch_once(int u, int v) { - - const XmlNode config(kConfigPath); - // auto& dm = cmvr::device::DeviceManager::getInstance(); - cmvr::device::DeviceManager::getInstance(config.getChild("DeviceManager")); - - - // cmvr::service::ServerRunner runner; - // runner.start(config); - - ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; - - cmvr::app::TouchScreenApp app; - ASSERT_TRUE(app.init()) - << "TouchScreenApp init failed"; - - ASSERT_TRUE(app.startFromPixel(u, v)) << "startFromPixel failed"; - - bool align_reached = false; - bool touch_triggered = false; - auto last_step_time = std::chrono::steady_clock::now(); - bool first_step = true; - while (app.isBusy()) { - const auto now = std::chrono::steady_clock::now(); - double dt = 0.02; - if (!first_step) { - dt = std::chrono::duration(now - last_step_time).count(); - dt = std::clamp(dt, 0.005, 0.05); - } - last_step_time = now; - first_step = false; - - const bool step_ok = app.step(dt); - const auto& p_c_target = app.tracker().lastTargetInCamera(); - std::cout << "phase=" << cmvr::app::TouchScreenApp::phaseToString(app.phase()) - << ", status=" << cmvr::app::TouchScreenApp::statusToString(app.lastStatus()) - << ", active_tag=" << app.lastActiveTagId() - << ", target_c=[" << p_c_target.x() << ", " - << p_c_target.y() << ", " - << p_c_target.z() << "]" - << ", nonzero_count=" << app.lastTouchNonzeroCount() - << ", pressure_sum=" << app.lastTouchPressureSum() - << ", err_c=[" << app.lastAlignErrorCamera().x() << ", " - << app.lastAlignErrorCamera().y() << ", " - << app.lastAlignErrorCamera().z() << "]\n"; - - if (app.lastStatus() == cmvr::app::TouchScreenApp::Status::ALIGN_REACHED) { - std::cout << "align reached, target_c=[" << p_c_target.x() << ", " - << p_c_target.y() << ", " - << p_c_target.z() << "]\n"; - align_reached = true; - } - if (app.lastStatus() == cmvr::app::TouchScreenApp::Status::TOUCH_TRIGGERED) { - std::cout << "touch triggered, nonzero_count=" << app.lastTouchNonzeroCount() - << ", pressure_sum=" << app.lastTouchPressureSum() << "\n"; - touch_triggered = true; - } - if (!step_ok) { - const auto failed_status = app.lastStatus(); - app.stop(); - FAIL() << "touch flow failed, status=" - << cmvr::app::TouchScreenApp::statusToString(failed_status); - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - - const auto final_status = app.lastStatus(); - ASSERT_TRUE(align_reached) - << "align was never reached, final status=" - << cmvr::app::TouchScreenApp::statusToString(final_status); - ASSERT_TRUE(touch_triggered) - << "touch was never triggered, final status=" - << cmvr::app::TouchScreenApp::statusToString(final_status); - ASSERT_TRUE(app.isFinished()) - << "touch did not finish successfully, final status=" - << cmvr::app::TouchScreenApp::statusToString(final_status); - std::cout << "touch done, final status=" - << cmvr::app::TouchScreenApp::statusToString(final_status) << "\n"; - app.stop(); -} - -} // namespace - -TEST(TouchScreenAppTest, RunTouchOnceOnRealRobot) { - run_touch_once(kTargetU, kTargetV); -} diff --git a/cmvr-es/common/CMakeLists.txt b/cmvr-es/common/CMakeLists.txt index a60df3c2..3bedb230 100644 --- a/cmvr-es/common/CMakeLists.txt +++ b/cmvr-es/common/CMakeLists.txt @@ -1,19 +1,12 @@ #find_package(protobuf REQUIRED) - -file(GLOB SRC - ${CMAKE_CURRENT_SOURCE_DIR}/utils/config_helper/src/config_setting.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/CameraCapture.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/RealSenseCapture.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/VideoFrameEncoder.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/VideoWriter.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/curve/src/s_curve.cpp +add_library(common SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/media/ffmpeg/camera_capture.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/media/ffmpeg/realsense_capture.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/media/ffmpeg/video_frame_encoder.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/media/ffmpeg/video_writer.cpp ) - -add_library(common SHARED ${SRC}) - - target_include_directories(common PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(common PUBLIC @@ -52,17 +45,3 @@ install(TARGETS common LIBRARY DESTINATION lib) # pthread #) # -#add_executable(s_curve_test -# curve/src/s_curve_test.cpp -#) -# -#target_link_libraries(s_curve_test -# PRIVATE -# cmvr_es::common -# cmvr_es::proto -# glog -# gtest -# gtest_main -# pthread -# Matplot++::matplot -#) diff --git a/cmvr-es/common/consts/constant.h b/cmvr-es/common/base/constants.h similarity index 100% rename from cmvr-es/common/consts/constant.h rename to cmvr-es/common/base/constants.h diff --git a/cmvr-es/utils/base/include/grpc_utils.h b/cmvr-es/common/base/grpc_utils.h similarity index 91% rename from cmvr-es/utils/base/include/grpc_utils.h rename to cmvr-es/common/base/grpc_utils.h index a70ca4bf..0d702451 100644 --- a/cmvr-es/utils/base/include/grpc_utils.h +++ b/cmvr-es/common/base/grpc_utils.h @@ -5,6 +5,8 @@ #ifndef GRPC_UTILS_H #define GRPC_UTILS_H +#include + #include inline void setCurrentTimestamp(google::protobuf::Timestamp* ts) { @@ -14,4 +16,4 @@ inline void setCurrentTimestamp(google::protobuf::Timestamp* ts) { ts->set_nanos(std::chrono::duration_cast(duration).count() % 1000000000); } -#endif //GRPC_UTILS_H \ No newline at end of file +#endif //GRPC_UTILS_H diff --git a/cmvr-es/common/base/logging/CMakeLists.txt b/cmvr-es/common/base/logging/CMakeLists.txt new file mode 100644 index 00000000..c6c31e5c --- /dev/null +++ b/cmvr-es/common/base/logging/CMakeLists.txt @@ -0,0 +1,15 @@ +add_library(logging STATIC + logger.cpp +) + +target_include_directories(logging PUBLIC + ${PROJECT_SOURCE_DIR}/cmvr-es +) + +target_link_libraries(logging PUBLIC + cmvr_es::proto +) + +add_library(cmvr_es::logging ALIAS logging) + +install(TARGETS logging ARCHIVE DESTINATION lib) diff --git a/cmvr-es/common/base/logging/logger.cpp b/cmvr-es/common/base/logging/logger.cpp new file mode 100644 index 00000000..0a6355ee --- /dev/null +++ b/cmvr-es/common/base/logging/logger.cpp @@ -0,0 +1,365 @@ +#include "common/base/logging/logger.h" + +#include +#include +#include +#include +#include +#include + +namespace cmvr::logging { + +namespace { + +std::string formatTimestamp() +{ + const auto now = std::chrono::system_clock::now(); + const auto time = std::chrono::system_clock::to_time_t(now); + const auto milliseconds = std::chrono::duration_cast( + now.time_since_epoch()) % 1000; + + std::tm local_time{}; + localtime_r(&time, &local_time); + + std::ostringstream output; + output << std::put_time(&local_time, "%Y-%m-%d %H:%M:%S") + << '.' << std::setfill('0') << std::setw(3) << milliseconds.count(); + return output.str(); +} + +const char* baseName(const char* path) +{ + if (path == nullptr) { + return "unknown"; + } + const char* result = path; + for (const char* current = path; *current != '\0'; ++current) { + if (*current == '/' || *current == '\\') { + result = current + 1; + } + } + return result; +} + +const char* terminalColor(const Level level) +{ + switch (level) { + case Level::WARNING: + return "\033[33m"; + case Level::ERROR: + case Level::FATAL: + return "\033[31m"; + case Level::DEBUG: + case Level::INFO: + case Level::COUNT: + break; + } + return ""; +} + +} // namespace + +Logger& Logger::instance() +{ + static Logger logger; + return logger; +} + +Logger::Logger() +{ + routes_[index(Level::INFO)].terminal = true; + routes_[index(Level::WARNING)].terminal = true; + routes_[index(Level::ERROR)].terminal = true; + routes_[index(Level::FATAL)].terminal = true; +} + +bool Logger::initialize(const config::LoggerConfig& config, + const std::string& application_name, + const std::filesystem::path& executable_directory) +{ + std::lock_guard lock(mutex_); + + std::array(Level::COUNT)> new_routes{}; + std::array(Level::COUNT)> configured{}; + bool any_file_route = false; + Level new_minimum_level; + + if (!convertLevel(config.minimum_level(), new_minimum_level)) { + std::cerr << "Invalid minimum logging level" << std::endl; + return false; + } + + for (const auto& route_config : config.routes()) { + Level level; + if (!convertLevel(route_config.level(), level)) { + std::cerr << "Invalid logging level in route" << std::endl; + return false; + } + const auto route_index = index(level); + if (configured[route_index]) { + std::cerr << "Duplicate logging route for " << levelName(level) << std::endl; + return false; + } + configured[route_index] = true; + new_routes[route_index] = {route_config.terminal(), route_config.file()}; + any_file_route = any_file_route || route_config.file(); + } + + if (config.routes().empty()) { + std::cerr << "No logging routes configured" << std::endl; + return false; + } + + std::ofstream new_log_file; + std::filesystem::path new_log_path; + if (any_file_route) { + std::filesystem::path directory = config.directory().empty() + ? std::filesystem::path("../log") + : std::filesystem::path(config.directory()); + if (directory.is_relative()) { + directory = executable_directory / directory; + } + directory = directory.lexically_normal(); + + std::error_code error; + std::filesystem::create_directories(directory, error); + if (error) { + std::cerr << "Failed to create log directory: " << directory + << ": " << error.message() << std::endl; + return false; + } + + const std::string file_name = application_name.empty() ? "cmvr_es.log" : application_name + ".log"; + new_log_path = directory / file_name; + new_log_file.open(new_log_path, std::ios::out | std::ios::app); + if (!new_log_file.is_open()) { + std::cerr << "Failed to open log file: " << new_log_path << std::endl; + return false; + } + } + + if (log_file_.is_open()) { + log_file_.flush(); + log_file_.close(); + } + + routes_ = new_routes; + format_ = parseFormat_(config); + minimum_level_ = new_minimum_level; + log_file_path_ = std::move(new_log_path); + log_file_ = std::move(new_log_file); + max_file_size_bytes_ = static_cast( + config.max_file_size_mb() > 0 ? config.max_file_size_mb() : 100) * 1024U * 1024U; + flush_interval_ = std::chrono::seconds( + config.flush_interval_seconds() > 0 ? config.flush_interval_seconds() : 1); + last_flush_ = std::chrono::steady_clock::now(); + initialized_ = true; + return true; +} + +void Logger::shutdown() +{ + std::lock_guard lock(mutex_); + if (log_file_.is_open()) { + log_file_.flush(); + log_file_.close(); + } + initialized_ = false; +} + +bool Logger::enabled(const Level level) const +{ + std::lock_guard lock(mutex_); + if (level != Level::FATAL && level < minimum_level_) { + return false; + } + const auto& route = routes_[index(level)]; + return route.terminal || route.file || level == Level::FATAL; +} + +void Logger::write(const Level level, + const char* source_file, + const int source_line, + const std::string& message) +{ + std::lock_guard lock(mutex_); + if (level != Level::FATAL && level < minimum_level_) { + return; + } + const Route route = routes_[index(level)]; + if (!route.terminal && !route.file && level != Level::FATAL) { + return; + } + + const std::string line = formatLine_(level, source_file, source_line, message); + + if (route.terminal || level == Level::FATAL) { + writeTerminal_(level, line); + } + if (route.file && log_file_.is_open()) { + rotateIfNeeded_(); + log_file_ << line << '\n'; + const auto now = std::chrono::steady_clock::now(); + if (level == Level::ERROR || level == Level::FATAL || now - last_flush_ >= flush_interval_) { + log_file_.flush(); + last_flush_ = now; + } + } +} + +std::size_t Logger::index(const Level level) +{ + return static_cast(level); +} + +const char* Logger::levelName(const Level level) +{ + switch (level) { + case Level::DEBUG: return "DEBUG"; + case Level::INFO: return "INFO"; + case Level::WARNING: return "WARNING"; + case Level::ERROR: return "ERROR"; + case Level::FATAL: return "FATAL"; + case Level::COUNT: break; + } + return "UNKNOWN"; +} + +bool Logger::convertLevel(const config::LogLevel input, Level& output) +{ + switch (input) { + case config::LOG_LEVEL_DEBUG: output = Level::DEBUG; return true; + case config::LOG_LEVEL_INFO: output = Level::INFO; return true; + case config::LOG_LEVEL_WARNING: output = Level::WARNING; return true; + case config::LOG_LEVEL_ERROR: output = Level::ERROR; return true; + case config::LOG_LEVEL_FATAL: output = Level::FATAL; return true; + case config::LOG_LEVEL_UNSPECIFIED: break; + } + return false; +} + +Format Logger::parseFormat_(const config::LoggerConfig& config) +{ + Format format; + if (!config.has_format()) { + return format; + } + + const auto& format_config = config.format(); + if (format_config.has_show_time()) { + format.show_time = format_config.show_time(); + } + if (format_config.has_show_level()) { + format.show_level = format_config.show_level(); + } + if (format_config.has_show_thread_id()) { + format.show_thread_id = format_config.show_thread_id(); + } + if (format_config.has_show_source_location()) { + format.show_source_location = format_config.show_source_location(); + } + return format; +} + +std::string Logger::formatLine_(const Level level, + const char* source_file, + const int source_line, + const std::string& message) const +{ + std::ostringstream formatted; + bool has_prefix = false; + + const auto append_prefix_part = [&formatted, &has_prefix](const std::string& part) { + if (has_prefix) { + formatted << ' '; + } + formatted << part; + has_prefix = true; + }; + + if (format_.show_time) { + append_prefix_part(formatTimestamp()); + } + if (format_.show_level) { + append_prefix_part(std::string("[") + levelName(level) + "]"); + } + if (format_.show_thread_id) { + std::ostringstream thread_id; + thread_id << '[' << std::this_thread::get_id() << ']'; + append_prefix_part(thread_id.str()); + } + if (format_.show_source_location) { + std::ostringstream source_location; + source_location << '[' << baseName(source_file) << ':' << source_line << ']'; + append_prefix_part(source_location.str()); + } + + if (has_prefix) { + formatted << ' '; + } + formatted << message; + return formatted.str(); +} + +void Logger::rotateIfNeeded_() +{ + if (log_file_path_.empty() || !log_file_.is_open()) { + return; + } + + std::error_code error; + const auto size = std::filesystem::file_size(log_file_path_, error); + if (error || size < max_file_size_bytes_) { + return; + } + + log_file_.flush(); + log_file_.close(); + + const auto backup_path = log_file_path_.string() + ".1"; + std::filesystem::remove(backup_path, error); + error.clear(); + std::filesystem::rename(log_file_path_, backup_path, error); + if (error) { + std::cerr << "Failed to rotate log file: " << error.message() << std::endl; + } + log_file_.open(log_file_path_, std::ios::out | std::ios::trunc); +} + +void Logger::writeTerminal_(const Level level, const std::string& line) +{ + const char* color = terminalColor(level); + if (color[0] == '\0') { + std::cout << line << std::endl; + return; + } + std::cout << color << line << "\033[0m" << std::endl; +} + +LogMessage::LogMessage(const Level level, const char* source_file, const int source_line) + : level_(level), source_file_(source_file), source_line_(source_line) +{ +} + +LogMessage::~LogMessage() +{ + Logger::instance().write(level_, source_file_, source_line_, stream_.str()); + if (level_ == Level::FATAL) { + Logger::instance().shutdown(); + std::abort(); + } +} + +bool initLogging(const config::LoggerConfig& config, + const std::string& application_name, + const std::filesystem::path& executable_directory) +{ + return Logger::instance().initialize(config, application_name, executable_directory); +} + +void shutdownLogging() +{ + Logger::instance().shutdown(); +} + +} // namespace cmvr::logging diff --git a/cmvr-es/common/base/logging/logger.h b/cmvr-es/common/base/logging/logger.h new file mode 100644 index 00000000..a7ee3a74 --- /dev/null +++ b/cmvr-es/common/base/logging/logger.h @@ -0,0 +1,116 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cmvr/config/logger_config/logger_config.pb.h" + +namespace cmvr::logging { + +enum class Level { + DEBUG = 0, + INFO, + WARNING, + ERROR, + FATAL, + COUNT +}; + +struct Route { + bool terminal{false}; + bool file{false}; +}; + +struct Format { + bool show_time{true}; + bool show_level{true}; + bool show_thread_id{true}; + bool show_source_location{true}; +}; + +class Logger { +public: + static Logger& instance(); + + bool initialize(const config::LoggerConfig& config, + const std::string& application_name, + const std::filesystem::path& executable_directory); + void shutdown(); + bool enabled(Level level) const; + void write(Level level, const char* source_file, int source_line, const std::string& message); + +private: + Logger(); + + static std::size_t index(Level level); + static const char* levelName(Level level); + static bool convertLevel(config::LogLevel input, Level& output); + static Format parseFormat_(const config::LoggerConfig& config); + + std::string formatLine_(Level level, const char* source_file, int source_line, const std::string& message) const; + void rotateIfNeeded_(); + void writeTerminal_(Level level, const std::string& line); + + mutable std::mutex mutex_; + std::array(Level::COUNT)> routes_{}; + Format format_{}; + Level minimum_level_{Level::INFO}; + std::filesystem::path log_file_path_; + std::ofstream log_file_; + std::uintmax_t max_file_size_bytes_{100U * 1024U * 1024U}; + std::chrono::seconds flush_interval_{1}; + std::chrono::steady_clock::time_point last_flush_{}; + bool initialized_{false}; +}; + +class LogMessage { +public: + LogMessage(Level level, const char* source_file, int source_line); + ~LogMessage(); + + std::ostream& stream() { return stream_; } + +private: + Level level_; + const char* source_file_; + int source_line_; + std::ostringstream stream_; +}; + +class LogMessageVoidify { +public: + void operator&(std::ostream&) const {} +}; + +bool initLogging(const config::LoggerConfig& config, + const std::string& application_name, + const std::filesystem::path& executable_directory); +void shutdownLogging(); + +} // namespace cmvr::logging + +#define CMVR_LOG(level) \ + !::cmvr::logging::Logger::instance().enabled(::cmvr::logging::Level::level) \ + ? static_cast(0) \ + : ::cmvr::logging::LogMessageVoidify() & \ + ::cmvr::logging::LogMessage( \ + ::cmvr::logging::Level::level, __FILE__, __LINE__).stream() + +#define CMVR_LOG_EVERY_N(level, n) \ + if (![]() { \ + static std::atomic counter{0}; \ + return counter.fetch_add(1, std::memory_order_relaxed) % (n) == 0; \ + }()) {} else CMVR_LOG(level) + +#define CMVR_LOG_IF_EVERY_N(level, condition, n) \ + if (!(condition) || ![]() { \ + static std::atomic counter{0}; \ + return counter.fetch_add(1, std::memory_order_relaxed) % (n) == 0; \ + }()) {} else CMVR_LOG(level) diff --git a/cmvr-es/utils/base/include/ring_buffer.h b/cmvr-es/common/base/ring_buffer.h similarity index 100% rename from cmvr-es/utils/base/include/ring_buffer.h rename to cmvr-es/common/base/ring_buffer.h diff --git a/cmvr-es/common/config/cabin_robot.xml b/cmvr-es/common/config/cabin_robot.xml deleted file mode 100644 index d67dffac..00000000 --- a/cmvr-es/common/config/cabin_robot.xml +++ /dev/null @@ -1,152 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/cmvr-es/common/config/camera_config/camera_config.pb.txt b/cmvr-es/common/config/camera_config/camera_config.pb.txt deleted file mode 100644 index 18b02c30..00000000 --- a/cmvr-es/common/config/camera_config/camera_config.pb.txt +++ /dev/null @@ -1,74 +0,0 @@ -realsense_cameras { - id: "cam1" - serialNumber: "243122074587" - width: 640 - height: 480 - fps: 30 - codec: "H265" - camera_mode: CAMERA_MODE_VIDEO - stream_mode: STREAM_MODE_RGBD - align_mode: ALIGN_MODE_COLOR - buffer_size: 30 - sync: false - enable: false -} - -realsense_cameras { - id: "right_hand_cam" - serialNumber: "243122072252" - width: 1280 - height: 720 - encode_width: 640 - encode_height: 360 - fps: 30 - codec: "H264" - camera_mode: CAMERA_MODE_VIDEO - stream_mode: STREAM_MODE_RGB - align_mode: ALIGN_MODE_COLOR - buffer_size: 30 - sync: false - enable: true -} - -realsense_cameras { - id: "cam3" - serialNumber: "243122075614" - width: 640 - height: 480 - fps: 30 - codec: "H265" - camera_mode: CAMERA_MODE_VIDEO - stream_mode: STREAM_MODE_RGBD - align_mode: ALIGN_MODE_COLOR - buffer_size: 30 - sync: false - enable: false -} - -uvc_cameras { - id: "left_eye_cam" - usb: "/dev/uvc_left_camera" - width: 640 - height: 480 - fps: 30 - codec: "H265" - camera_mode: CAMERA_MODE_VIDEO - stream_mode: STREAM_MODE_RGB - buffer_size: 30 - enable: false -} - -realsense_cameras { - id: "cam5" - serialNumber: "243122075389" - width: 640 - height: 480 - fps: 30 - codec: "H265" - camera_mode: CAMERA_MODE_VIDEO - stream_mode: STREAM_MODE_RGBD - align_mode: ALIGN_MODE_COLOR - buffer_size: 30 - sync: false - enable: false -} diff --git a/cmvr-es/common/config/config_files.h b/cmvr-es/common/config/config_files.h new file mode 100644 index 00000000..8b4af4da --- /dev/null +++ b/cmvr-es/common/config/config_files.h @@ -0,0 +1,152 @@ +#pragma once + +#include +#include +#include + +#include "common/base/logging/logger.h" +#include "common/io/proto_file_io.h" + +namespace cmvr { + +class ConfigHelper +{ +public: + static void setConfigRootFromFile(const std::string& file_name) + { + if (file_name.empty()) { + return; + } + + const auto parent = std::filesystem::path(file_name).lexically_normal().parent_path(); + if (!parent.empty()) { + configRoot() = parent; + } + } + + static std::string resolveConfigFile(const std::string& file_name) + { + if (file_name.empty()) { + return {}; + } + + const std::filesystem::path path(file_name); + if (path.is_absolute()) { + return path.lexically_normal().string(); + } + + const auto& root = configRoot(); + if (!root.empty()) { + return (root / path).lexically_normal().string(); + } + return path.lexically_normal().string(); + } + + static std::string resolveResourceFile(const std::string& file_name) + { + if (file_name.empty()) { + return {}; + } + + const std::filesystem::path path(file_name); + if (path.is_absolute()) { + return path.lexically_normal().string(); + } + + const auto& root = configRoot(); + if (root.empty()) { + return path.lexically_normal().string(); + } + + const std::filesystem::path candidates[] = { + root / path, + root.parent_path() / path, + root.parent_path().parent_path() / path, + root.parent_path().parent_path().parent_path() / path, + }; + for (const auto& candidate : candidates) { + const auto normalized = candidate.lexically_normal(); + if (std::filesystem::exists(normalized)) { + return normalized.string(); + } + } + return (root / path).lexically_normal().string(); + } + + template + static bool loadConfigFile(const std::string& file_name, T& message) + { + return getConfig(resolveConfigFile(file_name), message, true); + } + + template + static bool loadConfigFileSilent(const std::string& file_name, T& message) + { + return getConfig(resolveConfigFile(file_name), message, false); + } + + template + static bool saveConfigFile(const std::string& file_name, const T& message) + { + return setConfig(message, resolveConfigFile(file_name)); + } + +private: + static std::filesystem::path& configRoot() + { + static std::filesystem::path root; + return root; + } + + template + static bool setConfig(const T& message, const std::string& file_name) + { + CMVR_LOG(INFO) << "file_name = " << file_name; + if (file_name.empty()) { + CMVR_LOG(ERROR) << "Empty file name."; + return false; + } + + const bool ok = ProtoMessageIo::setProtoToAsciiFile(message, file_name); + if (!ok) { + CMVR_LOG(ERROR) << "Failed to write ASCII proto config to: " << file_name; + return false; + } + return true; + } + + template + static bool getConfig(const std::string& file_name, T& message, const bool log_success) + { + if (file_name.empty()) { + CMVR_LOG(ERROR) << "Empty file name."; + return false; + } + + if (log_success) { + CMVR_LOG(INFO) << "[ConfigHelper] Load config file: " << file_name; + } + const bool ok = ProtoMessageIo::getProtoFromAsciiFile(file_name, &message); + if (!ok) { + CMVR_LOG(ERROR) << "Failed to load ASCII proto config from: " << file_name; + return false; + } + return true; + } +}; + +namespace common::config { + +inline double positiveOr(const double value, const double fallback) +{ + return std::isfinite(value) && value > 0.0 ? value : fallback; +} + +inline int positiveIntOr(const int value, const int fallback) +{ + return value > 0 ? value : fallback; +} + +} // namespace common::config + +} // namespace cmvr diff --git a/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt b/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt deleted file mode 100644 index e6783dd2..00000000 --- a/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt +++ /dev/null @@ -1,36 +0,0 @@ -rh56dftp_dexhands { - id: "hand1" - ip: "192.168.1.213" - port: 6000 - poll_interval_ms: 10 - enable: false -} - -rh56dftp_dexhands { - id: "hand2" - ip: "192.168.1.224" - port: 6000 - poll_interval_ms: 10 - enable: true -} - -px_6ax_gen3 { - id: "paxini_tip_1" - serial_port: "/dev/ttyACM1" - sensor_model: "S1813_core" - module_id: 2 - baud_rate: 921600 - distributed_length: 153 - resultant_length: 3 - poll_interval_ms: 5 - response_timeout_ms: 200 - response_header_bytes: 14 - tactile_rows: 1 - tactile_cols: 51 - tactile_finger: "INDEX" - tactile_region: "TIP" - sensor_name: "Paxini Gen3末端压力" - polling_read_mode: PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE - auto_calibrate: false - enable: true -} diff --git a/cmvr-es/common/config/ik_solver_config/lawba_ik_solver_config.txt b/cmvr-es/common/config/ik_solver_config/lawba_ik_solver_config.txt deleted file mode 100644 index e69de29b..00000000 diff --git a/cmvr-es/common/config/ik_solver_config/pinocchio_dls_ik_solver_config.pb.txt b/cmvr-es/common/config/ik_solver_config/pinocchio_dls_ik_solver_config.pb.txt deleted file mode 100644 index af9da22b..00000000 --- a/cmvr-es/common/config/ik_solver_config/pinocchio_dls_ik_solver_config.pb.txt +++ /dev/null @@ -1,9 +0,0 @@ -urdf_path: "/home/lgv/cmvr/dual_arm.urdf" -base_frame_name: "PELVIS_S" -flange_frame_name: "R_FLANGE" -tcp_frame_name: "R_TCP" - -max_iters: 100 -pos_eps: 1e-6 -rot_eps: 1e-6 -damping: 1e-6 diff --git a/cmvr-es/common/config/ik_solver_config/pinocchio_qp_ik_solver_config.pb.txt b/cmvr-es/common/config/ik_solver_config/pinocchio_qp_ik_solver_config.pb.txt deleted file mode 100644 index 66096608..00000000 --- a/cmvr-es/common/config/ik_solver_config/pinocchio_qp_ik_solver_config.pb.txt +++ /dev/null @@ -1,10 +0,0 @@ -urdf_path: "/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf" -base_frame_name: "PELVIS_S" -flange_frame_name: "R_WRIST_R_S" -tcp_frame_name: "R_FINGER_TIP_FIXED" - -lambda: 0.0001 -w_posrot: 0.5 -max_iters: 80 -tol: 1e-6 -qp_time_limit: 0.005 diff --git a/cmvr-es/common/config/microphone_config/microphone_config.pb.txt b/cmvr-es/common/config/microphone_config/microphone_config.pb.txt deleted file mode 100644 index 46fe5bf4..00000000 --- a/cmvr-es/common/config/microphone_config/microphone_config.pb.txt +++ /dev/null @@ -1,8 +0,0 @@ -ffmpeg_microphones { - id: "mic1" - channels: 2 - sampleRate: 44100 - volume: 100 - enable: false - input_device: "default" -} \ No newline at end of file diff --git a/cmvr-es/common/config/speaker_config/speaker_config.pb.txt b/cmvr-es/common/config/speaker_config/speaker_config.pb.txt deleted file mode 100644 index cd1287ee..00000000 --- a/cmvr-es/common/config/speaker_config/speaker_config.pb.txt +++ /dev/null @@ -1,4 +0,0 @@ -ffmpeg_speakers { - id: "spk1" - enable: false -} \ No newline at end of file diff --git a/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt b/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt deleted file mode 100644 index 27a39f9a..00000000 --- a/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt +++ /dev/null @@ -1,151 +0,0 @@ -robot_id: "hc01" -dexhand_id: "paxini_tip_1" -camera_id: "right_hand_cam" -move_to_init_position_before_start: true -move_to_init_position: true -init_joint_positions { - joint_name: "R_SHOULDER_P" - rad: -0.3678 -} -init_joint_positions { - joint_name: "R_SHOULDER_R" - rad: 1.1127 -} -init_joint_positions { - joint_name: "R_SHOULDER_Y" - rad: 1.6084 -} -init_joint_positions { - joint_name: "R_ELBOW_R" - rad: 1.61 -} -init_joint_positions { - joint_name: "R_WRIST_P" - rad: -2.5718 -} -init_joint_positions { - joint_name: "R_WRIST_Y" - rad: 0.1276 -} -init_joint_positions { - joint_name: "R_WRIST_R" - rad: 0.1297 -} -init_movej_vel: 1.0 -init_movej_acc: 2.0 - -urdf_path: "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf" -base_link: "PELVIS_S" -flange_link: "R_WRIST_R_S" -camera_link: "R_CAM" - -tag_size_m: 0.012 -depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE -target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE - -hover_target_in_camera { - x: -0.001 - y: 0.08 - z: 0.15 -} -target_rx: 3.14159265358979323846 -target_ry: 0.0 -target_rz: 0.0 -align_mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION - -ibvs_lambda: 0.4 -ibvs_mu: 0.1 -ibvs_qdot_max: 1.0 -ibvs_vmax6 { - vx: 1.0 - vy: 1.0 - vz: 1.0 - wx: 0.6 - wy: 0.6 - wz: 0.6 -} -ibvs_amax6 { - vx: 2.4 - vy: 2.4 - vz: 4.5 - wx: 2.5 - wy: 2.5 - wz: 2.5 -} -ibvs_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 - m11: 1.0 - m22: 1.0 -} -r_camera_to_urdf { - m00: 1.0 - m11: 1.0 - m22: 1.0 -} - -control_joint_names: "R_SHOULDER_P" -control_joint_names: "R_SHOULDER_R" -control_joint_names: "R_SHOULDER_Y" -control_joint_names: "R_ELBOW_R" -control_joint_names: "R_WRIST_P" -control_joint_names: "R_WRIST_Y" -control_joint_names: "R_WRIST_R" - -align_error_threshold6 { - x: 0.005 - y: 0.005 - z: 0.01 - rx: 0.1026646259971647 - ry: 0.1026646259971647 - rz: 0.1026646259971647 -} -align_stable_frames: 2 -align_timeout_s: 20.0 -pause_after_align_reached: false - -touch_twist_base { - vx: 0.0 - vy: -0.04 - vz: 0.0 - wx: 0.0 - wy: 0.0 - wz: 0.0 -} -touch_use_speedl: true -touch_speedl_acceleration: 6.0 -touch_forward_l: 0.064 -touch_movel_speed: 0.1 -touch_movel_acceleration: 5.0 -touch_movel_jerk: 5.0 -touch_movel_qd_max: 2.5 -touch_movel_qd_max: 2.5 -touch_movel_qd_max: 2.5 -touch_movel_qd_max: 2.5 -touch_movel_qd_max: 2.5 -touch_movel_qd_max: 2.5 -touch_movel_qd_max: 2.5 -touch_speedl_forward_l: 0.035 - -dwell_time_s: -1 -retract_twist_base { - vx: 0.0 - vy: 0.08 - vz: 0.0 - wx: 0.0 - wy: 0.0 - wz: 0.0 -} -retract_acceleration: 8.0 -retract_duration_s: 0.45 - -tactile_finger: TOUCH_SCREEN_FINGER_TYPE_INDEX -tactile_region: TOUCH_SCREEN_TACTILE_REGION_TIP -tactile_criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ -tactile_pressure_sum_threshold: 1.0 -tactile_nonzero_count_threshold: 1 diff --git a/cmvr-es/common/utils/io/proto_message_utils.h b/cmvr-es/common/io/proto_file_io.h similarity index 66% rename from cmvr-es/common/utils/io/proto_message_utils.h rename to cmvr-es/common/io/proto_file_io.h index fa4b4768..7a098883 100644 --- a/cmvr-es/common/utils/io/proto_message_utils.h +++ b/cmvr-es/common/io/proto_file_io.h @@ -1,10 +1,12 @@ #pragma once -#include +#include "common/base/logging/logger.h" #include "google/protobuf/io/zero_copy_stream_impl.h" #include "google/protobuf/text_format.h" +#include +#include #include #include #include @@ -21,17 +23,17 @@ public: std::ofstream output(fileName, std::ios::out | std::ios::trunc | std::ios::binary); if (!output.good()) { - LOG(WARNING) << "Failed to open file for binary write: " << fileName; + CMVR_LOG(WARNING) << "Failed to open file for binary write: " << fileName; return false; } if (!message.SerializePartialToOstream(&output)) { - LOG(WARNING) << "Failed to serialize proto to binary file: " << fileName; + CMVR_LOG(WARNING) << "Failed to serialize proto to binary file: " << fileName; return false; } - LOG(INFO) << "Successfully wrote binary proto file: " << fileName; + CMVR_LOG(INFO) << "Successfully wrote binary proto file: " << fileName; return true; } @@ -40,20 +42,20 @@ public: { if (message == nullptr) { - LOG(ERROR) << "Null message pointer when reading binary proto file: " << fileName; + CMVR_LOG(ERROR) << "Null message pointer when reading binary proto file: " << fileName; return false; } std::ifstream input(fileName, std::ios::in | std::ios::binary); if (!input.good()) { - LOG(WARNING) << "Failed to open file for binary read: " << fileName; + CMVR_LOG(WARNING) << "Failed to open file for binary read: " << fileName; return false; } if (!message->ParseFromIstream(&input)) { - LOG(WARNING) << "Failed to parse binary proto file: " << fileName; + CMVR_LOG(WARNING) << "Failed to parse binary proto file: " << fileName; return false; } @@ -68,15 +70,16 @@ public: const int fd = ::open(fileName.c_str(), O_WRONLY | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR); if (fd < 0) { - PLOG(WARNING) << "Failed to open file for ASCII write: " << fileName; + CMVR_LOG(WARNING) << "Failed to open file for ASCII write: " << fileName + << ": " << std::strerror(errno); return false; } const bool ok = setProtoToAsciiFile(message, fd); if (ok) - LOG(INFO) << "Successfully wrote ASCII proto file: " << fileName; + CMVR_LOG(INFO) << "Successfully wrote ASCII proto file: " << fileName; else - LOG(WARNING) << "Failed to write ASCII proto file: " << fileName; + CMVR_LOG(WARNING) << "Failed to write ASCII proto file: " << fileName; return ok; } @@ -89,7 +92,7 @@ public: if (fileDescriptor < 0) { - LOG(WARNING) << "Invalid file descriptor for ASCII write."; + CMVR_LOG(WARNING) << "Invalid file descriptor for ASCII write."; return false; } @@ -110,17 +113,20 @@ public: if (message == nullptr) { - LOG(ERROR) << "Null message pointer when reading ASCII proto file: " << fileName; + CMVR_LOG(ERROR) << "Null message pointer when reading ASCII proto file: " << fileName; return false; } const int fd = ::open(fileName.c_str(), O_RDONLY); if (fd < 0) { - if (isOptional) - PLOG(INFO) << "Optional ASCII proto file not found/openable: " << fileName; - else - PLOG(ERROR) << "Failed to open ASCII proto file: " << fileName; + if (isOptional) { + CMVR_LOG(INFO) << "Optional ASCII proto file not found/openable: " << fileName + << ": " << std::strerror(errno); + } else { + CMVR_LOG(ERROR) << "Failed to open ASCII proto file: " << fileName + << ": " << std::strerror(errno); + } return false; } @@ -131,9 +137,9 @@ public: if (!ok) { if (isOptional) - LOG(INFO) << "Failed to parse optional ASCII proto file: " << fileName; + CMVR_LOG(INFO) << "Failed to parse optional ASCII proto file: " << fileName; else - LOG(ERROR) << "Failed to parse ASCII proto file: " << fileName; + CMVR_LOG(ERROR) << "Failed to parse ASCII proto file: " << fileName; } return ok; } diff --git a/cmvr-es/common/math/cartesian_motion_math.h b/cmvr-es/common/math/cartesian_motion_math.h new file mode 100644 index 00000000..8f5cb8ca --- /dev/null +++ b/cmvr-es/common/math/cartesian_motion_math.h @@ -0,0 +1,58 @@ +#ifndef CMVR_ES_CARTESIAN_MOTION_MATH_H +#define CMVR_ES_CARTESIAN_MOTION_MATH_H + +#include +#include + +#include +#include +#include + +namespace cmvr::device::cartesian_motion { + +inline double clamp(const double value, const double lo, const double hi) +{ + return std::max(lo, std::min(hi, value)); +} + +inline Eigen::VectorXd toEigenVector(const std::vector& values) +{ + if (values.empty()) { + return {}; + } + return Eigen::Map(values.data(), + static_cast(values.size())); +} + +inline std::vector toStdVector(const Eigen::VectorXd& values) +{ + return {values.data(), values.data() + values.size()}; +} + +inline double directionDeviationDeg(const Eigen::Vector3d& desired, + const Eigen::Vector3d& actual) +{ + const double desired_norm = desired.norm(); + const double actual_norm = actual.norm(); + if (desired_norm <= 1e-9 || actual_norm <= 1e-9) { + return 0.0; + } + const double direction_cos = + clamp(desired.dot(actual) / (desired_norm * actual_norm), -1.0, 1.0); + constexpr double rad_to_deg = 180.0 / 3.14159265358979323846; + return std::acos(direction_cos) * rad_to_deg; +} + +inline Eigen::Vector3d rotationVector(const Eigen::Matrix3d& rotation) +{ + Eigen::AngleAxisd angle_axis(rotation); + const double angle = angle_axis.angle(); + if (std::abs(angle) <= 1e-9) { + return Eigen::Vector3d::Zero(); + } + return angle_axis.axis() * angle; +} + +} // namespace cmvr::device::cartesian_motion + +#endif // CMVR_ES_CARTESIAN_MOTION_MATH_H diff --git a/cmvr-es/common/math/joint_limits.h b/cmvr-es/common/math/joint_limits.h new file mode 100644 index 00000000..c05a1ab4 --- /dev/null +++ b/cmvr-es/common/math/joint_limits.h @@ -0,0 +1,101 @@ +#ifndef CMVR_ES_JOINT_LIMITS_H +#define CMVR_ES_JOINT_LIMITS_H + +#include + +#include +#include +#include + +namespace cmvr::kinematics { + +inline Eigen::VectorXd clampToJointPositionLimits( + const Eigen::VectorXd& q, + const Eigen::VectorXd& lower, + const Eigen::VectorXd& upper) +{ + if (lower.size() != q.size() || upper.size() != q.size()) { + return q; + } + return q.cwiseMax(lower).cwiseMin(upper); +} + +inline double velocityLimitScale(const Eigen::VectorXd& qdot, + const Eigen::VectorXd& velocity_limits, + const double abs_max = std::numeric_limits::infinity()) +{ + double scale = 1.0; + for (Eigen::Index i = 0; i < qdot.size(); ++i) { + double limit = std::numeric_limits::infinity(); + if (std::isfinite(abs_max) && abs_max > 0.0) { + limit = std::min(limit, abs_max); + } + if (velocity_limits.size() == qdot.size()) { + const double joint_limit = std::abs(velocity_limits[i]); + if (std::isfinite(joint_limit) && joint_limit > 0.0) { + limit = std::min(limit, joint_limit); + } + } + + const double value = std::abs(qdot[i]); + if (std::isfinite(limit) && limit > 0.0 && value > limit) { + scale = std::min(scale, limit / value); + } + } + return scale; +} + +inline Eigen::VectorXd scaleToVelocityLimits( + const Eigen::VectorXd& qdot, + const Eigen::VectorXd& velocity_limits, + const double abs_max = std::numeric_limits::infinity()) +{ + return velocityLimitScale(qdot, velocity_limits, abs_max) * qdot; +} + +inline Eigen::VectorXd computeJointLimitAvoidanceVelocity( + const Eigen::VectorXd& q, + const Eigen::VectorXd& lower, + const Eigen::VectorXd& upper, + const bool enable, + const double gain, + const double margin_ratio, + const double max_push) +{ + const Eigen::Index dof = q.size(); + if (!enable || gain <= 0.0 || dof <= 0 || + lower.size() != dof || upper.size() != dof) { + return Eigen::VectorXd::Zero(dof); + } + + Eigen::VectorXd qdot_avoid = Eigen::VectorXd::Zero(dof); + for (Eigen::Index i = 0; i < dof; ++i) { + const double lo = lower[i]; + const double hi = upper[i]; + if (!std::isfinite(lo) || !std::isfinite(hi) || hi <= lo) { + continue; + } + + const double span = hi - lo; + const double margin = std::max(1e-4, margin_ratio * span); + double push = 0.0; + if (q[i] < lo + margin) { + const double s = (lo + margin - q[i]) / margin; + push += gain * s * s; + } else if (q[i] > hi - margin) { + const double s = (q[i] - (hi - margin)) / margin; + push -= gain * s * s; + } + + if (max_push > 0.0) { + push = std::max(-max_push, std::min(max_push, push)); + } + qdot_avoid[i] = push; + } + + return qdot_avoid; +} + +} // namespace cmvr::kinematics + +#endif // CMVR_ES_JOINT_LIMITS_H diff --git a/cmvr-es/common/math/proto_geometry.h b/cmvr-es/common/math/proto_geometry.h new file mode 100644 index 00000000..f065ef88 --- /dev/null +++ b/cmvr-es/common/math/proto_geometry.h @@ -0,0 +1,115 @@ +#ifndef CMVR_ES_COMMON_MATH_PROTO_GEOMETRY_H +#define CMVR_ES_COMMON_MATH_PROTO_GEOMETRY_H + +#include + +#include "cmvr/common/geometry.pb.h" + +namespace cmvr::common::math { + +inline Eigen::Vector3d toEigenVec3(const cmvr::common::Vec3& src, + Eigen::Vector3d defaults) +{ + if (src.has_x()) { + defaults.x() = src.x(); + } + if (src.has_y()) { + defaults.y() = src.y(); + } + if (src.has_z()) { + defaults.z() = src.z(); + } + return defaults; +} + +inline Eigen::Vector3d toEigenVec3(const cmvr::common::Vec3& src) +{ + return toEigenVec3(src, Eigen::Vector3d::Zero()); +} + +inline Eigen::Matrix toEigenVec6( + const cmvr::common::Vec6& src, + Eigen::Matrix defaults) +{ + if (src.has_x()) { + defaults[0] = src.x(); + } + if (src.has_y()) { + defaults[1] = src.y(); + } + if (src.has_z()) { + defaults[2] = src.z(); + } + if (src.has_rx()) { + defaults[3] = src.rx(); + } + if (src.has_ry()) { + defaults[4] = src.ry(); + } + if (src.has_rz()) { + defaults[5] = src.rz(); + } + return defaults; +} + +inline Eigen::Matrix toEigenVec6(const cmvr::common::Vec6& src) +{ + return toEigenVec6(src, Eigen::Matrix::Zero()); +} + +inline Eigen::Matrix3d toEigenMat3(const cmvr::common::Mat3& src, + Eigen::Matrix3d defaults) +{ + if (src.has_m00()) { + defaults(0, 0) = src.m00(); + } + if (src.has_m01()) { + defaults(0, 1) = src.m01(); + } + if (src.has_m02()) { + defaults(0, 2) = src.m02(); + } + if (src.has_m10()) { + defaults(1, 0) = src.m10(); + } + if (src.has_m11()) { + defaults(1, 1) = src.m11(); + } + if (src.has_m12()) { + defaults(1, 2) = src.m12(); + } + if (src.has_m20()) { + defaults(2, 0) = src.m20(); + } + if (src.has_m21()) { + defaults(2, 1) = src.m21(); + } + if (src.has_m22()) { + defaults(2, 2) = src.m22(); + } + return defaults; +} + +inline Eigen::Matrix3d toEigenMat3(const cmvr::common::Mat3& src) +{ + return toEigenMat3(src, Eigen::Matrix3d::Identity()); +} + +} // namespace cmvr::common::math + + +inline bool hasVec3(const cmvr::common::Vec3& value) { + return value.has_x() && value.has_y() && value.has_z(); +} + +inline bool hasVec6(const cmvr::common::Vec6& value) { + return value.has_x() && value.has_y() && value.has_z() && + value.has_rx() && value.has_ry() && value.has_rz(); +} + +inline bool hasMat3(const cmvr::common::Mat3& value) { + return value.has_m00() && value.has_m01() && value.has_m02() && + value.has_m10() && value.has_m11() && value.has_m12() && + value.has_m20() && value.has_m21() && value.has_m22(); +} +#endif // CMVR_ES_COMMON_MATH_PROTO_GEOMETRY_H diff --git a/cmvr-es/common/math/include/qp_solver.h b/cmvr-es/common/math/qp_solver.h similarity index 98% rename from cmvr-es/common/math/include/qp_solver.h rename to cmvr-es/common/math/qp_solver.h index 70251730..d8d913fd 100644 --- a/cmvr-es/common/math/include/qp_solver.h +++ b/cmvr-es/common/math/qp_solver.h @@ -11,7 +11,7 @@ #include #include #include -#include +#include "common/base/logging/logger.h" #include #include @@ -171,13 +171,13 @@ inline void QPSolverImpl::InitFunctionImpl() { inline void QPSolverImpl::AddCostFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &b) { if (A.rows() != b.rows()) { - std::cerr << "OSQP Solver Add cost function failed: Size issue Ax = b (A.rows(): " - << A.rows() << ", b.rows(): " << b.rows() << ")" << std::endl; + CMVR_LOG(ERROR) << "OSQP Solver Add cost function failed: Size issue Ax = b (A.rows(): " + << A.rows() << ", b.rows(): " << b.rows() << ")"; return; } if (b.cols() != 1) { - std::cerr << "OSQP Solver Add cost function failed: Size issue: b (Nx1)" << std::endl; + CMVR_LOG(ERROR) << "OSQP Solver Add cost function failed: Size issue: b (Nx1)"; return; } diff --git a/cmvr-es/common/math/include/support_functions.h b/cmvr-es/common/math/support_functions.h similarity index 96% rename from cmvr-es/common/math/include/support_functions.h rename to cmvr-es/common/math/support_functions.h index 4a86c60a..f6c38923 100644 --- a/cmvr-es/common/math/include/support_functions.h +++ b/cmvr-es/common/math/support_functions.h @@ -6,7 +6,8 @@ #include #include #include -#include +#include +#include "common/base/logging/logger.h" #include class SupportFunctions { @@ -124,10 +125,13 @@ public: } static void print_intervals(const std::vector > &intervals) { + std::ostringstream output; for (const auto &interval: intervals) { - std::cout << "[" << interval.first << ", " << interval.second << "] "; + output << "[" << interval.first << ", " << interval.second << "] "; + } + if (!output.str().empty()) { + CMVR_LOG(INFO) << output.str(); } - std::cout << std::endl; } diff --git a/cmvr-es/common/math/transform_math.h b/cmvr-es/common/math/transform_math.h new file mode 100644 index 00000000..4f38ee62 --- /dev/null +++ b/cmvr-es/common/math/transform_math.h @@ -0,0 +1,94 @@ +#ifndef CMVR_ES_COMMON_MATH_TRANSFORM_MATH_H +#define CMVR_ES_COMMON_MATH_TRANSFORM_MATH_H + +#include + +#include + +#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" +#include "common/types/arm/arm_types.h" + +namespace cmvr::common::math { + +inline Eigen::Matrix3d eulerZYXToRotationMatrix(const double rx, + const double ry, + const double rz) +{ + Eigen::Matrix3d r_x; + r_x << 1.0, 0.0, 0.0, + 0.0, std::cos(rx), -std::sin(rx), + 0.0, std::sin(rx), std::cos(rx); + + Eigen::Matrix3d r_y; + r_y << std::cos(ry), 0.0, std::sin(ry), + 0.0, 1.0, 0.0, + -std::sin(ry), 0.0, std::cos(ry); + + Eigen::Matrix3d r_z; + r_z << std::cos(rz), -std::sin(rz), 0.0, + std::sin(rz), std::cos(rz), 0.0, + 0.0, 0.0, 1.0; + + return r_x * r_y * r_z; +} + +inline Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d& rotation) +{ + const double ry = std::asin(rotation(0, 2)); + const double cy = std::cos(ry); + double rx = 0.0; + double rz = 0.0; + if (std::abs(cy) > 1e-6) { + rx = std::atan2(-rotation(1, 2), rotation(2, 2)); + rz = std::atan2(-rotation(0, 1), rotation(0, 0)); + } else { + rz = ry > 0.0 ? std::atan2(rotation(1, 0), rotation(1, 1)) + : std::atan2(-rotation(1, 0), rotation(1, 1)); + } + return {rx, ry, rz}; +} + +inline Eigen::Matrix4d poseToMatrix(const device::CartesianPose& pose) +{ + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); + transform.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose.rx, pose.ry, pose.rz); + transform(0, 3) = pose.x; + transform(1, 3) = pose.y; + transform(2, 3) = pose.z; + return transform; +} + +inline device::CartesianPose matrixToPose(const Eigen::Matrix4d& transform) +{ + device::CartesianPose pose; + pose.x = transform(0, 3); + pose.y = transform(1, 3); + pose.z = transform(2, 3); + const Eigen::Vector3d euler = rotationMatrixToEulerZYX(transform.block<3, 3>(0, 0)); + pose.rx = euler(0); + pose.ry = euler(1); + pose.rz = euler(2); + return pose; +} + +inline Eigen::Matrix velocityToVector(const device::CartesianVelocity& velocity) +{ + Eigen::Matrix value; + value << velocity.vx, velocity.vy, velocity.vz, velocity.wx, velocity.wy, velocity.wz; + return value; +} + +inline device::CartesianVelocity vectorToVelocity(const Eigen::Matrix& velocity) +{ + return {velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]}; +} + +inline cmvr::CartesianFrame toPlannerFrame(const device::FrameType frame) +{ + return frame == device::FrameType::Tool ? cmvr::CartesianFrame::Tool + : cmvr::CartesianFrame::Base; +} + +} // namespace cmvr::common::math + +#endif // CMVR_ES_COMMON_MATH_TRANSFORM_MATH_H diff --git a/cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp b/cmvr-es/common/media/ffmpeg/camera_capture.cpp similarity index 81% rename from cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp rename to cmvr-es/common/media/ffmpeg/camera_capture.cpp index 9347009d..d8f2c61a 100644 --- a/cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp +++ b/cmvr-es/common/media/ffmpeg/camera_capture.cpp @@ -1,6 +1,6 @@ // CameraCapture.cpp -#include "../include/CameraCapture.h" -#include +#include "common/media/ffmpeg/camera_capture.h" +#include "common/base/logging/logger.h" #include #include @@ -28,21 +28,21 @@ int CameraCapture::initialize(const Config& config) { // 初始化设备 int ret = init_device(); if (ret < 0) { - std::cerr << "初始化设备失败" << std::endl; + CMVR_LOG(ERROR) << "初始化设备失败"; return ret; } // 初始化解码器 ret = init_decoder(); if (ret < 0) { - std::cerr << "初始化解码器失败" << std::endl; + CMVR_LOG(ERROR) << "初始化解码器失败"; return ret; } // 初始化SWS上下文 ret = init_sws_context(); if (ret < 0) { - std::cerr << "初始化SWS上下文失败" << std::endl; + CMVR_LOG(ERROR) << "初始化SWS上下文失败"; return ret; } @@ -52,12 +52,12 @@ int CameraCapture::initialize(const Config& config) { rgb_frame_->format = AV_PIX_FMT_BGR24; // OpenCV使用BGR格式 ret = av_frame_get_buffer(rgb_frame_, 0); if (ret < 0) { - std::cerr << "分配RGB帧缓冲区失败" << std::endl; + CMVR_LOG(ERROR) << "分配RGB帧缓冲区失败"; return ret; } - std::cout << "摄像头初始化成功: " << config_.width << "x" << config_.height - << "@" << config_.fps << "fps" << std::endl; + CMVR_LOG(INFO) << "摄像头初始化成功: " << config_.width << "x" << config_.height + << "@" << config_.fps << "fps"; return 0; } @@ -74,7 +74,7 @@ int CameraCapture::init_device() { #endif if (!input_fmt) { - std::cerr << "找不到输入格式" << std::endl; + CMVR_LOG(ERROR) << "找不到输入格式"; return -1; } @@ -90,21 +90,21 @@ int CameraCapture::init_device() { if (ret < 0) { char err_buf[1024]; av_strerror(ret, err_buf, sizeof(err_buf)); - std::cerr << "打开摄像头失败: " << err_buf << std::endl; + CMVR_LOG(ERROR) << "打开摄像头失败: " << err_buf; return ret; } // 查找流信息 ret = avformat_find_stream_info(fmt_ctx_, nullptr); if (ret < 0) { - std::cerr << "查找流信息失败" << std::endl; + CMVR_LOG(ERROR) << "查找流信息失败"; return ret; } // 查找视频流 video_stream_index_ = av_find_best_stream(fmt_ctx_, AVMEDIA_TYPE_VIDEO, -1, -1, nullptr, 0); if (video_stream_index_ < 0) { - std::cerr << "找不到视频流" << std::endl; + CMVR_LOG(ERROR) << "找不到视频流"; return video_stream_index_; } @@ -116,27 +116,27 @@ int CameraCapture::init_decoder() { const AVCodec* decoder = avcodec_find_decoder(stream->codecpar->codec_id); if (!decoder) { - std::cerr << "找不到解码器" << std::endl; + CMVR_LOG(ERROR) << "找不到解码器"; return -1; } decoder_ctx_ = avcodec_alloc_context3(decoder); if (!decoder_ctx_) { - std::cerr << "分配解码器上下文失败" << std::endl; + CMVR_LOG(ERROR) << "分配解码器上下文失败"; return -1; } // 复制参数到解码器上下文 int ret = avcodec_parameters_to_context(decoder_ctx_, stream->codecpar); if (ret < 0) { - std::cerr << "复制解码器参数失败" << std::endl; + CMVR_LOG(ERROR) << "复制解码器参数失败"; return ret; } // 打开解码器 ret = avcodec_open2(decoder_ctx_, decoder, nullptr); if (ret < 0) { - std::cerr << "打开解码器失败" << std::endl; + CMVR_LOG(ERROR) << "打开解码器失败"; return ret; } @@ -154,7 +154,7 @@ int CameraCapture::init_sws_context() { ); if (!sws_ctx_) { - std::cerr << "创建SWS上下文失败" << std::endl; + CMVR_LOG(ERROR) << "创建SWS上下文失败"; return -1; } @@ -185,14 +185,14 @@ cv::Mat CameraCapture::avframe_to_cvmat(AVFrame* frame) { int CameraCapture::start_capture(FrameCallback callback) { if (!callback || !fmt_ctx_ || !decoder_ctx_) { - std::cerr << "参数无效或未初始化" << std::endl; + CMVR_LOG(ERROR) << "参数无效或未初始化"; return -1; } is_capturing_ = true; frame_count_ = 0; - std::cout << "开始采集..." << std::endl; + CMVR_LOG(INFO) << "开始采集..."; while (is_capturing_) { // 读取数据包 @@ -202,7 +202,7 @@ int CameraCapture::start_capture(FrameCallback callback) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); continue; } - std::cerr << "读取帧失败: " << ret << std::endl; + CMVR_LOG(ERROR) << "读取帧失败: " << ret; break; } @@ -211,7 +211,7 @@ int CameraCapture::start_capture(FrameCallback callback) { // 发送数据包到解码器 ret = avcodec_send_packet(decoder_ctx_, packet_); if (ret < 0 && ret != AVERROR(EAGAIN)) { - std::cerr << "发送数据包到解码器失败: " << ret << std::endl; + CMVR_LOG(ERROR) << "发送数据包到解码器失败: " << ret; av_packet_unref(packet_); continue; } @@ -222,7 +222,7 @@ int CameraCapture::start_capture(FrameCallback callback) { if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) { break; } else if (ret < 0) { - std::cerr << "接收解码帧失败: " << ret << std::endl; + CMVR_LOG(ERROR) << "接收解码帧失败: " << ret; break; } diff --git a/cmvr-es/common/utils/ffmpeg/include/CameraCapture.h b/cmvr-es/common/media/ffmpeg/camera_capture.h similarity index 100% rename from cmvr-es/common/utils/ffmpeg/include/CameraCapture.h rename to cmvr-es/common/media/ffmpeg/camera_capture.h diff --git a/cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp b/cmvr-es/common/media/ffmpeg/realsense_capture.cpp similarity index 83% rename from cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp rename to cmvr-es/common/media/ffmpeg/realsense_capture.cpp index 499e2612..c9d87495 100644 --- a/cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp +++ b/cmvr-es/common/media/ffmpeg/realsense_capture.cpp @@ -1,6 +1,6 @@ // RealSenseCapture.cpp -#include "../include/RealSenseCapture.h" -#include +#include "common/media/ffmpeg/realsense_capture.h" +#include "common/base/logging/logger.h" #include namespace ffmpeg { @@ -26,7 +26,7 @@ int RealSenseCapture::initialize(const Config& config) { // 初始化设备 int ret = init_device(); if (ret < 0) { - std::cerr << "初始化RealSense设备失败" << std::endl; + CMVR_LOG(ERROR) << "初始化RealSense设备失败"; return ret; } @@ -38,20 +38,20 @@ int RealSenseCapture::initialize(const Config& config) { rgb_frame_ = create_avframe(config_.width, config_.height, AV_PIX_FMT_BGR24); if (!frame_ || !rgb_frame_) { - std::cerr << "创建AVFrame失败" << std::endl; + CMVR_LOG(ERROR) << "创建AVFrame失败"; return -1; } - std::cout << "RealSense摄像头初始化成功: " - << config_.width << "x" << config_.height << "@" << config_.fps << "fps" << std::endl; + CMVR_LOG(INFO) << "RealSense摄像头初始化成功: " + << config_.width << "x" << config_.height << "@" << config_.fps << "fps"; return 0; } catch (const rs2::error& e) { - std::cerr << "RealSense错误: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "RealSense错误: " << e.what(); return -1; } catch (const std::exception& e) { - std::cerr << "常规错误: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "常规错误: " << e.what(); return -1; } } @@ -63,11 +63,11 @@ int RealSenseCapture::init_device() { size_t device_count = devices.size(); if (device_count == 0) { - std::cerr << "未检测到RealSense设备" << std::endl; + CMVR_LOG(ERROR) << "未检测到RealSense设备"; return -1; } - std::cout << "检测到 " << device_count << " 个RealSense设备" << std::endl; + CMVR_LOG(INFO) << "检测到 " << device_count << " 个RealSense设备"; // 如果指定了序列号，查找对应设备 if (!config_.serial_number.empty()) { @@ -77,7 +77,7 @@ int RealSenseCapture::init_device() { std::string serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); if (serial == config_.serial_number) { - std::cout << "找到指定序列号的设备: " << serial << std::endl; + CMVR_LOG(INFO) << "找到指定序列号的设备: " << serial; found = true; rs_cfg_.enable_device(serial); break; @@ -85,15 +85,15 @@ int RealSenseCapture::init_device() { } if (!found) { - std::cerr << "未找到序列号为 " << config_.serial_number << " 的设备" << std::endl; + CMVR_LOG(ERROR) << "未找到序列号为 " << config_.serial_number << " 的设备"; return -1; } } else { // 使用第一个设备 rs2::device dev = devices[0]; std::string serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); - std::cout << "使用第一个设备: " << dev.get_info(RS2_CAMERA_INFO_NAME) - << " (序列号: " << serial << ")" << std::endl; + CMVR_LOG(INFO) << "使用第一个设备: " << dev.get_info(RS2_CAMERA_INFO_NAME) + << " (序列号: " << serial << ")"; rs_cfg_.enable_device(serial); } @@ -107,7 +107,7 @@ int RealSenseCapture::init_device() { return 0; } catch (const rs2::error& e) { - std::cerr << "初始化设备失败: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "初始化设备失败: " << e.what(); return -1; } } @@ -170,12 +170,12 @@ int RealSenseCapture::start_capture(FrameCallback callback) { std::lock_guard lock(mutex_); if (!callback) { - std::cerr << "回调函数为空" << std::endl; + CMVR_LOG(ERROR) << "回调函数为空"; return -1; } if (is_capturing_) { - std::cerr << "已经在捕获中" << std::endl; + CMVR_LOG(ERROR) << "已经在捕获中"; return -1; } @@ -190,7 +190,7 @@ int RealSenseCapture::start_capture(FrameCallback callback) { // 等待第一帧，确保设备正常工作 rs2::frameset frames = pipe_.wait_for_frames(2000); // 2秒超时 if (!frames.get_color_frame()) { - std::cerr << "无法获取第一帧" << std::endl; + CMVR_LOG(ERROR) << "无法获取第一帧"; pipe_.stop(); return -1; } @@ -202,11 +202,11 @@ int RealSenseCapture::start_capture(FrameCallback callback) { capture_thread_ = std::make_unique(&RealSenseCapture::capture_thread_func, this, callback); - std::cout << "RealSense开始采集" << std::endl; + CMVR_LOG(INFO) << "RealSense开始采集"; return 0; } catch (const rs2::error& e) { - std::cerr << "启动采集失败: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "启动采集失败: " << e.what(); return -1; } } @@ -238,7 +238,7 @@ void RealSenseCapture::capture_thread_func(FrameCallback callback) { callback(av_frame, rgb_image, frame_count_++); } catch (const rs2::error& e) { - std::cerr << "采集错误: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "采集错误: " << e.what(); if (!is_capturing_) break; } @@ -270,10 +270,10 @@ void RealSenseCapture::stop_capture() { try { pipe_.stop(); } catch (const std::exception& e) { - std::cerr << "停止pipeline时出错: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "停止pipeline时出错: " << e.what(); } - std::cout << "RealSense停止采集" << std::endl; + CMVR_LOG(INFO) << "RealSense停止采集"; } std::string RealSenseCapture::get_device_info() const { diff --git a/cmvr-es/common/utils/ffmpeg/include/RealSenseCapture.h b/cmvr-es/common/media/ffmpeg/realsense_capture.h similarity index 100% rename from cmvr-es/common/utils/ffmpeg/include/RealSenseCapture.h rename to cmvr-es/common/media/ffmpeg/realsense_capture.h diff --git a/cmvr-es/common/utils/ffmpeg/src/VideoFrameEncoder.cpp b/cmvr-es/common/media/ffmpeg/video_frame_encoder.cpp similarity index 88% rename from cmvr-es/common/utils/ffmpeg/src/VideoFrameEncoder.cpp rename to cmvr-es/common/media/ffmpeg/video_frame_encoder.cpp index fc215319..57927420 100644 --- a/cmvr-es/common/utils/ffmpeg/src/VideoFrameEncoder.cpp +++ b/cmvr-es/common/media/ffmpeg/video_frame_encoder.cpp @@ -1,6 +1,6 @@ // VideoFrameEncoder.cpp -#include "../include/VideoFrameEncoder.h" -#include +#include "common/media/ffmpeg/video_frame_encoder.h" +#include "common/base/logging/logger.h" #include namespace ffmpeg { @@ -30,14 +30,14 @@ int VideoFrameEncoder::initialize(const Config& config) { //const AVCodec* encoder = avcodec_find_encoder(AV_CODEC_ID_H264); const AVCodec* encoder = avcodec_find_encoder_by_name(config.codec.c_str()); if (!encoder) { - std::cerr << "找不到H.264编码器" << std::endl; + CMVR_LOG(ERROR) << "找不到H.264编码器"; return -1; } // 分配编码器上下文 encoder_ctx_ = avcodec_alloc_context3(encoder); if (!encoder_ctx_) { - std::cerr << "分配编码器上下文失败" << std::endl; + CMVR_LOG(ERROR) << "分配编码器上下文失败"; return -1; } @@ -70,7 +70,7 @@ int VideoFrameEncoder::initialize(const Config& config) { if (ret < 0) { char err_buf[1024]; av_strerror(ret, err_buf, sizeof(err_buf)); - std::cerr << "打开编码器失败: " << err_buf << std::endl; + CMVR_LOG(ERROR) << "打开编码器失败: " << err_buf; return ret; } @@ -81,7 +81,7 @@ int VideoFrameEncoder::initialize(const Config& config) { ret = av_frame_get_buffer(converted_frame_, 0); if (ret < 0) { - std::cerr << "分配帧缓冲区失败" << std::endl; + CMVR_LOG(ERROR) << "分配帧缓冲区失败"; return ret; } @@ -90,7 +90,7 @@ int VideoFrameEncoder::initialize(const Config& config) { int VideoFrameEncoder::init_sws_context(AVFrame* frame) { if (!frame) { - std::cerr << "输入帧为空" << std::endl; + CMVR_LOG(ERROR) << "输入帧为空"; return -1; } @@ -110,7 +110,7 @@ int VideoFrameEncoder::init_sws_context(AVFrame* frame) { ); if (!sws_ctx_) { - std::cerr << "创建SWS上下文失败" << std::endl; + CMVR_LOG(ERROR) << "创建SWS上下文失败"; return -1; } @@ -119,7 +119,7 @@ int VideoFrameEncoder::init_sws_context(AVFrame* frame) { int VideoFrameEncoder::encode_frame(AVFrame* frame) { if (!encoder_ctx_ || !frame) { - std::cerr << "编码器未初始化或输入帧为空" << std::endl; + CMVR_LOG(ERROR) << "编码器未初始化或输入帧为空"; return -1; } @@ -151,7 +151,7 @@ int VideoFrameEncoder::encode_frame(AVFrame* frame) { if (ret < 0) { char err_buf[1024]; av_strerror(ret, err_buf, sizeof(err_buf)); - std::cerr << "发送帧到编码器失败: " << err_buf << std::endl; + CMVR_LOG(ERROR) << "发送帧到编码器失败: " << err_buf; return ret; } @@ -162,7 +162,7 @@ int VideoFrameEncoder::encode_frame(AVFrame* frame) { if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) { break; } else if (ret < 0) { - std::cerr << "接收编码数据包失败" << std::endl; + CMVR_LOG(ERROR) << "接收编码数据包失败"; av_packet_free(&packet); return ret; } @@ -191,7 +191,7 @@ int VideoFrameEncoder::flush() { // 发送空帧刷新编码器 int ret = avcodec_send_frame(encoder_ctx_, nullptr); if (ret < 0) { - std::cerr << "发送刷新帧失败" << std::endl; + CMVR_LOG(ERROR) << "发送刷新帧失败"; return ret; } @@ -202,7 +202,7 @@ int VideoFrameEncoder::flush() { if (ret == AVERROR_EOF) { break; } else if (ret < 0) { - std::cerr << "接收刷新数据包失败" << std::endl; + CMVR_LOG(ERROR) << "接收刷新数据包失败"; av_packet_free(&packet); return ret; } diff --git a/cmvr-es/common/utils/ffmpeg/include/VideoFrameEncoder.h b/cmvr-es/common/media/ffmpeg/video_frame_encoder.h similarity index 100% rename from cmvr-es/common/utils/ffmpeg/include/VideoFrameEncoder.h rename to cmvr-es/common/media/ffmpeg/video_frame_encoder.h diff --git a/cmvr-es/common/utils/ffmpeg/src/VideoWriter.cpp b/cmvr-es/common/media/ffmpeg/video_writer.cpp similarity index 79% rename from cmvr-es/common/utils/ffmpeg/src/VideoWriter.cpp rename to cmvr-es/common/media/ffmpeg/video_writer.cpp index 402565ca..883120ad 100644 --- a/cmvr-es/common/utils/ffmpeg/src/VideoWriter.cpp +++ b/cmvr-es/common/media/ffmpeg/video_writer.cpp @@ -1,6 +1,6 @@ // VideoWriter.cpp -#include "../include/VideoWriter.h" -#include +#include "common/media/ffmpeg/video_writer.h" +#include "common/base/logging/logger.h" namespace ffmpeg { @@ -15,28 +15,28 @@ int VideoWriter::initialize(const Config& config, AVCodecContext* codec_ctx) { codec_ctx_ref_ = codec_ctx; if (!codec_ctx) { - std::cerr << "编码器上下文为空" << std::endl; + CMVR_LOG(ERROR) << "编码器上下文为空"; return -1; } // 分配输出格式上下文 int ret = avformat_alloc_output_context2(&fmt_ctx_, nullptr, nullptr, config_.output_file.c_str()); if (ret < 0 || !fmt_ctx_) { - std::cerr << "分配输出格式上下文失败" << std::endl; + CMVR_LOG(ERROR) << "分配输出格式上下文失败"; return ret; } // 创建输出流 video_stream_ = avformat_new_stream(fmt_ctx_, nullptr); if (!video_stream_) { - std::cerr << "创建输出流失败" << std::endl; + CMVR_LOG(ERROR) << "创建输出流失败"; return -1; } // 复制编码器参数到输出流 ret = avcodec_parameters_from_context(video_stream_->codecpar, codec_ctx); if (ret < 0) { - std::cerr << "复制编码器参数失败" << std::endl; + CMVR_LOG(ERROR) << "复制编码器参数失败"; return ret; } @@ -52,7 +52,7 @@ int VideoWriter::initialize(const Config& config, AVCodecContext* codec_ctx) { if (ret < 0) { char err_buf[1024]; av_strerror(ret, err_buf, sizeof(err_buf)); - std::cerr << "打开输出文件失败: " << err_buf << std::endl; + CMVR_LOG(ERROR) << "打开输出文件失败: " << err_buf; return ret; } } @@ -60,26 +60,26 @@ int VideoWriter::initialize(const Config& config, AVCodecContext* codec_ctx) { // 写入文件头 ret = avformat_write_header(fmt_ctx_, nullptr); if (ret < 0) { - std::cerr << "写入文件头失败" << std::endl; + CMVR_LOG(ERROR) << "写入文件头失败"; return ret; } - std::cout << "视频写入器初始化完成，输出文件: " << config_.output_file + CMVR_LOG(INFO) << "视频写入器初始化完成，输出文件: " << config_.output_file << " 时间基: " << video_stream_->time_base.num << "/" << video_stream_->time_base.den - << std::endl; + ; return 0; } int VideoWriter::write_packet(AVPacket* packet, int64_t pts, int64_t dts) { if (!fmt_ctx_ || !video_stream_) { - std::cerr << "写入器未初始化" << std::endl; + CMVR_LOG(ERROR) << "写入器未初始化"; return -1; } // 克隆数据包，避免修改原始数据 AVPacket* cloned_packet = av_packet_clone(packet); if (!cloned_packet) { - std::cerr << "克隆数据包失败" << std::endl; + CMVR_LOG(ERROR) << "克隆数据包失败"; return -1; } @@ -112,7 +112,7 @@ int VideoWriter::write_packet(AVPacket* packet, int64_t pts, int64_t dts) { if (ret < 0) { char err_buf[1024]; av_strerror(ret, err_buf, sizeof(err_buf)); - std::cerr << "写入数据包失败: " << err_buf << std::endl; + CMVR_LOG(ERROR) << "写入数据包失败: " << err_buf; } else { frame_count_++; } @@ -129,9 +129,9 @@ int VideoWriter::finish() { // 写入文件尾 int ret = av_write_trailer(fmt_ctx_); if (ret < 0) { - std::cerr << "写入文件尾失败" << std::endl; + CMVR_LOG(ERROR) << "写入文件尾失败"; } else { - std::cout << "视频写入完成，总帧数: " << frame_count_ << std::endl; + CMVR_LOG(INFO) << "视频写入完成，总帧数: " << frame_count_; } cleanup(); diff --git a/cmvr-es/common/utils/ffmpeg/include/VideoWriter.h b/cmvr-es/common/media/ffmpeg/video_writer.h similarity index 100% rename from cmvr-es/common/utils/ffmpeg/include/VideoWriter.h rename to cmvr-es/common/media/ffmpeg/video_writer.h diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h new file mode 100644 index 00000000..567b503c --- /dev/null +++ b/cmvr-es/common/types/arm/arm_types.h @@ -0,0 +1,223 @@ +#ifndef CMVR_ES_ARM_TYPES_H +#define CMVR_ES_ARM_TYPES_H + +#include +#include +#include + +namespace cmvr::device { + +enum class ArmErrorCode { + OK = 0, + NotConnected, + AlreadyConnected, + ConnectionFailed, + Timeout, + InvalidArgument, + InvalidDof, + OutOfJointLimit, + OutOfVelocityLimit, + OutOfAccelerationLimit, + OutOfWorkspace, + RobotNotReady, + RobotNotPowered, + RobotInFault, + RobotInProtectiveStop, + RobotInEmergencyStop, + CommandRejected, + CommandFailed, + UnsupportedCommand, + UnknownError +}; + +struct Result { + ArmErrorCode code{ArmErrorCode::OK}; + std::string message{"OK"}; + + bool ok() const { return code == ArmErrorCode::OK; } + static Result success() { return {ArmErrorCode::OK, "OK"}; } + static Result failure(ArmErrorCode c, const std::string& msg) { return {c, msg}; } +}; + +enum class FrameType { + Base, + Tool, + World, + User +}; + +struct CartesianPose { + double x{0.0}; + double y{0.0}; + double z{0.0}; + double rx{0.0}; + double ry{0.0}; + double rz{0.0}; +}; + +struct CartesianVelocity { + double vx{0.0}; + double vy{0.0}; + double vz{0.0}; + double wx{0.0}; + double wy{0.0}; + double wz{0.0}; +}; + +struct CartesianWrench { + double fx{0.0}; + double fy{0.0}; + double fz{0.0}; + double tx{0.0}; + double ty{0.0}; + double tz{0.0}; +}; + +struct JointLimit { + double lower{0.0}; + double upper{0.0}; + double max_velocity{0.0}; + double max_acceleration{0.0}; + double max_torque{0.0}; +}; + +struct RobotModel { + std::string name; + std::string manufacturer; + std::string serial_number; + std::size_t dof{0}; + std::vector joint_names; + std::vector joint_limits; + double max_tcp_speed{0.0}; + double max_tcp_acceleration{0.0}; + double max_payload{0.0}; + + bool valid() const + { + return dof > 0 && joint_names.size() == dof && + (joint_limits.empty() || joint_limits.size() == dof); + } +}; + +struct JointGroupState { + std::vector position; + std::vector velocity; + std::vector effort; + + bool validForModel(const RobotModel& model) const + { + return position.size() == model.dof && + velocity.size() == model.dof && + (effort.empty() || effort.size() == model.dof); + } +}; + +struct JointPositionCommand { + std::vector position; + + bool validForModel(const RobotModel& model) const + { + return position.size() == model.dof; + } +}; + +struct JointVelocityCommand { + std::vector velocity; + + bool validForModel(const RobotModel& model) const + { + return velocity.size() == model.dof; + } +}; + +struct JointTorqueCommand { + std::vector torque; + + bool validForModel(const RobotModel& model) const + { + return torque.size() == model.dof; + } +}; + +struct ToolConfig { + CartesianPose tcp_offset; +}; + +struct PayloadConfig { + double mass{0.0}; + double cog_x{0.0}; + double cog_y{0.0}; + double cog_z{0.0}; +}; + +struct MotionOptions { + double velocity{0.0}; + double acceleration{0.0}; + double blend_radius{0.0}; + double jerk{5.0}; + std::vector joint_velocity_limits; + bool asynchronous{false}; +}; + +struct ServoOptions { + double period{0.008}; + double lookahead_time{0.1}; + double gain{300.0}; +}; + +enum class RobotMode { + Unknown = 0, + Disconnected, + PowerOff, + Idle, + Running, + Paused, + Stopped, + Fault +}; + +enum class SafetyMode { + Unknown = 0, + Normal, + Reduced, + ProtectiveStop, + EmergencyStop, + SafeguardStop, + SystemEmergencyStop, + Fault +}; + +enum class ControlMode { + None = 0, + Manual, + Position, + Velocity, + Torque, + Servo, + Freedrive +}; + +struct ArmState { + double timestamp{0.0}; + RobotMode robot_mode{RobotMode::Unknown}; + SafetyMode safety_mode{SafetyMode::Unknown}; + ControlMode control_mode{ControlMode::None}; + bool connected{false}; + bool powered_on{false}; + bool brake_released{false}; + bool moving{false}; + bool program_running{false}; + bool protective_stopped{false}; + bool emergency_stopped{false}; + bool fault{false}; + double speed_scaling{1.0}; + JointGroupState actual_joint_state; + JointGroupState target_joint_state; + CartesianPose actual_tcp_pose; + CartesianVelocity actual_tcp_velocity; + CartesianWrench actual_tcp_wrench; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_ARM_TYPES_H diff --git a/cmvr-es/utils/math/include/geometry.h b/cmvr-es/common/types/geometry_types.h similarity index 97% rename from cmvr-es/utils/math/include/geometry.h rename to cmvr-es/common/types/geometry_types.h index f530b8d8..e1378bf6 100644 --- a/cmvr-es/utils/math/include/geometry.h +++ b/cmvr-es/common/types/geometry_types.h @@ -54,4 +54,4 @@ namespace cmvr::math { } -#endif //GEOMETRY_H \ No newline at end of file +#endif //GEOMETRY_H diff --git a/cmvr-es/common/utils/config_helper/include/config_helper.h b/cmvr-es/common/utils/config_helper/include/config_helper.h deleted file mode 100644 index 7d189128..00000000 --- a/cmvr-es/common/utils/config_helper/include/config_helper.h +++ /dev/null @@ -1,163 +0,0 @@ -// -// Created by cmvr on 2026/1/16. -// -#pragma once -#include "common/utils/config_helper/include/config_setting.h" -#include "common/utils/io/proto_message_utils.h" -#include "cmvr/config/pinocchio_qp_ik_solver_config.pb.h" -#include "cmvr/config/camera_config/camera_config.pb.h" -#include "cmvr/config/dexhand_config/dexhand_config.pb.h" -#include "cmvr/config/microphone_config/microphone_config.pb.h" -#include "cmvr/config/speaker_config/speaker_conifg.pb.h" -#include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h" - -#define GET_CONFIG(file, para) \ - ([&]() -> bool { \ - LOG(INFO) << "FLAGS_" #file " = " << FLAGS_##file; \ - return getConfig(FLAGS_##file, (para)); \ - }()) - -#define GET_CONFIG_BIN(file, para) \ - ([&]() -> bool { \ - LOG(INFO) << "FLAGS_" #file " = " << FLAGS_##file; \ - return getConfigBin(FLAGS_##file, (para)); \ - }()) - -#define SET_CONFIG_BIN(para, file) \ - ([&]() -> bool { \ - LOG(INFO) << "FLAGS_" #file " = " << FLAGS_##file; \ - return setConfigBin((para), FLAGS_##file); \ - }()) - -#define SET_CONFIG(para, file) \ - ([&]() -> bool { \ - LOG(INFO) << "FLAGS_" #file " = " << FLAGS_##file; \ - return setConfig((para), FLAGS_##file); \ - }()) - -namespace cmvr -{ - class ConfigHelper - { - public: - - static bool getPinocchioQpIkSolverConfig(config::PinocchioQpIKConfig& config) - { - - return GET_CONFIG(pinocchio_qp_ik_solver_config_file, config); - } - - static bool setPinocchioQpIkSolverConfig(const config::PinocchioQpIKConfig& config) - { - return SET_CONFIG(config, pinocchio_qp_ik_solver_config_file); - } - - static bool getCamerasConfig(config::CameraConfig& config) - { - return GET_CONFIG(camera_config_file, config); - } - - static bool getDexHandsConfig(config::DexHandConfig& config) - { - return GET_CONFIG(dexhand_config_file, config); - } - - static bool getMicroPhonesConfig(config::MicroPhoneConfig& config) - { - return GET_CONFIG(microphone_config_file, config); - } - - static bool getSpeakersConfig(config::SpeakerConfig& config) - { - return GET_CONFIG(speaker_config_file, config); - } - - static bool getTouchScreenAppConfig(config::TouchScreenAppConfig& config) - { - return GET_CONFIG(touch_screen_app_config_file, config); - } - - static bool setTouchScreenAppConfig(const config::TouchScreenAppConfig& config) - { - return SET_CONFIG(config, touch_screen_app_config_file); - } - - private: - // Make macros able to call these (macros call ::cmvr::ConfigHelper::xxx) - template - static bool getConfigBin(const std::string& file_name, T& message) - { - LOG(INFO) << "file_name = " << file_name; - if (file_name.empty()) - { - LOG(ERROR) << "Empty file name."; - return false; - } - - bool ok = ProtoMessageIo::getProtoFromBinaryFile(file_name, &message); - if (!ok) - { - LOG(ERROR) << "Failed to load binary proto config from: " << file_name; - return false; - } - return true; - } - - template - static bool setConfig(const T& message, const std::string& file_name) - { - LOG(INFO) << "file_name = " << file_name; - if (file_name.empty()) - { - LOG(ERROR) << "Empty file name."; - return false; - } - - bool ok = ProtoMessageIo::setProtoToAsciiFile(message, file_name); - if (!ok) - { - LOG(ERROR) << "Failed to write ASCII proto config to: " << file_name; - return false; - } - return true; - } - - template - static bool setConfigBin(const T& message, const std::string& file_name) - { - LOG(INFO) << "file_name = " << file_name; - if (file_name.empty()) - { - LOG(ERROR) << "Empty file name."; - return false; - } - - bool ok = ProtoMessageIo::setProtoToBinaryFile(message, file_name); - if (!ok) - { - LOG(ERROR) << "Failed to write binary proto config to: " << file_name; - return false; - } - return true; - } - - template - static bool getConfig(const std::string& file_name, T& message) - { - // LOG(INFO) << "file_name = " << file_name; - if (file_name.empty()) - { - LOG(ERROR) << "Empty file name."; - return false; - } - - bool ok = ProtoMessageIo::getProtoFromAsciiFile(file_name, &message); - if (!ok) - { - LOG(ERROR) << "Failed to load ASCII proto config from: " << file_name; - return false; - } - return true; - } - }; -} // namespace cmvr diff --git a/cmvr-es/common/utils/config_helper/include/config_setting.h b/cmvr-es/common/utils/config_helper/include/config_setting.h deleted file mode 100644 index 5d58fb9e..00000000 --- a/cmvr-es/common/utils/config_helper/include/config_setting.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once -#include "gflags/gflags.h" -DECLARE_string(pinocchio_qp_ik_solver_config_file); -DECLARE_string(camera_config_file); -DECLARE_string(dexhand_config_file); -DECLARE_string(microphone_config_file); -DECLARE_string(speaker_config_file); -DECLARE_string(touch_screen_app_config_file); diff --git a/cmvr-es/common/utils/config_helper/src/config_setting.cpp b/cmvr-es/common/utils/config_helper/src/config_setting.cpp deleted file mode 100644 index 9e44092d..00000000 --- a/cmvr-es/common/utils/config_helper/src/config_setting.cpp +++ /dev/null @@ -1,90 +0,0 @@ -#include "common/utils/config_helper/include/config_setting.h" - -#include -#include -#include -#include - -namespace { - -std::string normalizeDirectory(std::filesystem::path path) { - auto text = path.lexically_normal().string(); - if (!text.empty() && text.back() != '/') { - text.push_back('/'); - } - return text; -} - -std::string probeConfigDirectory(std::filesystem::path start) { - static const std::array candidates = { - std::filesystem::path("output/bin/config"), - std::filesystem::path("cmvr-es/common/config"), - std::filesystem::path("config"), - std::filesystem::path("common/config") - }; - - start = start.lexically_normal(); - while (!start.empty()) { - for (const auto& candidate : candidates) { - const auto path = start / candidate; - if (std::filesystem::exists(path) && std::filesystem::is_directory(path)) { - return normalizeDirectory(path); - } - } - - const auto parent = start.parent_path(); - if (parent == start) { - break; - } - start = parent; - } - return {}; -} - -std::filesystem::path executableDirectory() { - char exe_path[PATH_MAX] = {0}; - const auto count = ::readlink("/proc/self/exe", exe_path, sizeof(exe_path) - 1); - if (count <= 0) { - return {}; - } - exe_path[count] = '\0'; - return std::filesystem::path(exe_path).parent_path(); -} - -} // namespace - -static std::string basePath() { - if (const auto resolved = probeConfigDirectory(executableDirectory()); !resolved.empty()) { - return resolved; - } - - if (const auto resolved = probeConfigDirectory(std::filesystem::current_path()); !resolved.empty()) { - return resolved; - } - - return "config/"; -} - -DEFINE_string(pinocchio_qp_ik_solver_config_file, - basePath() + "ik_solver_config/pinocchio_qp_ik_solver_config.pb.txt", - "The configuration file for pinocchio qp ik solver"); - -DEFINE_string(camera_config_file, - basePath() + "camera_config/camera_config.pb.txt", - "The configuration file for cameras"); - -DEFINE_string(dexhand_config_file, - basePath() + "dexhand_config/dexhand_config.pb.txt", - "The configuration file for dexhands"); - -DEFINE_string(microphone_config_file, - basePath() + "microphone_config/microphone_config.pb.txt", - "The configuration file for microphone"); - -DEFINE_string(speaker_config_file, - basePath() + "speaker_config/speaker_config.pb.txt", - "The configuration file for speaker"); - -DEFINE_string(touch_screen_app_config_file, - basePath() + "touch_screen_app_config/touch_screen_app_config.pb.txt", - "The configuration file for TouchScreenApp"); diff --git a/cmvr-es/common/utils/visualization/image_display.h b/cmvr-es/common/vision/image_display.h similarity index 100% rename from cmvr-es/common/utils/visualization/image_display.h rename to cmvr-es/common/vision/image_display.h diff --git a/cmvr-es/common/utils/image/image_process.h b/cmvr-es/common/vision/image_projection.h similarity index 100% rename from cmvr-es/common/utils/image/image_process.h rename to cmvr-es/common/vision/image_projection.h diff --git a/cmvr-es/common/utils/visualization/matplotlibcpp.h b/cmvr-es/common/vision/matplotlibcpp.h similarity index 100% rename from cmvr-es/common/utils/visualization/matplotlibcpp.h rename to cmvr-es/common/vision/matplotlibcpp.h diff --git a/cmvr-es/config/cmvr_es.pb.txt b/cmvr-es/config/cmvr_es.pb.txt new file mode 100644 index 00000000..f5674fb8 --- /dev/null +++ b/cmvr-es/config/cmvr_es.pb.txt @@ -0,0 +1,5 @@ +cmvr_es { + logger_config_file: "logger/logger.pb.txt" + device_manager_config_file: "manager/device_manager.pb.txt" + task_manager_config_file: "manager/task_manager.pb.txt" +} diff --git a/cmvr-es/config/devices/agv/agv.pb.txt b/cmvr-es/config/devices/agv/agv.pb.txt new file mode 100644 index 00000000..45881d7a --- /dev/null +++ b/cmvr-es/config/devices/agv/agv.pb.txt @@ -0,0 +1,9 @@ +agv { + agvs { + id: "agv_1" + my_agv { + ip: "127.0.0.1" + port: 8080 + } + } +} diff --git a/cmvr-es/config/devices/arm/arm.pb.txt b/cmvr-es/config/devices/arm/arm.pb.txt new file mode 100644 index 00000000..147f1a1f --- /dev/null +++ b/cmvr-es/config/devices/arm/arm.pb.txt @@ -0,0 +1,124 @@ +arm { + robot_arms { + id: "right_arm" + + motor { + motor_system_id: "ti5_motors" + motor_group_ids: "right_arm_can" + dof: 7 + joint_names: "R_SHOULDER_P" + joint_names: "R_SHOULDER_R" + joint_names: "R_SHOULDER_Y" + joint_names: "R_ELBOW_R" + joint_names: "R_WRIST_P" + joint_names: "R_WRIST_Y" + joint_names: "R_WRIST_R" + upd_freq: 1000 + buffer_size: 50 + default_vel: 1.0 + default_acc: 2.0 + } + + kinematics { + pinocchio_dls_ik_solver { + urdf_path: "model/xiaoyan_description/dual_arm.urdf" + base_frame_name: "PELVIS_S" + flange_frame_name: "R_WRIST_R_S" + tcp_frame_name: "R_FINGER_TIP_FIXED" + max_iters: 100 + pos_eps: 1e-6 + rot_eps: 1e-6 + damping: 1e-6 + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + joint_limit_avoidance { + enable: false + gain: 0.2 + margin_ratio: 0.15 + max_push: 0.25 + } + } + } + + motion { + move_j { + toppra_joint_motion_planner { + path_type: TOPPRA_PATH_TYPE_QUINTIC + sample_period_s: 0.001 + grid_size: 150 + high_grid_size: 300 + } + } + + move_l { + pinocchio_qp_cartesian_motion_planner { + sample_period_s: 0.001 + position_gain: 4.0 + rotation_gain: 4.0 + qp { + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } + qdot_regularization: 1e-4 + prev_qdot_regularization: 1e-4 + solver_eps: 1e-3 + joint_limit_avoidance { + enable: false + margin_ratio: 0.15 + gain: 0.2 + max_push: 0.25 + weight: 0.05 + } + } + } + } + + speed_l { + pinocchio_qp_cartesian_motion_planner { + qp { + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } + qdot_regularization: 1e-4 + prev_qdot_regularization: 2e-2 + solver_eps: 1e-3 + joint_limit_avoidance { + enable: false + margin_ratio: 0.15 + gain: 0.2 + max_push: 0.25 + weight: 0.05 + } + } + linear_velocity_max: 0.55 + linear_acceleration_max: 5.0 + linear_jerk_max: 10.0 + angular_velocity_max: 1.0 + angular_acceleration_max: 5.0 + angular_jerk_max: 12.0 + joint_acceleration_max: 8.0 + linear_target_replan_threshold: 1e-4 + angular_target_replan_threshold: 1e-4 + linear_reverse_cos_threshold: -0.8660254037844386 + linear_reverse_switch_speed_threshold: 1e-3 + normal_direction_deviation_deg: 10.0 + mild_direction_deviation_deg: 25.0 + severe_direction_deviation_deg: 45.0 + linear_min_speed_ratio: 0.2 + } + + speed_l_controller { + cartesian_velocity_controller { + control_period_s: 0.001 + stop_twist_norm: 1e-9 + stop_command_velocity_norm: 1e-3 + stop_measured_velocity_norm: 1e-2 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/arm/arm_dls_ik_dls_motion.pb.txt b/cmvr-es/config/devices/arm/arm_dls_ik_dls_motion.pb.txt new file mode 100644 index 00000000..8cf2cdb4 --- /dev/null +++ b/cmvr-es/config/devices/arm/arm_dls_ik_dls_motion.pb.txt @@ -0,0 +1,92 @@ +arm { + robot_arms { + id: "right_arm" + + motor { + motor_system_id: "ti5_motors" + motor_group_ids: "right_arm_can" + dof: 7 + joint_names: "R_SHOULDER_P" + joint_names: "R_SHOULDER_R" + joint_names: "R_SHOULDER_Y" + joint_names: "R_ELBOW_R" + joint_names: "R_WRIST_P" + joint_names: "R_WRIST_Y" + joint_names: "R_WRIST_R" + upd_freq: 1000 + buffer_size: 50 + default_vel: 1.0 + default_acc: 2.0 + } + + kinematics { + pinocchio_dls_ik_solver { + urdf_path: "model/xiaoyan_description/dual_arm.urdf" + base_frame_name: "PELVIS_S" + flange_frame_name: "R_WRIST_R_S" + tcp_frame_name: "R_FINGER_TIP_FIXED" + max_iters: 100 + pos_eps: 1e-6 + rot_eps: 1e-6 + damping: 1e-6 + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + joint_limit_avoidance { + enable: false + gain: 0.2 + margin_ratio: 0.15 + max_push: 0.25 + } + } + } + + motion { + move_j { + toppra_joint_motion_planner { + path_type: TOPPRA_PATH_TYPE_QUINTIC + sample_period_s: 0.001 + grid_size: 150 + high_grid_size: 300 + } + } + + move_l { + pinocchio_dls_cartesian_motion_planner { + sample_period_s: 0.001 + position_gain: 4.0 + rotation_gain: 4.0 + } + } + + speed_l { + pinocchio_dls_cartesian_motion_planner { + linear_velocity_max: 0.55 + linear_acceleration_max: 5.0 + linear_jerk_max: 10.0 + angular_velocity_max: 1.0 + angular_acceleration_max: 5.0 + angular_jerk_max: 12.0 + joint_acceleration_max: 8.0 + linear_target_replan_threshold: 1e-4 + angular_target_replan_threshold: 1e-4 + linear_reverse_cos_threshold: -0.8660254037844386 + linear_reverse_switch_speed_threshold: 1e-3 + normal_direction_deviation_deg: 10.0 + mild_direction_deviation_deg: 25.0 + severe_direction_deviation_deg: 45.0 + linear_min_speed_ratio: 0.2 + } + + speed_l_controller { + cartesian_velocity_controller { + control_period_s: 0.001 + stop_twist_norm: 1e-9 + stop_command_velocity_norm: 1e-3 + stop_measured_velocity_norm: 1e-2 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/arm/arm_dls_ik_qp_motion.pb.txt b/cmvr-es/config/devices/arm/arm_dls_ik_qp_motion.pb.txt new file mode 100644 index 00000000..9943fd8a --- /dev/null +++ b/cmvr-es/config/devices/arm/arm_dls_ik_qp_motion.pb.txt @@ -0,0 +1,138 @@ +arm { + robot_arms { + id: "right_arm" + + motor { + motor_system_id: "ti5_motors" + motor_group_ids: "right_arm_can" + dof: 7 + joint_names: "R_SHOULDER_P" + joint_names: "R_SHOULDER_R" + joint_names: "R_SHOULDER_Y" + joint_names: "R_ELBOW_R" + joint_names: "R_WRIST_P" + joint_names: "R_WRIST_Y" + joint_names: "R_WRIST_R" + upd_freq: 1000 + buffer_size: 50 + default_vel: 1.0 + default_acc: 2.0 + } + + kinematics { + pinocchio_dls_ik_solver { + urdf_path: "model/xiaoyan_description/dual_arm.urdf" + base_frame_name: "PELVIS_S" + flange_frame_name: "R_WRIST_R_S" + tcp_frame_name: "R_FINGER_TIP_FIXED" + max_iters: 100 + pos_eps: 1e-6 + rot_eps: 1e-6 + damping: 1e-6 + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + joint_limit_avoidance { + enable: false + gain: 0.2 + margin_ratio: 0.15 + max_push: 0.25 + } + } + } + + motion { + move_j { + toppra_joint_motion_planner { + path_type: TOPPRA_PATH_TYPE_QUINTIC + sample_period_s: 0.001 + grid_size: 150 + high_grid_size: 300 + } + } + + move_l { + pinocchio_qp_cartesian_motion_planner { + sample_period_s: 0.001 + position_gain: 4.0 + rotation_gain: 4.0 + qp { + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + twist_tracking_weight { + x: 1.0 + y: 1.0 + z: 1.0 + rx: 0.5 + ry: 0.5 + rz: 0.5 + } + qdot_regularization: 1e-4 + prev_qdot_regularization: 1e-4 + solver_eps: 1e-3 + joint_limit_avoidance { + enable: false + margin_ratio: 0.15 + gain: 0.2 + max_push: 0.25 + weight: 0.05 + } + } + } + } + + speed_l { + pinocchio_qp_cartesian_motion_planner { + qp { + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + twist_tracking_weight { + x: 1.0 + y: 1.0 + z: 1.0 + rx: 0.5 + ry: 0.5 + rz: 0.5 + } + qdot_regularization: 1e-4 + prev_qdot_regularization: 2e-2 + solver_eps: 1e-3 + joint_limit_avoidance { + enable: false + margin_ratio: 0.15 + gain: 0.2 + max_push: 0.25 + weight: 0.05 + } + } + linear_velocity_max: 0.55 + linear_acceleration_max: 5.0 + linear_jerk_max: 10.0 + angular_velocity_max: 1.0 + angular_acceleration_max: 5.0 + angular_jerk_max: 12.0 + joint_acceleration_max: 8.0 + linear_target_replan_threshold: 1e-4 + angular_target_replan_threshold: 1e-4 + linear_reverse_cos_threshold: -0.8660254037844386 + linear_reverse_switch_speed_threshold: 1e-3 + normal_direction_deviation_deg: 10.0 + mild_direction_deviation_deg: 25.0 + severe_direction_deviation_deg: 45.0 + linear_min_speed_ratio: 0.2 + } + + speed_l_controller { + cartesian_velocity_controller { + control_period_s: 0.001 + stop_twist_norm: 1e-9 + stop_command_velocity_norm: 1e-3 + stop_measured_velocity_norm: 1e-2 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/arm/arm_mujoco.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco.pb.txt new file mode 100644 index 00000000..a9cd606f --- /dev/null +++ b/cmvr-es/config/devices/arm/arm_mujoco.pb.txt @@ -0,0 +1,124 @@ +arm { + robot_arms { + id: "right_arm_mujoco" + + motor { + motor_system_id: "mujoco_motors" + motor_group_ids: "right_arm_mujoco" + dof: 7 + joint_names: "R_SHOULDER_P" + joint_names: "R_SHOULDER_R" + joint_names: "R_SHOULDER_Y" + joint_names: "R_ELBOW_R" + joint_names: "R_WRIST_P" + joint_names: "R_WRIST_Y" + joint_names: "R_WRIST_R" + upd_freq: 1000 + buffer_size: 50 + default_vel: 1.0 + default_acc: 2.0 + } + + kinematics { + pinocchio_dls_ik_solver { + urdf_path: "model/xiaoyan_description/dual_arm.urdf" + base_frame_name: "PELVIS_S" + flange_frame_name: "R_WRIST_R_S" + tcp_frame_name: "R_FINGER_TIP_FIXED" + max_iters: 100 + pos_eps: 1e-6 + rot_eps: 1e-6 + damping: 1e-6 + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + joint_limit_avoidance { + enable: true + gain: 0.2 + margin_ratio: 0.15 + max_push: 0.25 + } + } + } + + motion { + move_j { + toppra_joint_motion_planner { + path_type: TOPPRA_PATH_TYPE_QUINTIC + sample_period_s: 0.001 + grid_size: 150 + high_grid_size: 300 + } + } + + move_l { + pinocchio_qp_cartesian_motion_planner { + sample_period_s: 0.001 + position_gain: 4.0 + rotation_gain: 4.0 + qp { + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } + qdot_regularization: 1e-4 + prev_qdot_regularization: 1e-4 + solver_eps: 1e-3 + joint_limit_avoidance { + enable: false + margin_ratio: 0.15 + gain: 0.2 + max_push: 0.25 + weight: 0.05 + } + } + } + } + + speed_l { + pinocchio_qp_cartesian_motion_planner { + qp { + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } + qdot_regularization: 1e-4 + prev_qdot_regularization: 2e-2 + solver_eps: 1e-3 + joint_limit_avoidance { + enable: false + margin_ratio: 0.15 + gain: 0.2 + max_push: 0.25 + weight: 0.05 + } + } + linear_velocity_max: 0.55 + linear_acceleration_max: 5.0 + linear_jerk_max: 10.0 + angular_velocity_max: 1.0 + angular_acceleration_max: 5.0 + angular_jerk_max: 12.0 + joint_acceleration_max: 8.0 + linear_target_replan_threshold: 1e-4 + angular_target_replan_threshold: 1e-4 + linear_reverse_cos_threshold: -0.8660254037844386 + linear_reverse_switch_speed_threshold: 1e-3 + normal_direction_deviation_deg: 10.0 + mild_direction_deviation_deg: 25.0 + severe_direction_deviation_deg: 45.0 + linear_min_speed_ratio: 0.2 + } + + speed_l_controller { + cartesian_velocity_controller { + control_period_s: 0.001 + stop_twist_norm: 1e-9 + stop_command_velocity_norm: 1e-3 + stop_measured_velocity_norm: 1e-2 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/arm/arm_mujoco_dls_ik_dls_motion.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco_dls_ik_dls_motion.pb.txt new file mode 100644 index 00000000..1d7ea131 --- /dev/null +++ b/cmvr-es/config/devices/arm/arm_mujoco_dls_ik_dls_motion.pb.txt @@ -0,0 +1,92 @@ +arm { + robot_arms { + id: "right_arm_mujoco" + + motor { + motor_system_id: "mujoco_motors" + motor_group_ids: "right_arm_mujoco" + dof: 7 + joint_names: "R_SHOULDER_P" + joint_names: "R_SHOULDER_R" + joint_names: "R_SHOULDER_Y" + joint_names: "R_ELBOW_R" + joint_names: "R_WRIST_P" + joint_names: "R_WRIST_Y" + joint_names: "R_WRIST_R" + upd_freq: 1000 + buffer_size: 50 + default_vel: 1.0 + default_acc: 2.0 + } + + kinematics { + pinocchio_dls_ik_solver { + urdf_path: "model/xiaoyan_description/dual_arm.urdf" + base_frame_name: "PELVIS_S" + flange_frame_name: "R_WRIST_R_S" + tcp_frame_name: "R_FINGER_TIP_FIXED" + max_iters: 100 + pos_eps: 1e-6 + rot_eps: 1e-6 + damping: 1e-6 + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + joint_limit_avoidance { + enable: false + gain: 0.2 + margin_ratio: 0.15 + max_push: 0.25 + } + } + } + + motion { + move_j { + toppra_joint_motion_planner { + path_type: TOPPRA_PATH_TYPE_QUINTIC + sample_period_s: 0.001 + grid_size: 150 + high_grid_size: 300 + } + } + + move_l { + pinocchio_dls_cartesian_motion_planner { + sample_period_s: 0.001 + position_gain: 4.0 + rotation_gain: 4.0 + } + } + + speed_l { + pinocchio_dls_cartesian_motion_planner { + linear_velocity_max: 0.55 + linear_acceleration_max: 5.0 + linear_jerk_max: 10.0 + angular_velocity_max: 1.0 + angular_acceleration_max: 5.0 + angular_jerk_max: 12.0 + joint_acceleration_max: 8.0 + linear_target_replan_threshold: 1e-4 + angular_target_replan_threshold: 1e-4 + linear_reverse_cos_threshold: -0.8660254037844386 + linear_reverse_switch_speed_threshold: 1e-3 + normal_direction_deviation_deg: 10.0 + mild_direction_deviation_deg: 25.0 + severe_direction_deviation_deg: 45.0 + linear_min_speed_ratio: 0.2 + } + + speed_l_controller { + cartesian_velocity_controller { + control_period_s: 0.001 + stop_twist_norm: 1e-9 + stop_command_velocity_norm: 1e-3 + stop_measured_velocity_norm: 1e-2 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/arm/arm_mujoco_dls_ik_qp_motion.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco_dls_ik_qp_motion.pb.txt new file mode 100644 index 00000000..6c1f310e --- /dev/null +++ b/cmvr-es/config/devices/arm/arm_mujoco_dls_ik_qp_motion.pb.txt @@ -0,0 +1,120 @@ +arm { + robot_arms { + id: "right_arm_mujoco" + + motor { + motor_system_id: "mujoco_motors" + motor_group_ids: "right_arm_mujoco" + dof: 7 + joint_names: "R_SHOULDER_P" + joint_names: "R_SHOULDER_R" + joint_names: "R_SHOULDER_Y" + joint_names: "R_ELBOW_R" + joint_names: "R_WRIST_P" + joint_names: "R_WRIST_Y" + joint_names: "R_WRIST_R" + upd_freq: 1000 + buffer_size: 50 + default_vel: 1.0 + default_acc: 2.0 + } + + kinematics { + pinocchio_dls_ik_solver { + urdf_path: "model/xiaoyan_description/dual_arm.urdf" + base_frame_name: "PELVIS_S" + flange_frame_name: "R_WRIST_R_S" + tcp_frame_name: "R_FINGER_TIP_FIXED" + max_iters: 100 + pos_eps: 1e-6 + rot_eps: 1e-6 + damping: 1e-6 + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + joint_limit_avoidance { + enable: false + gain: 0.2 + margin_ratio: 0.15 + max_push: 0.25 + } + } + } + + motion { + move_j { + toppra_joint_motion_planner { + path_type: TOPPRA_PATH_TYPE_QUINTIC + sample_period_s: 0.001 + grid_size: 150 + high_grid_size: 300 + } + } + + move_l { + pinocchio_qp_cartesian_motion_planner { + sample_period_s: 0.001 + position_gain: 4.0 + rotation_gain: 4.0 + qp { + joint_limits { source: JOINT_LIMIT_SOURCE_URDF } + twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } + qdot_regularization: 1e-4 + prev_qdot_regularization: 1e-4 + solver_eps: 1e-3 + joint_limit_avoidance { + enable: false + margin_ratio: 0.15 + gain: 0.2 + max_push: 0.25 + weight: 0.05 + } + } + } + } + + speed_l { + pinocchio_qp_cartesian_motion_planner { + qp { + joint_limits { source: JOINT_LIMIT_SOURCE_URDF } + twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } + qdot_regularization: 1e-4 + prev_qdot_regularization: 2e-2 + solver_eps: 1e-3 + joint_limit_avoidance { + enable: false + margin_ratio: 0.15 + gain: 0.2 + max_push: 0.25 + weight: 0.05 + } + } + linear_velocity_max: 0.55 + linear_acceleration_max: 5.0 + linear_jerk_max: 10.0 + angular_velocity_max: 1.0 + angular_acceleration_max: 5.0 + angular_jerk_max: 12.0 + joint_acceleration_max: 8.0 + linear_target_replan_threshold: 1e-4 + angular_target_replan_threshold: 1e-4 + linear_reverse_cos_threshold: -0.8660254037844386 + linear_reverse_switch_speed_threshold: 1e-3 + normal_direction_deviation_deg: 10.0 + mild_direction_deviation_deg: 25.0 + severe_direction_deviation_deg: 45.0 + linear_min_speed_ratio: 0.2 + } + + speed_l_controller { + cartesian_velocity_controller { + control_period_s: 0.001 + stop_twist_norm: 1e-9 + stop_command_velocity_norm: 1e-3 + stop_measured_velocity_norm: 1e-2 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/arm/arm_mujoco_qp_ik_qp_motion.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco_qp_ik_qp_motion.pb.txt new file mode 100644 index 00000000..12ec39f9 --- /dev/null +++ b/cmvr-es/config/devices/arm/arm_mujoco_qp_ik_qp_motion.pb.txt @@ -0,0 +1,112 @@ +arm { + robot_arms { + id: "right_arm_mujoco" + + motor { + motor_system_id: "mujoco_motors" + motor_group_ids: "right_arm_mujoco" + dof: 7 + joint_names: "R_SHOULDER_P" + joint_names: "R_SHOULDER_R" + joint_names: "R_SHOULDER_Y" + joint_names: "R_ELBOW_R" + joint_names: "R_WRIST_P" + joint_names: "R_WRIST_Y" + joint_names: "R_WRIST_R" + upd_freq: 1000 + buffer_size: 50 + default_vel: 1.0 + default_acc: 2.0 + } + + kinematics { + pinocchio_qp_ik_solver { + urdf_path: "model/xiaoyan_description/dual_arm.urdf" + base_frame_name: "PELVIS_S" + flange_frame_name: "R_WRIST_R_S" + tcp_frame_name: "R_FINGER_TIP_FIXED" + lambda: 1e-4 + w_posrot: 0.5 + max_iters: 100 + tol: 1e-6 + qp_time_limit: 1e-2 + } + } + + motion { + move_j { + toppra_joint_motion_planner { + path_type: TOPPRA_PATH_TYPE_QUINTIC + sample_period_s: 0.001 + grid_size: 150 + high_grid_size: 300 + } + } + + move_l { + pinocchio_qp_cartesian_motion_planner { + sample_period_s: 0.001 + position_gain: 4.0 + rotation_gain: 4.0 + qp { + joint_limits { source: JOINT_LIMIT_SOURCE_URDF } + twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } + qdot_regularization: 1e-4 + prev_qdot_regularization: 1e-4 + solver_eps: 1e-3 + joint_limit_avoidance { + enable: false + margin_ratio: 0.15 + gain: 0.2 + max_push: 0.25 + weight: 0.05 + } + } + } + } + + speed_l { + pinocchio_qp_cartesian_motion_planner { + qp { + joint_limits { source: JOINT_LIMIT_SOURCE_URDF } + twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } + qdot_regularization: 1e-4 + prev_qdot_regularization: 2e-2 + solver_eps: 1e-3 + joint_limit_avoidance { + enable: false + margin_ratio: 0.15 + gain: 0.2 + max_push: 0.25 + weight: 0.05 + } + } + linear_velocity_max: 0.55 + linear_acceleration_max: 5.0 + linear_jerk_max: 10.0 + angular_velocity_max: 1.0 + angular_acceleration_max: 5.0 + angular_jerk_max: 12.0 + joint_acceleration_max: 8.0 + linear_target_replan_threshold: 1e-4 + angular_target_replan_threshold: 1e-4 + linear_reverse_cos_threshold: -0.8660254037844386 + linear_reverse_switch_speed_threshold: 1e-3 + normal_direction_deviation_deg: 10.0 + mild_direction_deviation_deg: 25.0 + severe_direction_deviation_deg: 45.0 + linear_min_speed_ratio: 0.2 + } + + speed_l_controller { + cartesian_velocity_controller { + control_period_s: 0.001 + stop_twist_norm: 1e-9 + stop_command_velocity_norm: 1e-3 + stop_measured_velocity_norm: 1e-2 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/arm/arm_qp_ik_qp_motion.pb.txt b/cmvr-es/config/devices/arm/arm_qp_ik_qp_motion.pb.txt new file mode 100644 index 00000000..e8f7dd20 --- /dev/null +++ b/cmvr-es/config/devices/arm/arm_qp_ik_qp_motion.pb.txt @@ -0,0 +1,130 @@ +arm { + robot_arms { + id: "right_arm" + + motor { + motor_system_id: "ti5_motors" + motor_group_ids: "right_arm_can" + dof: 7 + joint_names: "R_SHOULDER_P" + joint_names: "R_SHOULDER_R" + joint_names: "R_SHOULDER_Y" + joint_names: "R_ELBOW_R" + joint_names: "R_WRIST_P" + joint_names: "R_WRIST_Y" + joint_names: "R_WRIST_R" + upd_freq: 1000 + buffer_size: 50 + default_vel: 1.0 + default_acc: 2.0 + } + + kinematics { + pinocchio_qp_ik_solver { + urdf_path: "model/xiaoyan_description/dual_arm.urdf" + base_frame_name: "PELVIS_S" + flange_frame_name: "R_WRIST_R_S" + tcp_frame_name: "R_FINGER_TIP_FIXED" + lambda: 1e-4 + w_posrot: 0.5 + max_iters: 100 + tol: 1e-6 + qp_time_limit: 1e-2 + } + } + + motion { + move_j { + toppra_joint_motion_planner { + path_type: TOPPRA_PATH_TYPE_QUINTIC + sample_period_s: 0.001 + grid_size: 150 + high_grid_size: 300 + } + } + + move_l { + pinocchio_qp_cartesian_motion_planner { + sample_period_s: 0.001 + position_gain: 4.0 + rotation_gain: 4.0 + qp { + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + twist_tracking_weight { + x: 1.0 + y: 1.0 + z: 1.0 + rx: 0.5 + ry: 0.5 + rz: 0.5 + } + qdot_regularization: 1e-4 + prev_qdot_regularization: 1e-4 + solver_eps: 1e-3 + joint_limit_avoidance { + enable: false + margin_ratio: 0.15 + gain: 0.2 + max_push: 0.25 + weight: 0.05 + } + } + } + } + + speed_l { + pinocchio_qp_cartesian_motion_planner { + qp { + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + twist_tracking_weight { + x: 1.0 + y: 1.0 + z: 1.0 + rx: 0.5 + ry: 0.5 + rz: 0.5 + } + qdot_regularization: 1e-4 + prev_qdot_regularization: 2e-2 + solver_eps: 1e-3 + joint_limit_avoidance { + enable: false + margin_ratio: 0.15 + gain: 0.2 + max_push: 0.25 + weight: 0.05 + } + } + linear_velocity_max: 0.55 + linear_acceleration_max: 5.0 + linear_jerk_max: 10.0 + angular_velocity_max: 1.0 + angular_acceleration_max: 5.0 + angular_jerk_max: 12.0 + joint_acceleration_max: 8.0 + linear_target_replan_threshold: 1e-4 + angular_target_replan_threshold: 1e-4 + linear_reverse_cos_threshold: -0.8660254037844386 + linear_reverse_switch_speed_threshold: 1e-3 + normal_direction_deviation_deg: 10.0 + mild_direction_deviation_deg: 25.0 + severe_direction_deviation_deg: 45.0 + linear_min_speed_ratio: 0.2 + } + + speed_l_controller { + cartesian_velocity_controller { + control_period_s: 0.001 + stop_twist_norm: 1e-9 + stop_command_velocity_norm: 1e-3 + stop_measured_velocity_norm: 1e-2 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/arm/aubo_arm.pb.txt b/cmvr-es/config/devices/arm/aubo_arm.pb.txt new file mode 100644 index 00000000..b5696f9c --- /dev/null +++ b/cmvr-es/config/devices/arm/aubo_arm.pb.txt @@ -0,0 +1,22 @@ +arm { + robot_arms { + id: "aubo_arm" + + vendor { + brand: VENDOR_ROBOT_ARM_BRAND_AUBO_ARM + ip: "192.168.1.100" + port: 30004 + dof: 6 + joint_names: "joint_1" + joint_names: "joint_2" + joint_names: "joint_3" + joint_names: "joint_4" + joint_names: "joint_5" + joint_names: "joint_6" + base_frame: "base" + tool_frame: "tool0" + username: "aubo" + password: "123456" + } + } +} diff --git a/cmvr-es/config/devices/biohead/bio_head.pb.txt b/cmvr-es/config/devices/biohead/bio_head.pb.txt new file mode 100644 index 00000000..a47bf15e --- /dev/null +++ b/cmvr-es/config/devices/biohead/bio_head.pb.txt @@ -0,0 +1,86 @@ +bio_head { + id: "bio_head" + serial_port: "/dev/ttyUSB0" + baud_rate: 115200 + + servo_groups { + name: "eyebrow" + address: 64 + channel_start: 0 + channel_end: 3 + offset: 90 + offset: 90 + offset: 90 + offset: 90 + min_angles: 20 + min_angles: 77 + min_angles: 90 + min_angles: 20 + max_angles: 90 + max_angles: 170 + max_angles: 155 + max_angles: 110 + } + + servo_groups { + name: "eye" + address: 64 + channel_start: 4 + channel_end: 9 + offset: 90 + offset: 90 + offset: 90 + offset: 90 + offset: 90 + offset: 90 + min_angles: 20 + min_angles: 90 + min_angles: 90 + min_angles: 25 + min_angles: 70 + min_angles: 75 + max_angles: 90 + max_angles: 150 + max_angles: 165 + max_angles: 90 + max_angles: 120 + max_angles: 115 + } + + servo_groups { + name: "mouth" + address: 65 + channel_start: 0 + channel_end: 9 + offset: 90 + offset: 90 + offset: 90 + offset: 90 + offset: 90 + offset: 90 + offset: 90 + offset: 90 + offset: 90 + offset: 90 + min_angles: 70 + min_angles: 30 + min_angles: 80 + min_angles: 80 + min_angles: 65 + min_angles: 55 + min_angles: 45 + min_angles: 80 + min_angles: 85 + min_angles: 90 + max_angles: 150 + max_angles: 110 + max_angles: 130 + max_angles: 140 + max_angles: 100 + max_angles: 105 + max_angles: 110 + max_angles: 125 + max_angles: 90 + max_angles: 95 + } +} diff --git a/cmvr-es/config/devices/camera/camera.pb.txt b/cmvr-es/config/devices/camera/camera.pb.txt new file mode 100644 index 00000000..2647e5d9 --- /dev/null +++ b/cmvr-es/config/devices/camera/camera.pb.txt @@ -0,0 +1,81 @@ +camera { + cameras { + id: "cam1" + realsense { + serialNumber: "243122074587" + width: 640 + height: 480 + fps: 30 + codec: "H265" + camera_mode: CAMERA_MODE_VIDEO + stream_mode: STREAM_MODE_RGBD + align_mode: ALIGN_MODE_COLOR + buffer_size: 30 + sync: false + } + } + + cameras { + id: "right_hand_cam" + realsense { + serialNumber: "243122072252" + width: 1280 + height: 720 + encode_width: 640 + encode_height: 360 + fps: 30 + codec: "H264" + camera_mode: CAMERA_MODE_VIDEO + stream_mode: STREAM_MODE_RGB + align_mode: ALIGN_MODE_COLOR + buffer_size: 30 + sync: false + } + } + + cameras { + id: "cam3" + realsense { + serialNumber: "243122075614" + width: 640 + height: 480 + fps: 30 + codec: "H265" + camera_mode: CAMERA_MODE_VIDEO + stream_mode: STREAM_MODE_RGBD + align_mode: ALIGN_MODE_COLOR + buffer_size: 30 + sync: false + } + } + + cameras { + id: "left_eye_cam" + uvc { + usb: "/dev/uvc_left_camera" + width: 640 + height: 480 + fps: 30 + codec: "H265" + camera_mode: CAMERA_MODE_VIDEO + stream_mode: STREAM_MODE_RGB + buffer_size: 30 + } + } + + cameras { + id: "cam5" + realsense { + serialNumber: "243122075389" + width: 640 + height: 480 + fps: 30 + codec: "H265" + camera_mode: CAMERA_MODE_VIDEO + stream_mode: STREAM_MODE_RGBD + align_mode: ALIGN_MODE_COLOR + buffer_size: 30 + sync: false + } + } +} diff --git a/cmvr-es/config/devices/dexhand/dexhand.pb.txt b/cmvr-es/config/devices/dexhand/dexhand.pb.txt new file mode 100644 index 00000000..10e2f9f6 --- /dev/null +++ b/cmvr-es/config/devices/dexhand/dexhand.pb.txt @@ -0,0 +1,41 @@ +dexhand { + dexhands { + id: "hand1" + rh56dftp { + ip: "192.168.1.213" + port: 6000 + poll_interval_ms: 10 + } + } + + dexhands { + id: "hand2" + rh56dftp { + ip: "192.168.1.224" + port: 6000 + poll_interval_ms: 10 + } + } + + dexhands { + id: "paxini_tip_1" + px_6ax_gen3 { + serial_port: "/dev/ttyACM1" + sensor_model: "S1813_core" + module_id: 2 + baud_rate: 921600 + distributed_length: 153 + resultant_length: 3 + poll_interval_ms: 5 + response_timeout_ms: 200 + response_header_bytes: 14 + tactile_rows: 1 + tactile_cols: 51 + tactile_finger: "INDEX" + tactile_region: "TIP" + sensor_name: "Paxini Gen3末端压力" + polling_read_mode: PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE + auto_calibrate: false + } + } +} diff --git a/cmvr-es/config/devices/microphone/microphone.pb.txt b/cmvr-es/config/devices/microphone/microphone.pb.txt new file mode 100644 index 00000000..03c0ea1b --- /dev/null +++ b/cmvr-es/config/devices/microphone/microphone.pb.txt @@ -0,0 +1,11 @@ +microphone { + microphones { + id: "mic1" + ffmpeg { + channels: 2 + sampleRate: 44100 + volume: 100 + input_device: "default" + } + } +} diff --git a/cmvr-es/config/devices/motor/mujoco_motors.pb.txt b/cmvr-es/config/devices/motor/mujoco_motors.pb.txt new file mode 100644 index 00000000..2a885ac4 --- /dev/null +++ b/cmvr-es/config/devices/motor/mujoco_motors.pb.txt @@ -0,0 +1,17 @@ +motor { + id: "mujoco_motors" + + motor_groups { + id: "right_arm_mujoco" + bus_type: MOTOR_BUS_MUJOCO + tool_frame: "R_FINGER_TIP" + + motors { id: 1 joint_name: "R_SHOULDER_P" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 } + motors { id: 2 joint_name: "R_SHOULDER_R" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 } + motors { id: 3 joint_name: "R_SHOULDER_Y" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 } + motors { id: 4 joint_name: "R_ELBOW_R" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 } + motors { id: 5 joint_name: "R_WRIST_P" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 } + motors { id: 6 joint_name: "R_WRIST_Y" limit_q_lb: -1.102 limit_q_ub: 1.02 limit_qd: 3.0 } + motors { id: 7 joint_name: "R_WRIST_R" limit_q_lb: -0.293 limit_q_ub: 1.57079 limit_qd: 3.0 } + } +} diff --git a/cmvr-es/config/devices/motor/ti5_motors.pb.txt b/cmvr-es/config/devices/motor/ti5_motors.pb.txt new file mode 100644 index 00000000..872446dc --- /dev/null +++ b/cmvr-es/config/devices/motor/ti5_motors.pb.txt @@ -0,0 +1,75 @@ +motor { + id: "ti5_motors" + + motor_groups { + id: "left_arm_can" + bus_type: MOTOR_BUS_CAN + tool_frame: "L_FINGER_TIP" + can { + channel_id: 0 + } + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + joint_limits_urdf_path: "model/xiaoyan_description/dual_arm.urdf" + motors { id: 23 joint_name: "L_SHOULDER_P" } + motors { id: 24 joint_name: "L_SHOULDER_R" } + motors { id: 25 joint_name: "L_SHOULDER_Y" } + motors { id: 26 joint_name: "L_ELBOW_R" } + motors { id: 27 joint_name: "L_WRIST_P" } + motors { id: 28 joint_name: "L_WRIST_Y" } + motors { id: 29 joint_name: "L_WRIST_R" } + } + + motor_groups { + id: "right_arm_can" + bus_type: MOTOR_BUS_CAN + tool_frame: "R_FINGER_TIP" + can { + channel_id: 1 + } + joint_limits { + source: JOINT_LIMIT_SOURCE_URDF + } + joint_limits_urdf_path: "model/xiaoyan_description/dual_arm.urdf" + motors { id: 16 joint_name: "R_SHOULDER_P" } + motors { id: 17 joint_name: "R_SHOULDER_R" } + motors { id: 18 joint_name: "R_SHOULDER_Y" } + motors { id: 19 joint_name: "R_ELBOW_R" } + motors { id: 20 joint_name: "R_WRIST_P" } + motors { id: 21 joint_name: "R_WRIST_Y" } + motors { id: 22 joint_name: "R_WRIST_R" } + } + + motor_groups { + id: "head_can" + bus_type: MOTOR_BUS_CAN + can { + channel_id: 2 + } + joint_limits { + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "HEAD_Y" lower: -3.14 upper: 3.14 velocity: 3.0 } + joints { joint_name: "HEAD_P" lower: -3.14 upper: 3.14 velocity: 3.0 } + joints { joint_name: "HEAD_R" lower: -3.14 upper: 3.14 velocity: 3.0 } + } + motors { id: 32 joint_name: "HEAD_Y" } + motors { id: 30 joint_name: "HEAD_P" } + motors { id: 31 joint_name: "HEAD_R" } + } + + motor_groups { + id: "waist_can" + bus_type: MOTOR_BUS_CAN + can { + channel_id: 3 + } + joint_limits { + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "WAIST_Y" lower: -3.14 upper: 3.14 velocity: 3.0 } + joints { joint_name: "WAIST_P" lower: -3.14 upper: 3.14 velocity: 3.0 } + } + motors { id: 4 joint_name: "WAIST_Y" } + motors { id: 15 joint_name: "WAIST_P" } + } +} diff --git a/cmvr-es/config/devices/speaker/speaker.pb.txt b/cmvr-es/config/devices/speaker/speaker.pb.txt new file mode 100644 index 00000000..18568d23 --- /dev/null +++ b/cmvr-es/config/devices/speaker/speaker.pb.txt @@ -0,0 +1,7 @@ +speaker { + speakers { + id: "spk1" + ffmpeg { + } + } +} diff --git a/cmvr-es/config/logger/logger.pb.txt b/cmvr-es/config/logger/logger.pb.txt new file mode 100644 index 00000000..9ce39ef2 --- /dev/null +++ b/cmvr-es/config/logger/logger.pb.txt @@ -0,0 +1,38 @@ +logger { + minimum_level: LOG_LEVEL_DEBUG + + routes { + level: LOG_LEVEL_DEBUG + file: true + terminal: true + } + routes { + level: LOG_LEVEL_INFO + terminal: true + } + routes { + level: LOG_LEVEL_WARNING + terminal: true + file: true + } + routes { + level: LOG_LEVEL_ERROR + terminal: true + file: true + } + routes { + level: LOG_LEVEL_FATAL + terminal: true + file: true + } + + directory: "../log" + max_file_size_mb: 100 + flush_interval_seconds: 1 + format { + show_time: false + show_level: true + show_thread_id: false + show_source_location: true + } +} diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt new file mode 100644 index 00000000..3a838edb --- /dev/null +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -0,0 +1,61 @@ +device_manager { + name: "cmvr_es" + version: "0.1" + description: "cmvr edge system version 0.1" + + devices { + id: "right_hand_cam" + type: DEVICE_TYPE_CAMERA + config_file: "devices/camera/camera.pb.txt" + enable: true + } + + devices { + id: "hand2" + type: DEVICE_TYPE_DEXHAND + config_file: "devices/dexhand/dexhand.pb.txt" + enable: true + } + + devices { + id: "paxini_tip_1" + type: DEVICE_TYPE_DEXHAND + config_file: "devices/dexhand/dexhand.pb.txt" + enable: true + } + + devices { + id: "ti5_motors" + type: DEVICE_TYPE_MOTOR_SYSTEM + config_file: "devices/motor/ti5_motors.pb.txt" + enable: true + } + + devices { + id: "right_arm" + type: DEVICE_TYPE_ROBOT_ARM + config_file: "devices/arm/arm.pb.txt" + enable: true + } + + devices { + id: "aubo_arm" + type: DEVICE_TYPE_ROBOT_ARM + config_file: "devices/arm/aubo_arm.pb.txt" + enable: true + } + + devices { + id: "bio_head" + type: DEVICE_TYPE_BIO_HEAD_ROBOT + config_file: "devices/biohead/bio_head.pb.txt" + enable: true + } + + devices { + id: "agv_1" + type: DEVICE_TYPE_AGV + config_file: "devices/agv/agv.pb.txt" + enable: true + } +} diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt new file mode 100644 index 00000000..b8cfcc76 --- /dev/null +++ b/cmvr-es/config/manager/task_manager.pb.txt @@ -0,0 +1,17 @@ +task_manager { + tasks { + id: "touch_screen" + type: TASK_TYPE_TOUCH_SCREEN + run_mode: TASK_RUN_MODE_PERIODIC_STEP + control_period_s: 0.001 + config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt" + enable: true + } + tasks { + id: "grpc_server" + type: TASK_TYPE_GRPC_SERVER + run_mode: TASK_RUN_MODE_BLOCKING_SERVICE + config_file: "tasks/grpc_server_task/grpc_server_task.pb.txt" + enable: true + } +} diff --git a/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt b/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt new file mode 100644 index 00000000..86a70d57 --- /dev/null +++ b/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt @@ -0,0 +1,6 @@ +grpc_server { + id: "grpc_server" + host: "0.0.0.0" + port: "50052" + enable_reflection: true +} 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 new file mode 100644 index 00000000..e1ddec0c --- /dev/null +++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt @@ -0,0 +1,101 @@ +touch_screen_task { + id: "touch_screen" + + devices { + arm_id: "right_arm" + dexhand_id: "paxini_tip_1" + camera_id: "right_hand_cam" + } + + initialization { + before_start: true + after_finish: true + joint_positions { joint_name: "R_SHOULDER_P" rad: -0.3678 } + joint_positions { joint_name: "R_SHOULDER_R" rad: 1.1127 } + joint_positions { joint_name: "R_SHOULDER_Y" rad: 1.6084 } + joint_positions { joint_name: "R_ELBOW_R" rad: 1.61 } + joint_positions { joint_name: "R_WRIST_P" rad: -2.5718 } + joint_positions { joint_name: "R_WRIST_Y" rad: 0.1276 } + joint_positions { joint_name: "R_WRIST_R" rad: 0.1297 } + velocity: 1.0 + acceleration: 2.0 + } + + perception { + apriltag { + tag_size_m: 0.012 + depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE + target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE + } + } + + alignment { + ibvs { + camera_link: "R_CAM" + lambda: 0.4 + mu: 0.1 + qdot_max: 1.0 + 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 + m20: 0.0 m21: 0.0 m22: 1.0 + } + r_camera_to_urdf { + m00: 1.0 m01: 0.0 m02: 0.0 + m10: 0.0 m11: 1.0 m12: 0.0 + m20: 0.0 m21: 0.0 m22: 1.0 + } + control_joint_names: "R_SHOULDER_P" + control_joint_names: "R_SHOULDER_R" + control_joint_names: "R_SHOULDER_Y" + control_joint_names: "R_ELBOW_R" + control_joint_names: "R_WRIST_P" + control_joint_names: "R_WRIST_Y" + control_joint_names: "R_WRIST_R" + } + target { + position_in_camera { x: -0.001 y: 0.08 z: 0.15 } + rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 } + mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION + } + error_threshold { + x: 0.005 + y: 0.005 + z: 0.01 + rx: 0.1026646259971647 + ry: 0.1026646259971647 + rz: 0.1026646259971647 + } + stable_frames: 2 + timeout_s: 20.0 + pause_when_reached: false + } + + touch { + speed_l { + twist_tool { x: 0.0 y: -0.04 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } + acceleration: 6.0 + max_distance_m: 0.035 + } + tactile { + finger: TOUCH_SCREEN_FINGER_TYPE_INDEX + region: TOUCH_SCREEN_TACTILE_REGION_TIP + criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ + force_threshold: 1.0 + } + dwell_time_s: 0.0 + } + + retract { + twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } + acceleration: 8.0 + duration_s: 0.45 + } +} 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 new file mode 100644 index 00000000..b106ad9d --- /dev/null +++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt @@ -0,0 +1,101 @@ +touch_screen_task { + id: "touch_screen" + + devices { + arm_id: "right_arm_mujoco" + dexhand_id: "mujoco_zero_touch_dexhand" + camera_id: "hand_cam" + } + + initialization { + before_start: true + after_finish: true + joint_positions { joint_name: "R_SHOULDER_P" rad: -0.2423 } + joint_positions { joint_name: "R_SHOULDER_R" rad: 1.2929 } + joint_positions { joint_name: "R_SHOULDER_Y" rad: 1.61 } + joint_positions { joint_name: "R_ELBOW_R" rad: 1.58 } + joint_positions { joint_name: "R_WRIST_P" rad: -2.8792 } + joint_positions { joint_name: "R_WRIST_Y" rad: 0.1150 } + joint_positions { joint_name: "R_WRIST_R" rad: -0.08 } + velocity: 2.8 + acceleration: 20.0 + } + + perception { + apriltag { + tag_size_m: 0.12 + depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE + target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE + } + } + + alignment { + ibvs { + camera_link: "R_CAM" + lambda: 0.4 + mu: 0.1 + qdot_max: 0.8 + 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 + m20: 0.0 m21: 0.0 m22: -1.0 + } + r_camera_to_urdf { + m00: 1.0 m01: 0.0 m02: 0.0 + m10: 0.0 m11: -1.0 m12: 0.0 + m20: 0.0 m21: 0.0 m22: -1.0 + } + control_joint_names: "R_SHOULDER_P" + control_joint_names: "R_SHOULDER_R" + control_joint_names: "R_SHOULDER_Y" + control_joint_names: "R_ELBOW_R" + control_joint_names: "R_WRIST_P" + control_joint_names: "R_WRIST_Y" + control_joint_names: "R_WRIST_R" + } + target { + position_in_camera { x: 0.0 y: 0.0 z: 0.30 } + rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 } + mode: TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY + } + error_threshold { + x: 0.005 + y: 0.005 + z: 0.010 + rx: 0.08726646259971647 + ry: 0.08726646259971647 + rz: 0.08726646259971647 + } + stable_frames: 5 + timeout_s: 20.0 + pause_when_reached: false + } + + touch { + speed_l { + twist_tool { x: 0.0 y: -0.04 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } + acceleration: 6.0 + max_distance_m: 0.12 + } + tactile { + finger: TOUCH_SCREEN_FINGER_TYPE_INDEX + region: TOUCH_SCREEN_TACTILE_REGION_TIP + criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ + force_threshold: 1.0 + } + dwell_time_s: 0.0 + } + + retract { + twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } + acceleration: 8.0 + duration_s: 5.0 + } +} diff --git a/cmvr-es/controller/include/touch_controller.h b/cmvr-es/controller/include/touch_controller.h deleted file mode 100644 index b9377577..00000000 --- a/cmvr-es/controller/include/touch_controller.h +++ /dev/null @@ -1,114 +0,0 @@ -// -// Created by lgv on 2025/8/24. -// - -#pragma once - -#include - -#include "../../devices/camera/abstract_camera.h" -#include "../../devices/dexhand/abstract_dexhand.h" -#include "../../devices/robot/abstract_robot.h" -#include "cmvr/msgs/geometry.pb.h" -#include "librealsense2/rs.h" -#include "librealsense2/h/rs_frame.h" -namespace cmvr { - namespace ctrl { - - // 简易版 PID 控制器，带死区、积分限幅、输出限幅和输出斜率限制 - class PID { - public: - PID(double kp, double ki, double kd, - double i_max, - double output_max_pos, double output_max_neg, - double delta_max = 0.0) // 输出变化最大值，0 表示不限制 - : kp_(kp), ki_(ki), kd_(kd), - i_max_(i_max), - output_max_pos_(output_max_pos), - output_max_neg_(output_max_neg), - delta_max_(delta_max), - prev_error_(0), integral_(0), prev_output_(0) {} - - double compute(double target, double current, double dt, double deadband = 0.0) { - double error = target - current; - - // 死区处理 - if (fabs(error) <= deadband) { - error = 0.0; - } - - // 积分累加限幅 - integral_ += error * dt; - if (integral_ > i_max_) integral_ = i_max_; - if (integral_ < -i_max_) integral_ = -i_max_; - - // 微分 - double derivative = (error - prev_error_) / dt; - prev_error_ = error; - - // PID 输出 - double output = kp_ * error + ki_ * integral_ + kd_ * derivative; - - // 输出限幅 - if (output > output_max_pos_) output = output_max_pos_; - if (output < -output_max_neg_) output = -output_max_neg_; - - // 输出斜率限制 - if (delta_max_ > 0.0) { - double delta = output - prev_output_; - if (delta > delta_max_) output = prev_output_ + delta_max_; - else if (delta < -delta_max_) output = prev_output_ - delta_max_; - } - - prev_output_ = output; - return output; - } - - private: - double kp_, ki_, kd_; - double prev_error_; - double integral_; - double i_max_; // 积分限幅 - double output_max_pos_; // 向下按的最大输出 - double output_max_neg_; // 向上抬的最大输出 - double delta_max_; // 输出斜率限制 - double prev_output_; - }; - - - - - class TouchController { - public: - TouchController() {}; - TouchController(std::shared_ptr robot,std::shared_ptr hand,std::shared_ptr cam) - :robot_(std::move(robot)),hand_(std::move(hand)),cam_(std::move(cam)), - pid_(std::make_shared(0.005, 0.001, 0.001, 5000.0, 0.5, 1.0)){} - ~TouchController()=default; - - - bool isArrive(double max_force); - - void touch(int u,int v ,double max_force); - - void touch(std::shared_ptr robot,const msgs::Pose3d pose,const msgs::Pose3d offset); - void touch( msgs::Pose3d pose, msgs::Pose3d offset,double max_force); - - private: - std::shared_ptr robot_{nullptr}; - std::shared_ptr hand_{nullptr}; - std::shared_ptr cam_{nullptr}; - - std::shared_ptr pid_{nullptr}; - - const double touch_threshold_ = 5.0; // 触控判定阈值 - - - // 从压阻矩阵提取触控点与压力 - bool extractTouch(const std::vector>& matrix,double& force, int& x, int& y); - - - - }; - } -} diff --git a/cmvr-es/data_center/CMakeLists.txt b/cmvr-es/data_center/CMakeLists.txt deleted file mode 100644 index d0fda580..00000000 --- a/cmvr-es/data_center/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -add_library(data_center STATIC - src/data_center.cpp - src/motors_info.cpp -) - -target_include_directories(data_center PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -add_library(cmvr_es::data_center ALIAS data_center) - -target_link_libraries(data_center PRIVATE - cmvr_es::utils - cmvr_es::device::canbus - cmvr_es::device::ti5motor - protobuf - glog -) - - -install(TARGETS data_center LIBRARY DESTINATION lib) \ No newline at end of file diff --git a/cmvr-es/data_center/include/data_center.h b/cmvr-es/data_center/include/data_center.h deleted file mode 100644 index 9375e932..00000000 --- a/cmvr-es/data_center/include/data_center.h +++ /dev/null @@ -1,31 +0,0 @@ -// -// Created by lgv on 11/10/25. -// - -#pragma once -#include "data_center/include/motors_info.h" - -namespace cmvr { - class DataCenter { - private: - DataCenter(); - DataCenter(const DataCenter &) = delete; - DataCenter &operator=(const DataCenter &) = delete; - - public: - static DataCenter *getInstance() - { - static DataCenter instance; - return &instance; - } - ~DataCenter() {} - - public: - const MotorsInfo* getMotorsInfo() const { - return motors_info_; - } - - private: - MotorsInfo* motors_info_{nullptr}; - }; -} diff --git a/cmvr-es/data_center/include/motors_info.h b/cmvr-es/data_center/include/motors_info.h deleted file mode 100644 index 56c2de04..00000000 --- a/cmvr-es/data_center/include/motors_info.h +++ /dev/null @@ -1,85 +0,0 @@ -// -// Created by lgv on 11/10/25. -// - -#ifndef CMVR_ES_MOTORS_INFO_H -#define CMVR_ES_MOTORS_INFO_H - -#include "../../devices/canbus/abstract_canbus.h" -#include "canbus/can_comm/can_sender.h" -#include "canbus/can_comm/can_receiver.h" -#include "canbus/can_comm/message_manager.h" -#include "cmvr/msgs/robot_detail.pb.h" -#include "motor/motor_manager.h" - -namespace cmvr { - class MotorsInfo { - private: - MotorsInfo(); - - MotorsInfo(const MotorsInfo &) = delete; - - MotorsInfo &operator=(const MotorsInfo &) = delete; - - public: - ~MotorsInfo(){}; - - void init(const XmlNode &cfg); - - static MotorsInfo *getInstance() { - static MotorsInfo instance; - return &instance; - } - - bool getJointsQ(const std::vector &joints_name,std::unordered_map &joint_qs) const; - bool getJointsQ(const std::vector &joints_name,std::vector &joint_qs) const; - bool getJointsQ(std::unordered_map &joint_qs) const; - - std::shared_ptr getMotorManager() const { - return motor_manager_; - } - - - - - - private: - std::vector l_motors_cfg_{}; - std::vector r_motors_cfg_{}; - std::vector waist_motors_cfg_{}; - std::vector head_motors_cfg_{}; - - - std::shared_ptr l_can_client_{nullptr}; - std::shared_ptr r_can_client_{nullptr}; - std::shared_ptr waist_can_client_{nullptr}; - std::shared_ptr head_can_client_{nullptr}; - - std::shared_ptr> l_can_receiver_{nullptr}; - std::shared_ptr> r_can_receiver_{nullptr}; - std::shared_ptr> waist_can_receiver_{nullptr}; - std::shared_ptr> head_can_receiver_{nullptr}; - - std::shared_ptr> l_can_sender_{nullptr}; - std::shared_ptr> r_can_sender_{nullptr}; - std::shared_ptr> waist_can_sender_{nullptr}; - std::shared_ptr> head_can_sender_{nullptr}; - - std::shared_ptr> l_message_manager_{nullptr}; - std::shared_ptr> r_message_manager_{nullptr}; - std::shared_ptr> waist_message_manager_{nullptr}; - std::shared_ptr> head_message_manager_{nullptr}; - - bool waist_enabled_{false}; - bool right_arm_enabled_{false}; - bool left_arm_enabled_{false}; - bool head_enabled_{false}; - - std::shared_ptr motor_manager_{nullptr}; - - }; -} - - - -#endif //CMVR_ES_MOTORS_INFO_H diff --git a/cmvr-es/data_center/src/data_center.cpp b/cmvr-es/data_center/src/data_center.cpp deleted file mode 100644 index f5e532b2..00000000 --- a/cmvr-es/data_center/src/data_center.cpp +++ /dev/null @@ -1,13 +0,0 @@ -// -// Created by lgv on 11/10/25. -// - -#include "data_center/include/data_center.h" -using namespace cmvr; - - -DataCenter::DataCenter() -{ - motors_info_ = MotorsInfo::getInstance(); - -} \ No newline at end of file diff --git a/cmvr-es/data_center/src/motors_info.cpp b/cmvr-es/data_center/src/motors_info.cpp deleted file mode 100644 index 4b6a087c..00000000 --- a/cmvr-es/data_center/src/motors_info.cpp +++ /dev/null @@ -1,195 +0,0 @@ -// -// Created by lgv on 11/10/25. -// - -#include "data_center/include/motors_info.h" -#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" -#include "motor/ti5_motor/ti5_motor.h" - -using namespace cmvr; -using namespace cmvr::device; - -MotorsInfo::MotorsInfo() { -} - -void MotorsInfo::init(const XmlNode &cfg) { - // 定义 lambda 函数，用于读取 XML 节点 enable 属性 - auto readEnable = [](const XmlNode &node) -> bool { - std::string enable_str = node.getAttrString("enable"); // 默认 false - return (enable_str == "true" || enable_str == "1"); - }; - auto can_cfg = cfg.getChild("CanManger"); - - auto l_can_cfg = can_cfg.getChild("LeftArmCan"); - left_arm_enabled_ = readEnable(l_can_cfg); - - if (left_arm_enabled_) { - l_motors_cfg_ = l_can_cfg.getChildren("Motor"); - l_can_client_ = std::make_shared(l_can_cfg); - l_can_sender_ = std::make_shared >(); - l_can_receiver_ = std::make_shared >(); - l_message_manager_ = std::make_shared >(); - } - - - auto r_can_cfg = can_cfg.getChild("RightArmCan"); - right_arm_enabled_ = readEnable(r_can_cfg); - if (right_arm_enabled_) { - r_motors_cfg_ = r_can_cfg.getChildren("Motor"); - r_can_client_ = std::make_shared(r_can_cfg); - r_can_sender_ = std::make_shared >(); - r_can_receiver_ = std::make_shared >(); - r_message_manager_ = std::make_shared >(); - } - - - auto waist_can_cfg = can_cfg.getChild("WaistCan"); - waist_enabled_ = readEnable(waist_can_cfg); - if (waist_enabled_) { - waist_motors_cfg_ = waist_can_cfg.getChildren("Motor"); - waist_can_client_ = std::make_shared(waist_can_cfg); - waist_can_sender_ = std::make_shared >(); - waist_can_receiver_ = std::make_shared >(); - waist_message_manager_ = std::make_shared >(); - } - - auto head_can_cfg = can_cfg.getChild("HeadCan"); - head_enabled_ = readEnable(head_can_cfg); - if (head_enabled_) { - head_motors_cfg_ = head_can_cfg.getChildren("Motor"); - head_can_client_ = std::make_shared(head_can_cfg); - head_can_sender_ = std::make_shared >(); - head_can_receiver_ = std::make_shared >(); - head_message_manager_ = std::make_shared >(); - } - - // 开始初始化 - struct Limb { - std::string name; - bool enabled; - std::shared_ptr client; - std::shared_ptr > sender; - std::shared_ptr > receiver; - std::shared_ptr > message_manager; - std::vector motor_cfgs; - }; - - std::vector limbs{ - { - "WAIST", waist_enabled_, waist_can_client_, waist_can_sender_, waist_can_receiver_, waist_message_manager_, - waist_motors_cfg_ - }, - { - "LEFT_ARM", left_arm_enabled_, l_can_client_, l_can_sender_, l_can_receiver_, l_message_manager_, - l_motors_cfg_ - }, - { - "RIGHT_ARM", right_arm_enabled_, r_can_client_, r_can_sender_, r_can_receiver_, r_message_manager_, - r_motors_cfg_ - }, - { - "HEAD", head_enabled_, head_can_client_, head_can_sender_, head_can_receiver_, head_message_manager_, - head_motors_cfg_ - } - }; - - // 创建 MotorManager - motor_manager_ = std::make_shared(); - - std::vector > tasks; - - for (auto &limb: limbs) { - if (!limb.enabled) continue; - - - if (limb.client) limb.client->init(); - - // 2. 初始化 Sender / Receiver（如果有） - if (limb.sender && limb.receiver && limb.client) { - auto ret = limb.sender->Init(limb.client.get(), false); - if (ret != ErrorCode::OK) - LOG(ERROR) << "Failed to init " << limb.name << " CAN sender."; - - ret = limb.receiver->Init(limb.client.get(), limb.message_manager.get(), false); - if (ret != ErrorCode::OK) - LOG(ERROR) << "Failed to init " << limb.name << " CAN receiver."; - - limb.client->start(); - ret = limb.sender->Start(); - if (ret != ErrorCode::OK) - LOG(ERROR) << "Failed to start " << limb.name << " CAN sender."; - - ret = limb.receiver->Start(); - if (ret != ErrorCode::OK) - LOG(ERROR) << "Failed to start " << limb.name << " CAN receiver."; - } - - // 3. 创建协议（如果有CAN） - std::shared_ptr protocol = nullptr; - if (limb.sender && limb.message_manager) { - protocol = std::make_shared(limb.sender, limb.message_manager); - } - - // 4. 并行初始化电机 - if (!limb.motor_cfgs.empty()) { - tasks.push_back(std::async(std::launch::async, [this, protocol, &limb] { - LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing " << limb.name << - " motors..."; - for (const auto &cfg: limb.motor_cfgs) { - auto motor = std::make_shared(cfg); - if (protocol) motor->setProtocol(protocol); - motor->init(); - motor_manager_->addMotor(motor); - } - })); - } - } - - // 等待所有任务完成 - for (auto &task: tasks) task.get(); - - LOG(INFO) << "All enabled motors initialized successfully."; -} - -bool MotorsInfo::getJointsQ(const std::vector &joints_name, - std::unordered_map &joint_qs) const { - auto res{true}; - joint_qs.clear(); - for (auto &name: joints_name) { - auto motor = motor_manager_->getMotor(name); - if (motor) { - joint_qs.emplace(name, motor->getQ()); - } else { - joint_qs.emplace(name, std::numeric_limits::quiet_NaN()); - res = false; - } - } - return res; -} - -bool MotorsInfo::getJointsQ(const std::vector &joints_name, std::vector &joint_qs) const { - auto res{true}; - joint_qs.clear(); - for (auto &name: joints_name) { - auto motor = motor_manager_->getMotor(name); - if (motor) { - joint_qs.emplace_back(motor->getQ()); - } else { - joint_qs.emplace_back(std::numeric_limits::quiet_NaN()); - res = false; - } - } - return res; -} - -bool MotorsInfo::getJointsQ(std::unordered_map &joint_qs) const { - joint_qs.clear(); - auto motors = motor_manager_->motorsMap(); - for (auto &motor_pair: motors) { - auto motor = motor_pair.second; - joint_qs.emplace(motor->jointName(), motor->getQ()); - } - return true; - -} - diff --git a/cmvr-es/device_manager/include/device_factory.h b/cmvr-es/device_manager/include/device_factory.h deleted file mode 100644 index 1fcce739..00000000 --- a/cmvr-es/device_manager/include/device_factory.h +++ /dev/null @@ -1,44 +0,0 @@ -// -// Created by xtkuang on 2025/5/13. -// - -#ifndef CMVR_ES_DEVICE_FACTORY_H -#define CMVR_ES_DEVICE_FACTORY_H -#pragma once - -#include "../../devices/agv/abstract_agv.h" -#include "../../devices/battery/abstract_battery.h" -#include "../../devices/camera/abstract_camera.h" -#include "../../devices/dexhand/abstract_dexhand.h" -#include "../../devices/gripper/abstract_gripper.h" -#include "../../devices/robot/abstract_robot.h" -#include "../../devices/microphone/abstract_microphone.h" -#include "../../devices/speaker/abstract_speaker.h" -#include "../../devices/biohead/abstract_biohead.h" - -namespace cmvr::device { - - class DeviceFactory { - public: - DeviceFactory() = default; - - template - std::shared_ptr create(const XmlNode& cfg); - - template - std::shared_ptr createFromConfig(const ConfigType& cfg); - private: - std::shared_ptr create_agv_(const XmlNode& cfg); - std::shared_ptr create_battery_(const XmlNode& cfg); - std::shared_ptr create_camera_(const XmlNode& cfg); - std::shared_ptr create_dexhand_(const XmlNode& cfg); - std::shared_ptr create_gripper_(const XmlNode& cfg); - std::shared_ptr create_mic_(const XmlNode& cfg); - std::shared_ptr create_robot_(const XmlNode& cfg); - std::shared_ptr create_speaker_(const XmlNode& cfg); - std::shared_ptr create_biohead_(const XmlNode& cfg); - }; -} - - -#endif //CMVR_ES_DEVICE_FACTORY_H diff --git a/cmvr-es/device_manager/include/device_manager.h b/cmvr-es/device_manager/include/device_manager.h deleted file mode 100644 index ee782e5d..00000000 --- a/cmvr-es/device_manager/include/device_manager.h +++ /dev/null @@ -1,77 +0,0 @@ -// -// Created by xtkuang on 2025/5/6. -// - -#ifndef CMVR_ES_DEVICE_MANAGER_H -#define CMVR_ES_DEVICE_MANAGER_H - -#include -#include -#include -#include -#include "device_factory.h" -#include "system_monitor.h" -#include "../../utils/base/include/thread_pool.h" - -namespace cmvr::device { - - using DeviceVariant = std::variant< - std::shared_ptr, - std::shared_ptr, - std::shared_ptr, - std::shared_ptr, - std::shared_ptr, - std::shared_ptr, - std::shared_ptr, - std::shared_ptr, - std::shared_ptr - >; - - typedef struct { - std::string version; - std::string name; - std::string description; - std::string os; - std::string kernel_version; - std::string architecture; - } SystemInfo; - - class DeviceManager { - public: - DeviceManager(const DeviceManager&) = delete; - DeviceManager& operator=(const DeviceManager&) = delete; - - static DeviceManager& getInstance(const XmlNode &cfg); - static DeviceManager& getInstance(); - static void destroyInstance(); - - void start(); - void restart(); - void stop(); - - void updateParam(const std::string& id, const std::pair ¶m); - void getDeviceList(std::list> &device_list); - - void getSystemInfo(SystemInfo &info) const; - void getSystemStatus(device::SystemStatus &status) const; - - template - std::shared_ptr getDevice(const std::string &device_id); - - private: - static std::once_flag init_flag_; - static std::shared_ptr instance_; - - XmlNode cfg_; - SystemInfo info_; - std::unordered_map devices_; - std::unique_ptr dev_factory_; - std::unique_ptr sys_monitor_; - - explicit DeviceManager(const XmlNode &cfg); - void init_devices_(); - void get_os_info_(); - }; -} // cmvr - -#endif //CMVR_ES_DEVICE_MANAGER_H diff --git a/cmvr-es/device_manager/include/system_monitor.h b/cmvr-es/device_manager/include/system_monitor.h deleted file mode 100644 index f39a24b2..00000000 --- a/cmvr-es/device_manager/include/system_monitor.h +++ /dev/null @@ -1,45 +0,0 @@ -// -// Created by xtkuang on 2025/5/13. -// - -#ifndef CMVR_ES_MONITOR_H -#define CMVR_ES_MONITOR_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include "../../utils/base/include/timer.h" -#include "rapidxml/xml_parser.h" - -namespace cmvr::device { - struct SystemStatus { - float cpu_usage; - float used_memory; - float total_memory; - float used_disk; - float total_disk; - float cpu_temperature; - }; - - class SystemMonitor { - public: - SystemMonitor()=default; - ~SystemMonitor()=default; - void getStatus(SystemStatus &status); - private: - SystemStatus status_{}; - void get_cpu_(); - void get_memory_(); - void get_disk_(); - }; - -} - - - -#endif //CMVR_ES_MONITOR_H diff --git a/cmvr-es/device_manager/src/device_factory.cpp b/cmvr-es/device_manager/src/device_factory.cpp deleted file mode 100644 index fba92e51..00000000 --- a/cmvr-es/device_manager/src/device_factory.cpp +++ /dev/null @@ -1,255 +0,0 @@ -// -// Created by xtkuang on 2025/5/13. -// - -#include "device_manager/include/device_factory.h" -#include "devices/biohead/biohead_esp32/include/biohead_esp32.h" -#include "devices/camera/uvc_camera/include/uvc_camera.h" -//#include "devices/camera/mechmind/include/mechmind_camera.h" -#include "devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h" -#include "devices/microphone/ffmpeg_microphone/include/ffmpeg_microphone.h" -#include "devices/camera/realsense_camera/include/realsense_camera.h" -#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h" -#include "devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h" -#include "devices/robot/humanoid_robot/include/humanoid_robot.h" -//#include "robot/ti5_robot/ti5_robot.h" -#include "cmvr/api/system_command.pb.h" -#include "data_center/include/motors_info.h" -//#include "devices/robot/aubo_robot/include/aubo_robot.h" -using namespace std; -using namespace cmvr::device; - - -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); - -template -std::shared_ptr DeviceFactory::create(const XmlNode& cfg) { - if constexpr (std::is_same_v) { - return create_agv_(cfg); - } else if constexpr (std::is_same_v) { - return create_battery_(cfg); - } else if constexpr (std::is_same_v) { - return create_camera_(cfg); - } else if constexpr (std::is_same_v) { - return create_dexhand_(cfg); - } else if constexpr (std::is_same_v) { - return create_gripper_(cfg); - } else if constexpr (std::is_same_v) { - return create_mic_(cfg); - } else if constexpr (std::is_same_v) { - return create_robot_(cfg); - } else if constexpr (std::is_same_v) { - return create_speaker_(cfg); - }else if constexpr (std::is_same_v){ - return create_biohead_(cfg); - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type"; - throw runtime_error("[DeviceFactory]: Unsupported device type"); - } -} - -std::shared_ptr DeviceFactory::create_agv_(const XmlNode& cfg) { - return nullptr; -} - -std::shared_ptr DeviceFactory::create_battery_(const XmlNode& cfg) { - return nullptr; -} - -std::shared_ptr DeviceFactory::create_camera_(const XmlNode& cfg) { - try { - if (cfg.getNodeName() == "UVCCamera") { - return std::make_shared(cfg); - } - else if (cfg.getNodeName() == "MechMind") { - //return std::make_shared(cfg); - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - else if (cfg.getNodeName() == "RealsenseCamera") { - return std::make_shared(cfg); - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - } - catch (const exception &e) { - return nullptr; - } -} - -std::shared_ptr DeviceFactory::create_dexhand_(const XmlNode& cfg) { - try { - if (cfg.getNodeName() == "RH56DFTP") { - return std::make_shared(cfg); - } - else if (cfg.getNodeName() == "PX6AXGen3") { - return std::make_shared(cfg); - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - } - catch (const exception &e) { - return nullptr; - } -} - -std::shared_ptr DeviceFactory::create_gripper_(const XmlNode& cfg) { - return nullptr; -} - -std::shared_ptr DeviceFactory::create_mic_(const XmlNode& cfg) { - try { - if (cfg.getNodeName() == "ffmpegMicPhone") { - return std::make_shared(cfg); - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - } - catch (const exception &e) { - return nullptr; - } - return nullptr; -} - -std::shared_ptr DeviceFactory::create_robot_(const XmlNode& cfg) { - try { - if (cfg.getNodeName() == "Humanoid") { - auto motos_info = MotorsInfo::getInstance(); - motos_info->init(cfg); - return std::make_shared>(cfg); - } - else if (cfg.getNodeName() == "AuboRobot") { - //return std::make_shared>(cfg); - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - } - catch (const exception &e) { - return nullptr; - } -} - -std::shared_ptrDeviceFactory::create_biohead_(const XmlNode& cfg) { - try { - if (cfg.getNodeName() == "esp32") { - return std::make_shared(cfg); - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - } - catch (const exception &e) { - return nullptr; - } -} - -std::shared_ptr DeviceFactory::create_speaker_(const XmlNode& cfg) { - try { - if (cfg.getNodeName() == "ffmpegSpeaker") { - return std::make_shared(cfg); - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - } - catch (const exception &e) { - return nullptr; - } -} - -template std::shared_ptr DeviceFactory::createFromConfig(const config::UVCCameraConfig& cfg); -template std::shared_ptr DeviceFactory::createFromConfig(const config::RealSenseCameraConfig& cfg); -template std::shared_ptr DeviceFactory::createFromConfig(const config::RH56DFTPDexHandConfig& cfg); -template std::shared_ptr DeviceFactory::createFromConfig(const config::PX6AXGen3& cfg); -template std::shared_ptr DeviceFactory::createFromConfig(const config::FFMpegMicroPhoneConfig& cfg); -template std::shared_ptr DeviceFactory::createFromConfig(const config::FFMpegSpeakerConfig& cfg); -template -std::shared_ptr DeviceFactory::createFromConfig(const ConfigType& cfg) -{ - try { - if constexpr (std::is_same_v) - { - if constexpr (std::is_same_v) - { - return std::make_shared(cfg); - } - else if constexpr (std::is_same_v) - { - return std::make_shared(cfg); - } - else - { - LOG(ERROR) << "[DeviceFactory]: Unsupported camera device type "; - throw runtime_error("[DeviceFactory]: Unsupported camera device type"); - } - } - else if constexpr (std::is_same_v) - { - if constexpr (std::is_same_v) - { - return std::make_shared(cfg); - } - else if constexpr (std::is_same_v) - { - return std::make_shared(cfg); - } - else - { - LOG(ERROR) << "[DeviceFactory]: Unsupported dexhand device type "; - throw runtime_error("[DeviceFactory]: Unsupported dexhand device type"); - } - } - else if constexpr (std::is_same_v) - { - if constexpr (std::is_same_v) - { - return std::make_shared(cfg); - } - else - { - LOG(ERROR) << "[DeviceFactory]: Unsupported microphone device type "; - throw runtime_error("[DeviceFactory]: Unsupported microphone device type"); - } - } - else if constexpr (std::is_same_v) - { - if constexpr (std::is_same_v) - { - return std::make_shared(cfg); - } - else - { - LOG(ERROR) << "[DeviceFactory]: Unsupported microphone device type "; - throw runtime_error("[DeviceFactory]: Unsupported microphone device type"); - } - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type"; - throw runtime_error("[DeviceFactory]: Unsupported device type"); - } - } - catch (const exception &e) { - return nullptr; - } -} diff --git a/cmvr-es/device_manager/src/device_manager.cpp b/cmvr-es/device_manager/src/device_manager.cpp deleted file mode 100644 index 805516f9..00000000 --- a/cmvr-es/device_manager/src/device_manager.cpp +++ /dev/null @@ -1,390 +0,0 @@ -// -// Created by xtkuang on 2025/5/6. -// - -#include "../include/device_manager.h" - -#include "common/utils/config_helper/include/config_helper.h" -using namespace std; -using namespace cmvr::device; -using namespace cmvr::device; - -template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); -template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); -template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); -template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); -template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); -template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); -template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); -template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); -template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); - - -std::shared_ptr DeviceManager::instance_ = nullptr; -std::once_flag DeviceManager::init_flag_; - - -DeviceManager::DeviceManager(const XmlNode& cfg) { - try { - cfg_ = cfg; - info_.version = cfg_.getAttrDefault("ver", "1.0"); - info_.name = cfg_.getAttrDefault("name", "cmvr_es"); - info_.description = cfg_.getAttrDefault("description", "empty"); - get_os_info_(); - - dev_factory_ = std::make_unique(); - sys_monitor_ = std::make_unique(); - init_devices_(); - } - catch (const exception &e) { - LOG(FATAL) << "[DeviceManager] (DeviceManager): Device manager initialization failed: " << e.what(); - throw std::runtime_error("[DeviceManager] (DeviceManager): Device manager create failed" + string(e.what())); - } -} - -DeviceManager& DeviceManager::getInstance(const XmlNode& cfg) { - std::call_once(init_flag_, [&cfg] { - instance_.reset(new DeviceManager(cfg)); - }); - return *instance_; -} - -DeviceManager& DeviceManager::getInstance() { - if (!instance_) { - throw std::runtime_error("[DeviceManager] (getInstance): DeviceManager not initialized. Call getInstance(const XmlNode&) first."); - } - return *instance_; -} - -void DeviceManager::destroyInstance() { - instance_.reset(); -} - -void DeviceManager::start(){ - try { - for (auto& [id, variant] : devices_) { - std::visit([&](auto&& ptr) { - if (ptr) { - ptr->start(); // 调用各子类实现的 start() - LOG(INFO) << "[DeviceManager]: Start device " << id << " Success"; - } else { - LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id; - } - }, variant); - } - } - catch (const exception &e) { - LOG(ERROR) << "[DeviceManager]: " << e.what(); - throw std::runtime_error("[DeviceManager] (start): Device manager start failed: " + string(e.what())); - } -} - -void DeviceManager::restart() { - try { - stop(); - start(); - } - catch (const exception &e) { - throw runtime_error(e.what()); - } -} - -void DeviceManager::stop() { - try { - for (auto& [id, variant] : devices_) { - std::visit([&](auto&& ptr) { - if (ptr) { - ptr->stop(); // 调用各子类实现的 stop() - LOG(INFO) << "[DeviceManager]: Stop device " << id << " Success"; - } else { - LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id; - } - }, variant); - } - } - catch (const exception &e) { - LOG(ERROR) << "[DeviceManager]: " << e.what(); - throw std::runtime_error("[DeviceManager] (stop): Device manager start failed: " + string(e.what())); - } -} -std::shared_ptr getDeviceBasePtr(DeviceVariant& variant) { - return std::visit([](auto& device_ptr) -> std::shared_ptr { - // 利用多态，自动转换为基类指针 - return device_ptr; - }, variant); -} -void DeviceManager::updateParam(const std::string& id, const std::pair ¶m){ - auto it = devices_.find(id); - if (it == devices_.end()) { - LOG(WARNING) << "[DeviceManager]: Device ID " << id << " not found."; - throw runtime_error("[DeviceManager]: Device ID " + id + " not found."); - } - // 获取基类指针（无需知道具体类型） - std::shared_ptr base_dev = getDeviceBasePtr(it->second); - - // 直接调用基类中定义的通用接口 - base_dev->updateParams(param); // 调用配置接口 -} - - -template -std::shared_ptr DeviceManager::getDevice(const std::string& device_id) -{ - auto it = devices_.find(device_id); - if (it == devices_.end()) { - LOG(WARNING) << "[DeviceManager]: Device ID " << device_id << " not found."; - throw runtime_error("[DeviceManager]: Device ID " + device_id + " not found."); - } - - if (auto ptr = std::get_if>(&it->second)) { - return *ptr; - } - else { - LOG(WARNING) << "[DeviceManager]: Device ID " << device_id << " type mismatch."; - throw runtime_error("[DeviceManager]: Device ID " + device_id + " type mismatch."); - } -} - -void DeviceManager::getDeviceList(std::list>& device_list){ - try { - device_list.clear(); - for (const auto& [device_id, variant] : devices_) { - string dev_type; - std::visit([&](const auto& ptr) { - using T = std::decay_t; - if constexpr (std::is_same_v>) { - dev_type = "AGV"; - } else if constexpr (std::is_same_v>) { - dev_type = "Battery"; - } else if constexpr (std::is_same_v>) { - dev_type = "Camera"; - } else if constexpr (std::is_same_v>) { - dev_type = "DexHand"; - } else if constexpr (std::is_same_v>) { - dev_type = "Gripper"; - } else if constexpr (std::is_same_v>) { - dev_type = "Microphone"; - } else if constexpr (std::is_same_v>) { - dev_type = "Robot"; - } else if constexpr (std::is_same_v>) { - dev_type = "Speaker"; - } else if constexpr (std::is_same_v>){ - dev_type = "BioHead"; - } else { - dev_type = "Unknown"; - } - }, variant); - device_list.push_back(std::make_pair(device_id, dev_type)); - } - LOG(INFO) << "[DeviceManager] (getDeviceList): get device list success"; - } - catch (const exception &e) { - LOG(INFO) << "[DeviceManager] (getDeviceList): get device list failed: " << e.what(); - throw runtime_error("[DeviceManager] (getDeviceList): get device list failed: " + string(e.what())); - } -} - -void DeviceManager::getSystemInfo(SystemInfo& info) const { - info = info_; -} - -void DeviceManager::getSystemStatus(SystemStatus& status) const { - try { - sys_monitor_->getStatus(status); - } - catch (const exception &e) { - throw runtime_error(e.what()); - } -} - -void DeviceManager::init_devices_() { - try{ - auto dmgr_node = cfg_.getChild("Devices"); - auto battery_node = dmgr_node.getChild("Battery"); - for (auto &node: battery_node.getChildren()){ - string id = node.getAttrString("id"); - if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate Battery Device ID " << id; - throw runtime_error("[DeviceManager]: Duplicate Battery Device ID " + id); - } - auto device = dev_factory_->create(node); - if (device == nullptr) { - throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); - } - devices_[id] = device; - std::get>(devices_[id])->init(); - LOG(INFO) << "[DeviceManager]: Init Battery " << id << " Success"; - } - - auto agv_node = dmgr_node.getChild("AGV"); - for (auto &node: agv_node.getChildren()){ - string id = node.getAttrString("id"); - if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate AGV Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate AGV Device ID" + id); - } - auto device = dev_factory_->create(node); - if (device == nullptr) { - throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); - } - devices_[id] = device; - std::get>(devices_[id])->init(); - LOG(INFO) << "[DeviceManager]: Init AGV " << id << " Success"; - } - - config::CameraConfig camera_config; - ConfigHelper::getCamerasConfig(camera_config); - - for (int i = 0; i < camera_config.uvc_cameras().size(); i++) { - auto cam = camera_config.uvc_cameras(i); - if (!cam.enable()) - continue; - auto id = cam.id(); - if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate Camera Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate Camera Device ID" + id); - } - std::cout << "UVCCamera[" << i << "]: " << id << std::endl; - auto device = dev_factory_->createFromConfig(cam); - devices_[id] = device; - std::get>(devices_[id])->init(); - } - for (int i = 0; i < camera_config.realsense_cameras().size(); i++) { - auto cam = camera_config.realsense_cameras(i); - if (!cam.enable()) - continue; - auto id = cam.id(); - if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate Camera Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate Camera Device ID" + id); - } - std::cout << "UVCCamera[" << i << "]: " << id << std::endl; - auto device = dev_factory_->createFromConfig(cam); - devices_[id] = device; - std::get>(devices_[id])->init(); - } - - config::DexHandConfig dexhand_cfg; - ConfigHelper::getDexHandsConfig(dexhand_cfg); - for (int i = 0; i < dexhand_cfg.rh56dftp_dexhands().size(); i++) { - auto dexhand = dexhand_cfg.rh56dftp_dexhands(i); - if (!dexhand.enable()) - continue; - auto id = dexhand.id(); - if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate DexHand Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate DexHand Device ID" + id); - } - std::cout << "DexHand[" << i << "]: " << id << std::endl; - auto device = dev_factory_->createFromConfig(dexhand); - devices_[id] = device; - std::get>(devices_[id])->init(); - } - - for (int i = 0; i < dexhand_cfg.px_6ax_gen3().size(); i++) { - auto dexhand = dexhand_cfg.px_6ax_gen3(i); - if (!dexhand.enable()) - continue; - auto id = dexhand.id(); - if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate DexHand Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate DexHand Device ID" + id); - } - std::cout << "DexHand[" << i << "]: " << id << std::endl; - auto device = dev_factory_->createFromConfig(dexhand); - devices_[id] = device; - std::get>(devices_[id])->init(); - } - - auto robot_node = dmgr_node.getChild("Robot"); - for (auto &node: robot_node.getChildren()){ - string id = node.getAttrString("id"); - if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate robot Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate robot Device ID" + id); - } - auto device = dev_factory_->create(node); - if (device == nullptr) { - throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); - } - devices_[id] = device; - std::get>(devices_[id])->init(); - LOG(INFO) << "[DeviceManager]: Init Robot " << id << " Success"; - } - - config::MicroPhoneConfig micro_phone_config; - ConfigHelper::getMicroPhonesConfig(micro_phone_config); - for (int i = 0; i < micro_phone_config.ffmpeg_microphones().size(); i++) { - auto mic = micro_phone_config.ffmpeg_microphones(i); - if (!mic.enable()) - continue; - auto id = mic.id(); - if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate MicroPhone Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate MicroPhone Device ID" + id); - } - std::cout << "DexHand[" << i << "]: " << id << std::endl; - auto device = dev_factory_->createFromConfig(mic); - devices_[id] = device; - std::get>(devices_[id])->init(); - } - - config::SpeakerConfig speaker_config; - ConfigHelper::getSpeakersConfig(speaker_config); - for (int i = 0; i < speaker_config.ffmpeg_speakers().size(); i++) { - auto speaker = speaker_config.ffmpeg_speakers(i); - if (!speaker.enable()) - continue; - auto id = speaker.id(); - if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate Speaker Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate Speaker Device ID" + id); - } - std::cout << "DexHand[" << i << "]: " << id << std::endl; - auto device = dev_factory_->createFromConfig(speaker); - devices_[id] = device; - std::get>(devices_[id])->init(); - } - - - auto biohead_node = dmgr_node.getChild("BioHead"); - for (auto &node: biohead_node.getChildren()){ - string id = node.getAttrString("id"); - if (devices_.count(id)){ - LOG(ERROR) << "[DeviceManager]: Duplicate AbstractBiohead Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate AbstractBiohead Device ID" + id); - } - auto device = dev_factory_->create(node); - if (device == nullptr){ - throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); - } - devices_[id] = device; - std::get>(devices_[id])->init(); - LOG(INFO) << "[BioHead]: Init biohead " << id << " Success"; - - } - } - catch (const exception& e) { - LOG(ERROR) << e.what(); - throw runtime_error(e.what()); - } -} - -void DeviceManager::get_os_info_() { - utsname buf; - if (uname(&buf) == 0) { - info_.kernel_version = buf.release; - info_.architecture = buf.machine; - } - - std::ifstream osRelease("/etc/os-release"); - if (osRelease) { - std::string line; - while (std::getline(osRelease, line)) { - if (line.find("PRETTY_NAME=") == 0) { - info_.os = line.substr(13, line.size()-13); - break; - } - } - } -} diff --git a/cmvr-es/device_manager/src/system_monitor.cpp b/cmvr-es/device_manager/src/system_monitor.cpp deleted file mode 100644 index c529ca5f..00000000 --- a/cmvr-es/device_manager/src/system_monitor.cpp +++ /dev/null @@ -1,110 +0,0 @@ -// -// Created by xtkuang on 2025/5/13. -// - -#include "../include/system_monitor.h" - -using namespace std; -using namespace cmvr::device; - - -void SystemMonitor::getStatus(SystemStatus& status) { - try { - get_cpu_(); - get_disk_(); - get_memory_(); - status = status_; - } - catch (const exception& e) { - throw runtime_error(e.what()); - } -} - - -void SystemMonitor::get_cpu_() { - try { - static long prevIdleTime = 0, prevTotalTime = 0; - std::ifstream stat_file("/proc/stat"); - std::string line; - std::getline(stat_file, line); - stat_file.close(); - std::istringstream ss(line); - - std::string cpu; - long user, nice, system, idle, iowait, irq, softirq, steal; - ss >> cpu >> user >> nice >> system >> idle >> iowait >> irq >> softirq >> steal; - - long idleTime = idle + iowait; - long totalTime = user + nice + system + idle + iowait + irq + softirq + steal; - - long diffIdle = idleTime - prevIdleTime; - long diffTotal = totalTime - prevTotalTime; - prevIdleTime = idleTime; - prevTotalTime = totalTime; - - if (diffTotal == 0) { - status_.cpu_usage = 0; - throw std::runtime_error("[SystemMonitor] (get_cpu_):CPU usage is zero"); - } - else - status_.cpu_usage = (1.0f - (float)diffIdle / diffTotal) * 100.0f; - - int temp; - std::ifstream temp_file("/sys/class/thermal/thermal_zone0/temp"); - temp_file >> temp; - temp_file.close(); - status_.cpu_temperature = temp / 1000.0f; - } - catch (const std::exception& e) { - LOG(ERROR) << "[SystemMonitor] (get_cpu_): " << e.what(); - } -} - -void SystemMonitor::get_memory_() { - try { - std::ifstream mem_file("/proc/meminfo"); - std::string key; - long total = 0, available = 0, value; - std::string unit; - - while (mem_file >> key >> value >> unit) { - if (key == "MemTotal:") - total = value; - if (key == "MemAvailable:") { - available = value; - break; - } - } - mem_file.close(); - if (total == 0) { - status_.used_memory = 0.0f; - status_.total_memory = 0.0f; - throw runtime_error("[SystemMonitor] (get_disk_): invalid memory status"); - } - else { - status_.total_memory = (float)total / 1024.0f; - status_.used_memory = (float)(total - available) / 1024.0f; - } - } - catch (const std::exception& e) { - LOG(ERROR) << "[SystemMonitor] (get_cpu_): " << e.what(); - } -} - -void SystemMonitor::get_disk_() { - try { - struct statvfs stat{}; - if (statvfs("/", &stat) != 0) { - status_.used_disk = 0.0f; - status_.total_disk = 0.0f; - throw runtime_error("[SystemMonitor] (get_disk_): invalid disk status"); - } - unsigned long total = stat.f_blocks * stat.f_frsize; - unsigned long free = stat.f_bfree * stat.f_frsize; - status_.used_disk = float(total - free) / (1024.0f * 1024.0f); - status_.total_disk = (float)total / (1024.0f * 1024.0f); // return in MB - } - catch (const std::exception& e) { - LOG(ERROR) << "[SystemMonitor] (get_cpu_): " << e.what(); - } -} diff --git a/cmvr-es/devices/CMakeLists.txt b/cmvr-es/devices/CMakeLists.txt index 41c64db3..0fcfbade 100644 --- a/cmvr-es/devices/CMakeLists.txt +++ b/cmvr-es/devices/CMakeLists.txt @@ -1,8 +1,10 @@ add_subdirectory(camera) +add_subdirectory(agv) add_subdirectory(speaker) add_subdirectory(microphone) add_subdirectory(dexhand) add_subdirectory(biohead) +add_subdirectory(arm) add_subdirectory(robot) add_subdirectory(canbus) add_subdirectory(motor) diff --git a/cmvr-es/devices/abstract_device.h b/cmvr-es/devices/abstract_device.h index eff73bcd..cba2eb23 100644 --- a/cmvr-es/devices/abstract_device.h +++ b/cmvr-es/devices/abstract_device.h @@ -5,35 +5,37 @@ #ifndef CMVR_ES_ABSTRACT_DEVICES_H #define CMVR_ES_ABSTRACT_DEVICES_H +#include #include -#include "json/json.h" -#include "rapidxml/xml_parser.h" +#include + +#include "device_types.h" #include "state_define.h" #pragma warning(disable:4996) #define GLOG_USE_GLOG_EXPORT -#include +#include "common/base/logging/logger.h" namespace cmvr::device { class AbstractDevice { public: AbstractDevice() = default; - explicit AbstractDevice(const XmlNode& config) {cfg_ = config;} + explicit AbstractDevice(std::string id) : id_(std::move(id)) {} virtual ~AbstractDevice() = default; - [[maybe_unused]] XmlNode getConfig() {return cfg_;} - virtual void getState() {} - virtual void init() {} - virtual void start() {} - virtual void stop() {} - virtual void update() {} + const std::string& id() const noexcept { return id_; } - virtual void updateParams(const std::pair& param) {} - virtual void customCommand(const Json::Value &cmd, Json::Value &feedback) {} + virtual DeviceKind kind() const noexcept { return DeviceKind::Unknown; } + virtual std::string typeName() const = 0; + DeviceInfo info() const { return {id_, kind(), typeName()}; } + + virtual bool init() { return true; } + virtual bool start() { return true; } + virtual bool stop() { return true; } + virtual bool update() { return true; } protected: - XmlNode cfg_; std::string id_; // 设备名称 }; } diff --git a/cmvr-es/devices/agv/CMakeLists.txt b/cmvr-es/devices/agv/CMakeLists.txt new file mode 100644 index 00000000..d3963c05 --- /dev/null +++ b/cmvr-es/devices/agv/CMakeLists.txt @@ -0,0 +1,13 @@ +add_subdirectory(my_agv) + +add_library(agv INTERFACE) + +target_include_directories(agv INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(agv + INTERFACE + cmvr_es::device::my_agv + cmvr_es::proto +) + +add_library(cmvr_es::device::agv ALIAS agv) diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index a838fa89..788ae2a1 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -6,31 +6,32 @@ #define CMVR_ES_ABSTRACT_AGV_H #pragma once -#include "../abstract_device.h" +#include "devices/abstract_device.h" namespace cmvr::device{ class AbstractAGV: public AbstractDevice { public: - explicit AbstractAGV(const XmlNode &config): AbstractDevice(config) {}; + AbstractAGV() = default; ~AbstractAGV() override=default; - virtual void getState(AGVState &state) {} + DeviceKind kind() const noexcept override { return DeviceKind::AGV; } + virtual bool getState(AGVState &state) { return true; } // navigation - virtual void eStop() {} - virtual void goHome() {} - virtual void moveto(math::Pose2d &location, double speed_ratio) {} - virtual void setVelocity(math::Vec3 linear, math::Vec3 angular) {} + virtual bool eStop() { return true; } + virtual bool goHome() { return true; } + virtual bool moveto(math::Pose2d &location, double speed_ratio) { return true; } + virtual bool setVelocity(math::Vec3 linear, math::Vec3 angular) { return true; } // map - virtual void initMap(float resolution, int width, int height) {} - virtual void updateMap() {} - virtual void saveMap(const std::string& file_path) {} - virtual void loadMap(const std::string& file_path) {} + virtual bool initMap(float resolution, int width, int height) { return true; } + virtual bool updateMap() { return true; } + virtual bool saveMap(const std::string& file_path) { return true; } + virtual bool loadMap(const std::string& file_path) { return true; } protected: AGVState state_; }; } -#endif //CMVR_ES_ABSTRACT_AGV_H \ No newline at end of file +#endif //CMVR_ES_ABSTRACT_AGV_H diff --git a/cmvr-es/devices/agv/agv_factory.h b/cmvr-es/devices/agv/agv_factory.h new file mode 100644 index 00000000..b866face --- /dev/null +++ b/cmvr-es/devices/agv/agv_factory.h @@ -0,0 +1,44 @@ +#ifndef CMVR_ES_AGV_FACTORY_H +#define CMVR_ES_AGV_FACTORY_H + +#include + +#include "cmvr/config/agv_config/agv_config.pb.h" +#include "common/base/logging/logger.h" +#include "devices/agv/abstract_agv.h" +#include "devices/agv/my_agv/include/my_agv.h" + +namespace cmvr::device { + +class AGVFactory { +public: + static std::shared_ptr create(const config::AGVDeviceConfig& cfg) + { + if (cfg.id().empty()) { + CMVR_LOG(ERROR) << "[AGVFactory]: AGV id is empty"; + return nullptr; + } + + switch (cfg.backend_case()) { + case config::AGVDeviceConfig::kMyAgv: + { + if (!cfg.my_agv().id().empty() && cfg.my_agv().id() != cfg.id()) { + CMVR_LOG(ERROR) << "[AGVFactory]: AGV id does not match backend id: " << cfg.id(); + return nullptr; + } + auto backend = cfg.my_agv(); + backend.set_id(cfg.id()); + return std::make_shared(backend); + } + + case config::AGVDeviceConfig::BACKEND_NOT_SET: + default: + CMVR_LOG(ERROR) << "[AGVFactory]: AGV backend is not configured: " << cfg.id(); + return nullptr; + } + } +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_AGV_FACTORY_H diff --git a/cmvr-es/devices/agv/my_agv/CMakeLists.txt b/cmvr-es/devices/agv/my_agv/CMakeLists.txt new file mode 100644 index 00000000..c4bc53e0 --- /dev/null +++ b/cmvr-es/devices/agv/my_agv/CMakeLists.txt @@ -0,0 +1,8 @@ +add_library(my_agv SHARED src/my_agv.cpp) + +target_include_directories(my_agv PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + +target_link_libraries(my_agv PUBLIC cmvr_es::proto) + +add_library(cmvr_es::device::my_agv ALIAS my_agv) +install(TARGETS my_agv LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/agv/my_agv/include/my_agv.h b/cmvr-es/devices/agv/my_agv/include/my_agv.h new file mode 100644 index 00000000..8f0a788a --- /dev/null +++ b/cmvr-es/devices/agv/my_agv/include/my_agv.h @@ -0,0 +1,39 @@ +#ifndef CMVR_ES_MY_AGV_H +#define CMVR_ES_MY_AGV_H + +#include + +#include "cmvr/config/agv_config/agv_config.pb.h" +#include "devices/agv/abstract_agv.h" + +namespace cmvr::device { + +class MyAgv final : public AbstractAGV { +public: + explicit MyAgv(const config::MyAgvConfig& cfg); + ~MyAgv() override = default; + + std::string typeName() const override { return "MyAgv"; } + + bool init() override; + bool start() override; + bool stop() override; + bool update() override; + + bool getState(AGVState& state) override; + bool eStop() override; + bool goHome() override; + bool moveto(math::Pose2d& location, double speed_ratio) override; + bool setVelocity(math::Vec3 linear, math::Vec3 angular) override; + bool initMap(float resolution, int width, int height) override; + bool updateMap() override; + bool saveMap(const std::string& file_path) override; + bool loadMap(const std::string& file_path) override; + +private: + config::MyAgvConfig config_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MY_AGV_H diff --git a/cmvr-es/devices/agv/my_agv/src/my_agv.cpp b/cmvr-es/devices/agv/my_agv/src/my_agv.cpp new file mode 100644 index 00000000..a1c6ea77 --- /dev/null +++ b/cmvr-es/devices/agv/my_agv/src/my_agv.cpp @@ -0,0 +1,75 @@ +#include "devices/agv/my_agv/include/my_agv.h" + +namespace cmvr::device { + +MyAgv::MyAgv(const config::MyAgvConfig& cfg) : config_(cfg) +{ + id_ = config_.id(); +} + +bool MyAgv::init() +{ + return true; +} + +bool MyAgv::start() +{ + return true; +} + +bool MyAgv::stop() +{ + return true; +} + +bool MyAgv::update() +{ + return true; +} + +bool MyAgv::getState(AGVState&) +{ + return true; +} + +bool MyAgv::eStop() +{ + return true; +} + +bool MyAgv::goHome() +{ + return true; +} + +bool MyAgv::moveto(math::Pose2d&, double) +{ + return true; +} + +bool MyAgv::setVelocity(math::Vec3, math::Vec3) +{ + return true; +} + +bool MyAgv::initMap(float, int, int) +{ + return true; +} + +bool MyAgv::updateMap() +{ + return true; +} + +bool MyAgv::saveMap(const std::string&) +{ + return true; +} + +bool MyAgv::loadMap(const std::string&) +{ + return true; +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/CMakeLists.txt b/cmvr-es/devices/arm/CMakeLists.txt new file mode 100644 index 00000000..f6d9fd39 --- /dev/null +++ b/cmvr-es/devices/arm/CMakeLists.txt @@ -0,0 +1,15 @@ +add_subdirectory(motor_robot_arm) +add_subdirectory(aubo_arm) + +add_library(robot_arm INTERFACE) + +target_include_directories(robot_arm INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(robot_arm + INTERFACE + cmvr_es::device::motor_robot_arm + cmvr_es::device::aubo_arm + cmvr_es::proto +) + +add_library(cmvr_es::device::arm ALIAS robot_arm) diff --git a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt new file mode 100644 index 00000000..0a98cfe5 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt @@ -0,0 +1,29 @@ +add_library(aubo_arm SHARED + src/aubo_arm.cpp +) + +target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +set(AUBO_SDK_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/include) +set(AUBO_SDK_LIB_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/lib) + +if (EXISTS "${AUBO_SDK_INCLUDE_DIR}/aubo_sdk/rpc.h") + target_compile_definitions(aubo_arm PRIVATE CMVR_HAS_AUBO_SDK) + target_include_directories(aubo_arm PRIVATE ${AUBO_SDK_INCLUDE_DIR}) + if (EXISTS "${AUBO_SDK_LIB_DIR}") + target_link_directories(aubo_arm PRIVATE ${AUBO_SDK_LIB_DIR}) + target_link_libraries(aubo_arm PRIVATE aubo_sdk) + endif() +else() + message(WARNING "Aubo SDK not found; AuboArm will compile as an unavailable vendor arm.") +endif() + +target_link_libraries(aubo_arm + PUBLIC + cmvr_es::proto + PRIVATE + glog +) + +add_library(cmvr_es::device::aubo_arm ALIAS aubo_arm) +install(TARGETS aubo_arm LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h new file mode 100644 index 00000000..7b43db79 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h @@ -0,0 +1,112 @@ +#ifndef CMVR_ES_AUBO_ARM_H +#define CMVR_ES_AUBO_ARM_H + +#include +#include +#include +#include +#include + +#include "cmvr/config/arm_config/arm_config.pb.h" +#include "devices/arm/robot_arm.h" + +namespace cmvr::device { + +class AuboArm final : public RobotArm { +public: + explicit AuboArm(const config::RobotArmConfig& cfg); + ~AuboArm() override; + + std::string typeName() const override { return "AuboARM"; } + bool init() override; + bool stop() override; + + RobotModel getRobotModel() const override { return model_; } + std::size_t getDof() const override { return model_.dof; } + ArmState getRobotState() const override; + JointGroupState getJointState() const override; + CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; + RobotMode getRobotMode() const override; + SafetyMode getSafetyMode() const override { return SafetyMode::Normal; } + ControlMode getControlMode() const override { return ControlMode::Position; } + + Result torqueOn() override; + Result torqueOff() override; + Result calibrateZeroQ(const std::string& joint_name) override; + Result emergencyStop() override; + Result protectiveStop() override { return emergencyStop(); } + Result setSpeedScaling(double scaling) override; + double getSpeedScaling() const override { return speed_scaling_; } + bool isProtectiveStopped() const override { return false; } + bool isEmergencyStopped() const override { return emergency_stopped_; } + bool isFault() const override { return false; } + + Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override; + Result speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) override; + Result stopJ(double acceleration) override; + Result moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame = FrameType::Base) override; + Result speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame = FrameType::Base) override; + Result stopL(double acceleration) override; + Result stopMotion() override; + + Result startServoMode(const ServoOptions& options) override; + Result servoJ(const JointPositionCommand& target) override; + Result servoL(const CartesianPose& target, FrameType frame = FrameType::Base) override; + Result servoSpeedJ(const JointVelocityCommand& velocity) override; + Result servoSpeedL(const CartesianVelocity& velocity, FrameType frame = FrameType::Base) override; + Result stopServoMode() override; + + Result connect(const std::string& ip, int port) override; + Result disconnect() override; + bool isConnected() const override { return connected_.load(); } + Result powerOn() override { return torqueOn(); } + Result powerOff() override { return torqueOff(); } + Result brakeRelease() override { return torqueOn(); } + Result shutdown() override; + Result clearFault() override { return Result::success(); } + Result unlockProtectiveStop() override { return Result::success(); } + Result loadProgram(const std::string& program_name) override; + Result playProgram() override; + Result pauseProgram() override; + Result stopProgram() override; + + std::vector ik(const std::string& base_link, + const std::string& ee_link, + const CartesianPose& pose) override; + std::shared_ptr kinematicsSolver() const override { return nullptr; } + CartesianPose fk(const std::string& base_link, const std::string& ee_link) override; + CartesianPose fk(bool is_tcp = true) override; + CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; } + bool busy() const override { return busy_.load(); } + +private: + Result unsupported_(const std::string& name) const; + bool validDof_(std::size_t size, std::string& error) const; + Result ensureConnected_(const std::string& context) const; + +#if defined(CMVR_HAS_AUBO_SDK) + struct SdkState; +#endif + +private: + config::RobotArmConfig cfg_; + config::VendorRobotArmBackendConfig vendor_cfg_; + RobotModel model_; + std::string ip_; + int port_{30004}; + std::string username_; + std::string password_; + double speed_scaling_{1.0}; + std::atomic connected_{false}; + std::atomic busy_{false}; + bool emergency_stopped_{false}; + mutable std::mutex mutex_; + +#if defined(CMVR_HAS_AUBO_SDK) + std::unique_ptr sdk_; +#endif +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_AUBO_ARM_H diff --git a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp new file mode 100644 index 00000000..e1cf5a55 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp @@ -0,0 +1,575 @@ +#include "devices/arm/aubo_arm/include/aubo_arm.h" + +#include +#include +#include +#include + +#include "common/base/logging/logger.h" + +#if defined(CMVR_HAS_AUBO_SDK) +#include "aubo_sdk/rpc.h" +#endif + +namespace cmvr::device { +namespace { + +struct BusyGuard { + std::atomic& busy; + ~BusyGuard() { busy.store(false); } +}; + +std::vector defaultJointNames(const std::size_t dof) +{ + std::vector names; + names.reserve(dof); + for (std::size_t i = 0; i < dof; ++i) { + names.push_back("joint_" + std::to_string(i + 1)); + } + return names; +} + +std::string vendorBrandName(const config::VendorRobotArmBrand brand) +{ + switch (brand) { + case config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM: + return "AuboARM"; + case config::VENDOR_ROBOT_ARM_BRAND_UNKNOWN: + default: + return "Unknown"; + } +} + +} // namespace + +#if defined(CMVR_HAS_AUBO_SDK) +struct AuboArm::SdkState { + std::shared_ptr rpc_client; +}; +#endif + +AuboArm::AuboArm(const config::RobotArmConfig& cfg) + : cfg_(cfg) +{ + id_ = cfg.id(); + if (cfg.has_vendor()) { + vendor_cfg_ = cfg.vendor(); + } + + ip_ = vendor_cfg_.ip(); + port_ = vendor_cfg_.port() > 0 ? vendor_cfg_.port() : 30004; + username_ = vendor_cfg_.username().empty() ? "aubo" : vendor_cfg_.username(); + password_ = vendor_cfg_.password().empty() ? "123456" : vendor_cfg_.password(); + + const auto dof = vendor_cfg_.dof() > 0 ? static_cast(vendor_cfg_.dof()) : 6U; + model_.name = vendor_cfg_.model().empty() ? "AuboARM" : vendor_cfg_.model(); + model_.manufacturer = vendorBrandName(vendor_cfg_.brand()); + model_.dof = dof; + model_.joint_names.assign(vendor_cfg_.joint_names().begin(), vendor_cfg_.joint_names().end()); + if (model_.joint_names.empty()) { + model_.joint_names = defaultJointNames(dof); + } + if (model_.joint_names.size() != dof) { + CMVR_LOG(ERROR) << "[AuboArm] joint_names size mismatch, id=" << id_; + model_.joint_names = defaultJointNames(dof); + } +} + +AuboArm::~AuboArm() +{ + (void)disconnect(); +} + +bool AuboArm::init() +{ + if (ip_.empty()) { + CMVR_LOG(ERROR) << "[AuboArm] ip is empty, id=" << id_; + return false; + } + const auto result = connect(ip_, port_); + if (!result.ok()) { + CMVR_LOG(ERROR) << "[AuboArm] init failed: " << result.message; + return false; + } + return true; +} + +bool AuboArm::stop() +{ + return stopMotion().ok(); +} + +ArmState AuboArm::getRobotState() const +{ + ArmState state; + state.connected = connected_.load(); + state.powered_on = state.connected; + state.brake_released = state.connected; + state.moving = busy_.load(); + state.robot_mode = getRobotMode(); + state.safety_mode = getSafetyMode(); + state.control_mode = getControlMode(); + state.emergency_stopped = emergency_stopped_; + state.speed_scaling = speed_scaling_; + state.actual_joint_state = getJointState(); + state.target_joint_state = state.actual_joint_state; + return state; +} + +JointGroupState AuboArm::getJointState() const +{ + JointGroupState state; + state.position.assign(model_.dof, 0.0); + state.velocity.assign(model_.dof, 0.0); + state.effort.assign(model_.dof, 0.0); + +#if defined(CMVR_HAS_AUBO_SDK) + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + return state; + } + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot name list is empty"; + return state; + } + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot interface is null"; + return state; + } + const auto robot_state = robot_interface->getRobotState(); + const auto positions = robot_state->getJointPositions(); + const auto velocities = robot_state->getJointSpeeds(); + const auto n = std::min(model_.dof, positions.size()); + for (std::size_t i = 0; i < n; ++i) { + state.position[i] = positions[i]; + } + const auto vn = std::min(model_.dof, velocities.size()); + for (std::size_t i = 0; i < vn; ++i) { + state.velocity[i] = velocities[i]; + } + } catch (const std::exception& e) { + CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: " << e.what(); + } +#endif + return state; +} + +CartesianPose AuboArm::getTcpPose(FrameType frame) const +{ + (void)frame; + return {}; +} + +RobotMode AuboArm::getRobotMode() const +{ + if (!connected_.load()) { + return RobotMode::Disconnected; + } + if (emergency_stopped_) { + return RobotMode::Stopped; + } + return busy_.load() ? RobotMode::Running : RobotMode::Idle; +} + +Result AuboArm::torqueOn() +{ + const auto ready = ensureConnected_("torqueOn"); + if (!ready.ok()) { + return ready; + } + +#if defined(CMVR_HAS_AUBO_SDK) + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); + } + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); + } + + double mass = 0.0; + std::vector cog(3, 0.0); + std::vector aom(3, 0.0); + std::vector inertia(6, 0.0); + robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia); + + if (robot_interface->getRobotState()->getRobotModeType() != + arcs::common_interface::RobotModeType::Running) { + robot_interface->getRobotManage()->poweron(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + robot_interface->getRobotManage()->startup(); + } + emergency_stopped_ = false; + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what()); + } +#else + return unsupported_("torqueOn"); +#endif +} + +Result AuboArm::torqueOff() +{ + const auto ready = ensureConnected_("torqueOff"); + if (!ready.ok()) { + return ready; + } + +#if defined(CMVR_HAS_AUBO_SDK) + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); + } + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); + } + robot_interface->getRobotManage()->poweroff(); + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOff failed: ") + e.what()); + } +#else + return unsupported_("torqueOff"); +#endif +} + +Result AuboArm::calibrateZeroQ(const std::string& joint_name) +{ + (void)joint_name; + return unsupported_("calibrateZeroQ"); +} + +Result AuboArm::emergencyStop() +{ + emergency_stopped_ = true; + return stopMotion(); +} + +Result AuboArm::setSpeedScaling(const double scaling) +{ + if (scaling < 0.0 || scaling > 1.0) { + return Result::failure(ArmErrorCode::InvalidArgument, "speed scaling must be in [0, 1]"); + } + speed_scaling_ = scaling; + return Result::success(); +} + +Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& options) +{ + std::string error; + if (!validDof_(target.position.size(), error)) { + return Result::failure(ArmErrorCode::InvalidDof, error); + } + const auto ready = ensureConnected_("moveJ"); + if (!ready.ok()) { + return ready; + } + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); + } + BusyGuard busy_guard{busy_}; + +#if defined(CMVR_HAS_AUBO_SDK) + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); + } + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); + } + robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); + robot_interface->getMotionControl()->moveJoint( + target.position, + options.acceleration > 0.0 ? options.acceleration : 0.5, + options.velocity > 0.0 ? options.velocity : 0.5, + options.blend_radius, + 0); + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveJ failed: ") + e.what()); + } +#else + return unsupported_("moveJ"); +#endif +} + +Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) +{ + (void)velocity; + (void)acceleration; + (void)duration; + return unsupported_("speedJ"); +} + +Result AuboArm::stopJ(double acceleration) +{ + (void)acceleration; + return stopMotion(); +} + +Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame) +{ + (void)frame; + const auto ready = ensureConnected_("moveL"); + if (!ready.ok()) { + return ready; + } + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); + } + BusyGuard busy_guard{busy_}; + +#if defined(CMVR_HAS_AUBO_SDK) + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); + } + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); + } + robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); + std::vector tcp_offset(6, 0.0); + robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); + std::vector pose{target.x, target.y, target.z, target.rx, target.ry, target.rz}; + robot_interface->getMotionControl()->moveLine( + pose, + options.acceleration > 0.0 ? options.acceleration : 0.5, + options.velocity > 0.0 ? options.velocity : 0.25, + options.blend_radius, + 0); + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveL failed: ") + e.what()); + } +#else + return unsupported_("moveL"); +#endif +} + +Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame) +{ + (void)velocity; + (void)acceleration; + (void)duration; + (void)frame; + return unsupported_("speedL"); +} + +Result AuboArm::stopL(double acceleration) +{ + (void)acceleration; + return stopMotion(); +} + +Result AuboArm::stopMotion() +{ +#if defined(CMVR_HAS_AUBO_SDK) + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + return Result::success(); + } + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return Result::success(); + } + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (robot_interface) { + robot_interface->getMotionControl()->stopMove(); + } + busy_.store(false); + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what()); + } +#else + busy_.store(false); + return Result::success(); +#endif +} + +Result AuboArm::startServoMode(const ServoOptions& options) +{ + (void)options; + return unsupported_("startServoMode"); +} + +Result AuboArm::servoJ(const JointPositionCommand& target) +{ + (void)target; + return unsupported_("servoJ"); +} + +Result AuboArm::servoL(const CartesianPose& target, FrameType frame) +{ + (void)target; + (void)frame; + return unsupported_("servoL"); +} + +Result AuboArm::servoSpeedJ(const JointVelocityCommand& velocity) +{ + (void)velocity; + return unsupported_("servoSpeedJ"); +} + +Result AuboArm::servoSpeedL(const CartesianVelocity& velocity, FrameType frame) +{ + (void)velocity; + (void)frame; + return unsupported_("servoSpeedL"); +} + +Result AuboArm::stopServoMode() +{ + return Result::success(); +} + +Result AuboArm::connect(const std::string& ip, const int port) +{ + if (connected_.load()) { + return Result::success(); + } + if (ip.empty()) { + return Result::failure(ArmErrorCode::InvalidArgument, "[AuboArm] ip is empty"); + } + +#if defined(CMVR_HAS_AUBO_SDK) + try { + sdk_ = std::make_unique(); + sdk_->rpc_client = std::make_shared(); + sdk_->rpc_client->setRequestTimeout(1000); + sdk_->rpc_client->connect(ip, port > 0 ? port : 30004); + sdk_->rpc_client->login(username_, password_); + ip_ = ip; + port_ = port > 0 ? port : 30004; + connected_.store(true); + return Result::success(); + } catch (const std::exception& e) { + sdk_.reset(); + connected_.store(false); + return Result::failure(ArmErrorCode::ConnectionFailed, + std::string("[AuboArm] connect failed: ") + e.what()); + } +#else + (void)port; + return Result::failure(ArmErrorCode::UnsupportedCommand, + "[AuboArm] Aubo SDK is not available in this build"); +#endif +} + +Result AuboArm::disconnect() +{ +#if defined(CMVR_HAS_AUBO_SDK) + try { + if (sdk_ && sdk_->rpc_client) { + sdk_->rpc_client->logout(); + sdk_->rpc_client->disconnect(); + } + } catch (const std::exception& e) { + CMVR_LOG(ERROR) << "[AuboArm] disconnect failed: " << e.what(); + } + sdk_.reset(); +#endif + connected_.store(false); + busy_.store(false); + return Result::success(); +} + +Result AuboArm::shutdown() +{ + (void)stopMotion(); + return disconnect(); +} + +Result AuboArm::loadProgram(const std::string& program_name) +{ + (void)program_name; + return unsupported_("loadProgram"); +} + +Result AuboArm::playProgram() +{ + return unsupported_("playProgram"); +} + +Result AuboArm::pauseProgram() +{ + return unsupported_("pauseProgram"); +} + +Result AuboArm::stopProgram() +{ + return unsupported_("stopProgram"); +} + +std::vector AuboArm::ik(const std::string& base_link, + const std::string& ee_link, + const CartesianPose& pose) +{ + (void)base_link; + (void)ee_link; + (void)pose; + CMVR_LOG(ERROR) << "[AuboArm] ik is not implemented"; + return {}; +} + +CartesianPose AuboArm::fk(const std::string& base_link, const std::string& ee_link) +{ + (void)base_link; + (void)ee_link; + CMVR_LOG(ERROR) << "[AuboArm] fk(base,ee) is not implemented"; + return {}; +} + +CartesianPose AuboArm::fk(bool is_tcp) +{ + (void)is_tcp; + CMVR_LOG(ERROR) << "[AuboArm] fk is not implemented"; + return {}; +} + +Result AuboArm::unsupported_(const std::string& name) const +{ + const std::string message = "[AuboArm] " + name + " is not implemented"; + CMVR_LOG(ERROR) << message; + return Result::failure(ArmErrorCode::UnsupportedCommand, message); +} + +bool AuboArm::validDof_(const std::size_t size, std::string& error) const +{ + if (size != model_.dof) { + error = "[AuboArm] command dof mismatch, expected=" + std::to_string(model_.dof) + + ", actual=" + std::to_string(size); + CMVR_LOG(ERROR) << error; + return false; + } + return true; +} + +Result AuboArm::ensureConnected_(const std::string& context) const +{ + if (!connected_.load()) { + return Result::failure(ArmErrorCode::NotConnected, + "[AuboArm] " + context + " failed: arm is not connected"); + } +#if defined(CMVR_HAS_AUBO_SDK) + if (!sdk_ || !sdk_->rpc_client) { + return Result::failure(ArmErrorCode::NotConnected, + "[AuboArm] " + context + " failed: SDK client is null"); + } +#endif + return Result::success(); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt new file mode 100644 index 00000000..91088f69 --- /dev/null +++ b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt @@ -0,0 +1,35 @@ +add_library(motor_robot_arm SHARED + src/motor_robot_arm.cpp +) + +target_include_directories(motor_robot_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(motor_robot_arm + PUBLIC + cmvr_es::proto + PRIVATE + cmvr_es::algorithms::arm_motion + cmvr_es::ik_solver + cmvr_es::algorithms::arm_control + cmvr_es::device::motor_system + glog +) + +add_library(cmvr_es::device::motor_robot_arm ALIAS motor_robot_arm) +install(TARGETS motor_robot_arm LIBRARY DESTINATION lib) + +add_executable(motor_robot_arm_mujoco_test + src/motor_robot_arm_mujoco_test.cpp +) + +target_link_libraries(motor_robot_arm_mujoco_test + PRIVATE + cmvr_es::device::motor_robot_arm + cmvr_es::device::motor_system + cmvr_es::device::mujoco_motor + cmvr_es::mujoco_viewer + cmvr_es::proto + gtest + gtest_main + pthread +) diff --git a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h new file mode 100644 index 00000000..a4ac49b0 --- /dev/null +++ b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h @@ -0,0 +1,135 @@ +#ifndef CMVR_ES_MOTOR_ROBOT_ARM_H +#define CMVR_ES_MOTOR_ROBOT_ARM_H + +#include +#include +#include +#include +#include +#include + +#include "algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h" +#include "algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h" +#include "algorithms/controllers/arm_control/include/cartesian_velocity_controller.h" +#include "cmvr/config/arm_config/arm_config.pb.h" +#include "devices/arm/robot_arm.h" +#include "algorithms/kinematics/ik_solver/common/include/ik_solver.h" +#include "motor/motor_manager.h" + +namespace cmvr::device { + +class MotorRobotArm final : public RobotArm { +public: + explicit MotorRobotArm(const config::RobotArmConfig& cfg); + ~MotorRobotArm() override; + + std::string typeName() const override { return "MotorRobotArm"; } + bool init() override; + bool stop() override; + + RobotModel getRobotModel() const override { return model_; } + std::size_t getDof() const override { return static_cast(dof_); } + ArmState getRobotState() const override; + JointGroupState getJointState() const override; + CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; + RobotMode getRobotMode() const override { return RobotMode::Idle; } + SafetyMode getSafetyMode() const override; + ControlMode getControlMode() const override { return ControlMode::Position; } + + Result torqueOn() override; + Result torqueOff() override; + Result calibrateZeroQ(const std::string& joint_name) override; + Result emergencyStop() override; + Result protectiveStop() override { return emergencyStop(); } + Result setSpeedScaling(double scaling) override; + double getSpeedScaling() const override { return speed_scaling_; } + bool isProtectiveStopped() const override { return false; } + bool isEmergencyStopped() const override { return emergency_stopped_; } + bool isFault() const override { return false; } + + Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override; + Result speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) override; + Result stopJ(double acceleration) override; + Result moveL(const CartesianPose& target, + const MotionOptions& options, + FrameType frame = FrameType::Base) override; + Result speedL(const CartesianVelocity& velocity, + double acceleration, + double duration, + FrameType frame = FrameType::Base) override; + Result stopL(double acceleration) override; + Result stopMotion() override; + + Result startServoMode(const ServoOptions& options) override; + Result servoJ(const JointPositionCommand& target) override; + Result servoL(const CartesianPose& target, FrameType frame = FrameType::Base) override; + Result servoSpeedJ(const JointVelocityCommand& velocity) override; + Result servoSpeedL(const CartesianVelocity& velocity, FrameType frame = FrameType::Base) override; + Result stopServoMode() override; + + Result connect(const std::string& ip, int port) override; + Result disconnect() override; + bool isConnected() const override { return motor_manager_ != nullptr; } + Result powerOn() override { return torqueOn(); } + Result powerOff() override { return torqueOff(); } + Result brakeRelease() override { return torqueOn(); } + Result shutdown() override; + Result clearFault() override { return Result::success(); } + Result unlockProtectiveStop() override { return Result::success(); } + Result loadProgram(const std::string& program_name) override; + Result playProgram() override; + Result pauseProgram() override; + Result stopProgram() override; + + std::vector ik(const std::string& base_link, + const std::string& ee_link, + const CartesianPose& pose) override; + std::shared_ptr kinematicsSolver() const override { return ik_solver_; } + CartesianPose fk(const std::string& base_link, const std::string& ee_link) override; + CartesianPose fk(bool is_tcp = true) override; + CartesianVelocity getSpeedLCommandTwistBase() const override; + + bool busy() const override; + +private: + bool containsJoint_(const std::string& joint_name) const; + bool validatePositionCommand_(const JointPositionCommand& cmd, std::string& error) const; + bool validateVelocityCommand_(const JointVelocityCommand& cmd, std::string& error) const; + std::shared_ptr getMotor_(const std::string& joint_name) const; + bool readArmState_(std::vector& q_now, std::vector& qd_now) const; + std::vector readJointPosition_() const; + + bool configureAlgorithms_(); + bool executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory); + + static CartesianVelocityController::Config toCartesianVelocityControllerConfig_( + const config::CartesianVelocityControllerConfig& config); + static std::vector withDefaultQdMax_(const std::vector& qd_max, + std::size_t dof); + static Result unsupported_(const std::string& name); + +private: + config::RobotArmConfig cfg_; + config::MotorRobotArmBackendConfig motor_cfg_; + int dof_{0}; + RobotModel model_; + std::vector joint_names_; + std::unordered_set joint_set_; + std::string motor_system_id_; + std::shared_ptr motor_manager_{nullptr}; + + std::shared_ptr ik_solver_{nullptr}; + std::shared_ptr joint_planner_{nullptr}; + std::shared_ptr cartesian_planner_{nullptr}; + std::unique_ptr cartesian_velocity_controller_{nullptr}; + + mutable std::mutex mutex_; + std::atomic busy_{false}; + double speed_scaling_{1.0}; + bool emergency_stopped_{false}; + ServoOptions servo_options_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MOTOR_ROBOT_ARM_H diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp new file mode 100644 index 00000000..faf2ce83 --- /dev/null +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp @@ -0,0 +1,770 @@ +#include "arm/motor_robot_arm/include/motor_robot_arm.h" + +#include +#include +#include +#include +#include +#include + +#include "algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h" +#include "algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner_factory.h" +#include "algorithms/kinematics/ik_solver/ik_solver_factory.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" +#include "common/base/logging/logger.h" +#include "common/math/transform_math.h" +#include "motor/abstract_motor.h" +#include "motor/motor_system/include/motor_system.h" + +namespace cmvr::device { + +namespace { + +struct BusyGuard { + std::atomic& busy; + ~BusyGuard() { busy.store(false); } +}; + +std::string joinStrings(const std::vector& values) +{ + std::ostringstream oss; + for (std::size_t i = 0; i < values.size(); ++i) { + if (i > 0) { + oss << ", "; + } + oss << values[i]; + } + return oss.str(); +} + +std::vector repeatedToVector( + const google::protobuf::RepeatedPtrField& values) +{ + return {values.begin(), values.end()}; +} + +} // namespace + +MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg) + : cfg_(cfg) +{ + id_ = cfg.id(); + if (id_.empty()) { + CMVR_LOG(ERROR) << "[MotorRobotArm] id is empty"; + return; + } + if (!cfg_.has_motor()) { + CMVR_LOG(ERROR) << "[MotorRobotArm] missing motor config: " << id_; + return; + } + + motor_cfg_ = cfg_.motor(); + dof_ = motor_cfg_.dof() > 0 ? motor_cfg_.dof() : motor_cfg_.joint_names_size(); + motor_system_id_ = motor_cfg_.motor_system_id(); + joint_names_.assign(motor_cfg_.joint_names().begin(), motor_cfg_.joint_names().end()); + joint_set_.insert(joint_names_.begin(), joint_names_.end()); + + if (motor_system_id_.empty()) { + CMVR_LOG(ERROR) << "[MotorRobotArm] motor_system_id is empty: " << id_; + return; + } + if (joint_names_.empty()) { + CMVR_LOG(ERROR) << "[MotorRobotArm] joint_names is empty: " << id_; + return; + } + if (static_cast(joint_names_.size()) != dof_) { + CMVR_LOG(ERROR) << "[MotorRobotArm] dof mismatched with joint_names: " << id_; + return; + } + + model_.name = id_; + model_.manufacturer = "cmvr"; + model_.dof = static_cast(dof_); + model_.joint_names = joint_names_; +} + +MotorRobotArm::~MotorRobotArm() +{ + if (cartesian_velocity_controller_) { + cartesian_velocity_controller_->shutdown(); + } +} + +bool MotorRobotArm::init() +{ + const auto motor_group_ids = repeatedToVector(motor_cfg_.motor_group_ids()); + CMVR_LOG(INFO) << "[MotorRobotArm] (init): Arm '" << id_ + << "' uses motor_system=" << motor_system_id_ + << ", motor_groups=[" << joinStrings(motor_group_ids) + << "], joints=[" << joinStrings(joint_names_) << "]"; + + if (!motor_manager_) { + motor_manager_ = MotorSystem::managerFor(motor_system_id_); + } + if (!motor_manager_) { + CMVR_LOG(ERROR) << "[MotorRobotArm] MotorSystem is not initialized: " << motor_system_id_; + CMVR_LOG(ERROR) << "[MotorRobotArm] (init): Arm '" << id_ + << "' initialized motors=[], missing motors=[" + << joinStrings(joint_names_) << "]"; + return false; + } + + std::vector initialized_motors; + std::vector missing_motors; + for (const auto& joint_name : joint_names_) { + auto motor = motor_manager_->getMotor(joint_name); + if (!motor) { + missing_motors.push_back(joint_name); + continue; + } + std::ostringstream motor_desc; + motor_desc << joint_name << "(node_id=" << static_cast(motor->id()) + << ", type=" << motor->typeName() << ")"; + initialized_motors.push_back(motor_desc.str()); + } + + CMVR_LOG(INFO) << "[MotorRobotArm] (init): Arm '" << id_ + << "' initialized motors=[" << joinStrings(initialized_motors) << "]"; + if (!missing_motors.empty()) { + CMVR_LOG(ERROR) << "[MotorRobotArm] (init): Arm '" << id_ + << "' missing or uninitialized motors=[" + << joinStrings(missing_motors) << "]"; + return false; + } + + if (!configureAlgorithms_()) { + CMVR_LOG(ERROR) << "[MotorRobotArm] failed to configure algorithms: " << id_; + return false; + } + CMVR_LOG(INFO) << "[MotorRobotArm] (init): Arm '" << id_ << "' init success"; + return true; +} + +bool MotorRobotArm::stop() +{ + return shutdown().ok(); +} + +ArmState MotorRobotArm::getRobotState() const +{ + ArmState state; + state.connected = motor_manager_ != nullptr; + state.powered_on = true; + state.brake_released = !emergency_stopped_; + state.moving = busy(); + state.emergency_stopped = emergency_stopped_; + state.speed_scaling = speed_scaling_; + state.robot_mode = RobotMode::Idle; + state.safety_mode = getSafetyMode(); + state.control_mode = ControlMode::Position; + state.actual_joint_state = getJointState(); + state.actual_tcp_pose = getTcpPose(); + state.actual_tcp_velocity = getSpeedLCommandTwistBase(); + return state; +} + +JointGroupState MotorRobotArm::getJointState() const +{ + JointGroupState state; + state.position.reserve(joint_names_.size()); + state.velocity.reserve(joint_names_.size()); + state.effort.reserve(joint_names_.size()); + for (const auto& joint_name : joint_names_) { + auto motor = getMotor_(joint_name); + if (!motor) { + continue; + } + state.position.push_back(motor->getQ()); + state.velocity.push_back(motor->getQd()); + state.effort.push_back(0.0); + } + return state; +} + +CartesianPose MotorRobotArm::getTcpPose(const FrameType frame) const +{ + (void)frame; + if (!ik_solver_) { + return {}; + } + std::vector q_now; + std::vector qd_now; + if (!readArmState_(q_now, qd_now)) { + return {}; + } + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); + if (!ik_solver_->fk(q_now, transform, true)) { + return {}; + } + return common::math::matrixToPose(transform); +} + +SafetyMode MotorRobotArm::getSafetyMode() const +{ + return emergency_stopped_ ? SafetyMode::EmergencyStop : SafetyMode::Normal; +} + +Result MotorRobotArm::torqueOn() +{ + for (const auto& joint_name : joint_names_) { + auto motor = getMotor_(joint_name); + if (!motor) { + return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); + } + motor->brake(); + } + emergency_stopped_ = false; + return Result::success(); +} + +Result MotorRobotArm::torqueOff() +{ + for (const auto& joint_name : joint_names_) { + auto motor = getMotor_(joint_name); + if (!motor) { + return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); + } + motor->torqueOff(); + } + return Result::success(); +} + +Result MotorRobotArm::calibrateZeroQ(const std::string& joint_name) +{ + if (!containsJoint_(joint_name)) { + return Result::failure(ArmErrorCode::InvalidArgument, + "[MotorRobotArm] joint does not belong to arm: " + joint_name); + } + auto motor = getMotor_(joint_name); + if (!motor) { + return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); + } + motor->calibrateZeroQ(); + return Result::success(); +} + +Result MotorRobotArm::emergencyStop() +{ + if (cartesian_velocity_controller_) { + cartesian_velocity_controller_->shutdown(); + } + for (const auto& joint_name : joint_names_) { + auto motor = getMotor_(joint_name); + if (!motor) { + return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); + } + motor->brake(); + } + emergency_stopped_ = true; + return Result::success(); +} + +Result MotorRobotArm::setSpeedScaling(const double scaling) +{ + if (scaling < 0.0 || scaling > 1.0) { + return Result::failure(ArmErrorCode::InvalidArgument, "speed scaling must be in [0, 1]"); + } + speed_scaling_ = scaling; + return Result::success(); +} + +Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOptions& options) +{ + std::string error; + if (!validatePositionCommand_(target, error)) { + return Result::failure(ArmErrorCode::InvalidArgument, error); + } + if (!joint_planner_) { + return Result::failure(ArmErrorCode::RobotNotReady, "joint planner is not initialized"); + } + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[MotorRobotArm] arm is busy: " + id_); + } + BusyGuard busy_guard{busy_}; + std::lock_guard lock(mutex_); + + std::vector samples; + if (!joint_planner_->planMoveJ(readJointPosition_(), target, options, speed_scaling_, samples)) { + return Result::failure(ArmErrorCode::CommandFailed, "[MotorRobotArm] moveJ planner failed: " + id_); + } + if (samples.size() < 2) { + return Result::success(); + } + + std::vector> motors; + motors.reserve(joint_names_.size()); + for (const auto& joint_name : joint_names_) { + auto motor = getMotor_(joint_name); + if (!motor) { + return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); + } + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + } + motors.push_back(std::move(motor)); + } + + const auto t0 = std::chrono::steady_clock::now(); + constexpr double fallback_dt = 0.001; + for (std::size_t k = 1; k < samples.size(); ++k) { + const auto& sample = samples[k]; + if (sample.position.size() != motors.size()) { + return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch"); + } + for (std::size_t i = 0; i < motors.size(); ++i) { + const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0; + motors[i]->setTarget(sample.position[i], qd); + } + if (k + 1 < samples.size()) { + const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t + : static_cast(k + 1) * fallback_dt; + std::this_thread::sleep_until(t0 + std::chrono::duration_cast( + std::chrono::duration(next_t))); + } + } + return Result::success(); +} + +Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity, + const double acceleration, + const double duration) +{ + (void)acceleration; + std::string error; + if (!validateVelocityCommand_(velocity, error)) { + return Result::failure(ArmErrorCode::InvalidArgument, error); + } + + { + std::lock_guard lock(mutex_); + for (std::size_t i = 0; i < joint_names_.size(); ++i) { + auto motor = getMotor_(joint_names_[i]); + if (!motor) { + return Result::failure(ArmErrorCode::RobotNotReady, + "motor not found for joint: " + joint_names_[i]); + } + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); + } + motor->setTarget(velocity.velocity[i] * speed_scaling_); + } + } + + if (duration > 0.0) { + std::this_thread::sleep_for(std::chrono::duration(duration)); + return stopJ(acceleration); + } + return Result::success(); +} + +Result MotorRobotArm::stopJ(const double acceleration) +{ + JointVelocityCommand zero; + zero.velocity.assign(joint_names_.size(), 0.0); + return speedJ(zero, acceleration, 0.0); +} + +Result MotorRobotArm::moveL(const CartesianPose& target, + const MotionOptions& options, + const FrameType frame) +{ + if (cartesian_velocity_controller_) { + cartesian_velocity_controller_->shutdown(); + } + if (options.asynchronous) { + return Result::failure(ArmErrorCode::UnsupportedCommand, "moveL asynchronous=true is not supported"); + } + if (!cartesian_planner_) { + return Result::failure(ArmErrorCode::RobotNotReady, "cartesian planner is not initialized"); + } + if (options.velocity <= 0.0 || options.acceleration <= 0.0 || options.jerk <= 0.0) { + return Result::failure(ArmErrorCode::InvalidArgument, "moveL velocity/acceleration/jerk must be positive"); + } + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy"); + } + BusyGuard busy_guard{busy_}; + + std::vector q_start; + std::vector qd_now; + if (!readArmState_(q_start, qd_now)) { + return Result::failure(ArmErrorCode::RobotNotReady, "failed to read arm state"); + } + + CartesianJointTrajectory trajectory; + if (!cartesian_planner_->planMoveL(target, + q_start, + withDefaultQdMax_(options.joint_velocity_limits, getDof()), + options.velocity, + options.acceleration, + options.jerk, + frame, + trajectory)) { + return Result::failure(ArmErrorCode::CommandFailed, "moveL planner failed"); + } + + return executeMoveLTrajectory_(trajectory) ? Result::success() + : Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed"); +} + +Result MotorRobotArm::speedL(const CartesianVelocity& velocity, + const double acceleration, + const double duration, + const FrameType frame) +{ + if (busy_.load()) { + return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy"); + } + if (!cartesian_velocity_controller_) { + return Result::failure(ArmErrorCode::RobotNotReady, "cartesian velocity controller is not initialized"); + } + return cartesian_velocity_controller_->speedL(velocity, acceleration, duration, frame); +} + +Result MotorRobotArm::stopL(const double acceleration) +{ + if (!cartesian_velocity_controller_) { + return Result::success(); + } + return cartesian_velocity_controller_->stop(acceleration); +} + +Result MotorRobotArm::stopMotion() +{ + stopL(0.0); + return stopJ(0.0); +} + +Result MotorRobotArm::shutdown() +{ + if (cartesian_velocity_controller_) { + cartesian_velocity_controller_->shutdown(); + } + return stopJ(0.0); +} + +Result MotorRobotArm::startServoMode(const ServoOptions& options) +{ + servo_options_ = options; + return Result::success(); +} + +Result MotorRobotArm::servoJ(const JointPositionCommand& target) +{ + std::string error; + if (!validatePositionCommand_(target, error)) { + return Result::failure(ArmErrorCode::InvalidArgument, error); + } + + std::lock_guard lock(mutex_); + for (std::size_t i = 0; i < joint_names_.size(); ++i) { + auto motor = getMotor_(joint_names_[i]); + if (!motor) { + return Result::failure(ArmErrorCode::RobotNotReady, + "motor not found for joint: " + joint_names_[i]); + } + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + } + motor->setTarget(target.position[i], 0.0); + } + return Result::success(); +} + +Result MotorRobotArm::servoL(const CartesianPose& target, const FrameType frame) +{ + return moveL(target, MotionOptions{0.05, 0.25, 0.0, 2.0}, frame); +} + +Result MotorRobotArm::servoSpeedJ(const JointVelocityCommand& velocity) +{ + return speedJ(velocity, 0.0, 0.0); +} + +Result MotorRobotArm::servoSpeedL(const CartesianVelocity& velocity, const FrameType frame) +{ + return speedL(velocity, 0.25, 0.0, frame); +} + +Result MotorRobotArm::stopServoMode() +{ + return stopJ(0.0); +} + +Result MotorRobotArm::connect(const std::string& ip, const int port) +{ + (void)ip; + (void)port; + return unsupported_("connect"); +} + +Result MotorRobotArm::disconnect() +{ + return unsupported_("disconnect"); +} + +Result MotorRobotArm::loadProgram(const std::string& program_name) +{ + (void)program_name; + return unsupported_("loadProgram"); +} + +Result MotorRobotArm::playProgram() +{ + return unsupported_("playProgram"); +} + +Result MotorRobotArm::pauseProgram() +{ + return unsupported_("pauseProgram"); +} + +Result MotorRobotArm::stopProgram() +{ + return unsupported_("stopProgram"); +} + +std::vector MotorRobotArm::ik(const std::string& base_link, + const std::string& ee_link, + const CartesianPose& pose) +{ + if (!ik_solver_) { + CMVR_LOG(ERROR) << "[MotorRobotArm] IK solver is not initialized"; + return {}; + } + std::vector joints; + if (auto dls_solver = std::dynamic_pointer_cast(ik_solver_)) { + if (!dls_solver->ik(base_link, ee_link, common::math::poseToMatrix(pose), joints)) { + CMVR_LOG(ERROR) << "[MotorRobotArm] ik failed"; + return {}; + } + return joints; + } + + ik_solver_->update_joints_state(readJointPosition_()); + if (!ik_solver_->ik(common::math::poseToMatrix(pose), joints, true)) { + CMVR_LOG(ERROR) << "[MotorRobotArm] ik failed"; + return {}; + } + return joints; +} + +CartesianPose MotorRobotArm::fk(const std::string& base_link, const std::string& ee_link) +{ + if (!ik_solver_) { + CMVR_LOG(ERROR) << "[MotorRobotArm] IK solver is not initialized"; + return {}; + } + std::vector q_current; + std::vector qd_current; + if (!readArmState_(q_current, qd_current)) { + CMVR_LOG(ERROR) << "[MotorRobotArm] failed to read arm state"; + return {}; + } + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); + if (auto pinocchio_solver = std::dynamic_pointer_cast(ik_solver_)) { + if (!pinocchio_solver->fk(base_link, ee_link, q_current, transform)) { + CMVR_LOG(ERROR) << "[MotorRobotArm] fk failed"; + return {}; + } + return common::math::matrixToPose(transform); + } + + if (!ik_solver_->fk(q_current, transform, true)) { + CMVR_LOG(ERROR) << "[MotorRobotArm] fk failed"; + return {}; + } + return common::math::matrixToPose(transform); +} + +CartesianPose MotorRobotArm::fk(const bool is_tcp) +{ + if (!ik_solver_) { + CMVR_LOG(ERROR) << "[MotorRobotArm] IK solver is not initialized"; + return {}; + } + std::vector q_current; + std::vector qd_current; + if (!readArmState_(q_current, qd_current)) { + CMVR_LOG(ERROR) << "[MotorRobotArm] failed to read arm state"; + return {}; + } + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); + if (!ik_solver_->fk(q_current, transform, is_tcp)) { + CMVR_LOG(ERROR) << "[MotorRobotArm] fk failed"; + return {}; + } + return common::math::matrixToPose(transform); +} + +CartesianVelocity MotorRobotArm::getSpeedLCommandTwistBase() const +{ + if (!cartesian_velocity_controller_) { + return {}; + } + return cartesian_velocity_controller_->getCommandTwistBase(); +} + +bool MotorRobotArm::busy() const +{ + return busy_.load() || + (cartesian_velocity_controller_ && cartesian_velocity_controller_->busy()); +} + +bool MotorRobotArm::containsJoint_(const std::string& joint_name) const +{ + return joint_set_.count(joint_name) > 0; +} + +bool MotorRobotArm::validatePositionCommand_(const JointPositionCommand& cmd, std::string& error) const +{ + if (cmd.position.size() != joint_names_.size()) { + error = "[MotorRobotArm] joint position command size mismatch"; + return false; + } + return true; +} + +bool MotorRobotArm::validateVelocityCommand_(const JointVelocityCommand& cmd, std::string& error) const +{ + if (cmd.velocity.size() != joint_names_.size()) { + error = "[MotorRobotArm] joint velocity command size mismatch"; + return false; + } + return true; +} + +std::shared_ptr MotorRobotArm::getMotor_(const std::string& joint_name) const +{ + if (!motor_manager_) { + return nullptr; + } + return motor_manager_->getMotor(joint_name); +} + +bool MotorRobotArm::readArmState_(std::vector& q_now, std::vector& qd_now) const +{ + const auto state = getJointState(); + q_now = state.position; + qd_now = state.velocity; + return q_now.size() == getDof() && qd_now.size() == getDof(); +} + +std::vector MotorRobotArm::readJointPosition_() const +{ + std::vector q_start; + q_start.reserve(joint_names_.size()); + for (const auto& joint_name : joint_names_) { + auto motor = getMotor_(joint_name); + if (!motor) { + CMVR_LOG(ERROR) << "[MotorRobotArm] motor not found for joint: " << joint_name; + return {}; + } + q_start.push_back(motor->getQ()); + } + return q_start; +} + +bool MotorRobotArm::configureAlgorithms_() +{ + joint_planner_ = JointMotionPlannerFactory::create(cfg_.motion().move_j()); + if (!joint_planner_) { + return false; + } + + ik_solver_ = cmvr::IKSolverFactory::create(cfg_.kinematics()); + if (!ik_solver_ || !ik_solver_->init()) { + return false; + } + + auto pinocchio_solver = std::dynamic_pointer_cast(ik_solver_); + if (!pinocchio_solver) { + return false; + } + + const auto& move_l = cfg_.motion().move_l(); + const auto& speed_l = cfg_.motion().speed_l(); + auto cartesian_motion = CartesianMotionPlannerFactory::create(move_l, speed_l, pinocchio_solver); + if (!cartesian_motion) { + return false; + } + const auto* speed_l_config = CartesianMotionPlannerFactory::speedLConfig(speed_l); + const auto* move_l_config = CartesianMotionPlannerFactory::moveLConfig(move_l); + if (!speed_l_config || !move_l_config || + !cartesian_motion->configureMoveL(*move_l_config) || + !cartesian_motion->configureSpeedL(*speed_l_config, getDof())) { + return false; + } + + cartesian_planner_ = cartesian_motion; + cartesian_velocity_controller_ = std::make_unique( + toCartesianVelocityControllerConfig_( + speed_l.speed_l_controller().cartesian_velocity_controller()), + cartesian_planner_, + getDof(), + [this](std::vector& q, std::vector& qd) { + return readArmState_(q, qd); + }, + [this](const JointVelocityCommand& velocity, const double acceleration) { + return speedJ(velocity, acceleration, 0.0); + }); + return true; +} + +bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory) +{ + if (trajectory.position.empty() || trajectory.time.size() != trajectory.position.size()) { + return false; + } + if (trajectory.position.size() == 1) { + return true; + } + + auto next_deadline = std::chrono::steady_clock::now(); + for (std::size_t i = 1; i < trajectory.position.size(); ++i) { + const double dt_segment = std::max(1e-4, trajectory.time[i] - trajectory.time[i - 1]); + JointPositionCommand joint_cmd; + joint_cmd.position = trajectory.position[i]; + const auto result = servoJ(joint_cmd); + if (!result.ok()) { + return false; + } + next_deadline += std::chrono::duration_cast( + std::chrono::duration(dt_segment)); + std::this_thread::sleep_until(next_deadline); + } + + return true; +} + +CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityControllerConfig_( + const config::CartesianVelocityControllerConfig& config) +{ + CartesianVelocityController::Config result; + result.control_period_s = config.control_period_s() > 0.0 ? config.control_period_s() : result.control_period_s; + result.stop_twist_norm = config.stop_twist_norm() > 0.0 ? config.stop_twist_norm() : result.stop_twist_norm; + result.stop_command_velocity_norm = + config.stop_command_velocity_norm() > 0.0 ? config.stop_command_velocity_norm() + : result.stop_command_velocity_norm; + result.stop_measured_velocity_norm = + config.stop_measured_velocity_norm() > 0.0 ? config.stop_measured_velocity_norm() + : result.stop_measured_velocity_norm; + return result; +} + +std::vector MotorRobotArm::withDefaultQdMax_(const std::vector& qd_max, + const std::size_t dof) +{ + if (!qd_max.empty()) { + return qd_max; + } + return std::vector(dof, 2.5); +} + +Result MotorRobotArm::unsupported_(const std::string& name) +{ + return Result::failure(ArmErrorCode::UnsupportedCommand, + name + " is not supported by MotorRobotArm"); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp new file mode 100644 index 00000000..79b468d8 --- /dev/null +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp @@ -0,0 +1,457 @@ +#include "arm/motor_robot_arm/include/motor_robot_arm.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "common/io/proto_file_io.h" +#include "devices/motor/mujoco_motor/include/mujoco_joint_bridge.h" +#include "devices/motor/motor_system/include/motor_system.h" +#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" + +namespace cmvr::device { +namespace { + +constexpr std::size_t kDof = 7; +constexpr std::array kJointNames = { + "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", + "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R" +}; + +std::filesystem::path findProjectRoot() +{ + const std::filesystem::path marker = "model/xiaoyan_description/dual_arm.xml"; + const auto search = [&](std::filesystem::path current) { + while (!current.empty()) { + if (std::filesystem::exists(current / marker)) { + return current; + } + const auto parent = current.parent_path(); + if (parent == current) { + break; + } + current = parent; + } + return std::filesystem::path{}; + }; + + auto root = search(std::filesystem::current_path()); + if (!root.empty()) { + return root; + } + return search(std::filesystem::path(__FILE__).parent_path()); +} + +class MotorRobotArmViewer final : public MuJocoViewer { +public: + MotorRobotArmViewer(const std::string& model_path, + std::shared_ptr bridge) + : MuJocoViewer(model_path.c_str()), bridge_(std::move(bridge)) + { + position_actuator_ids_.fill(-1); + qpos_ids_.fill(-1); + qvel_ids_.fill(-1); + } + +protected: + void initOnce(mjModel* model, mjData* data) override + { + setupCamera(2.5, -160.0, -25.0); + bool valid = true; + for (std::size_t i = 0; i < kDof; ++i) { + const std::string actuator_name = std::string(kJointNames[i]) + "_pos"; + position_actuator_ids_[i] = mj_name2id(model, mjOBJ_ACTUATOR, actuator_name.c_str()); + const int joint_id = mj_name2id(model, mjOBJ_JOINT, kJointNames[i]); + if (position_actuator_ids_[i] < 0 || joint_id < 0) { + valid = false; + continue; + } + qpos_ids_[i] = model->jnt_qposadr[joint_id]; + qvel_ids_[i] = model->jnt_dofadr[joint_id]; + position_reference_[i] = data->qpos[qpos_ids_[i]]; + } + bridge_->markReady(valid); + } + + void controlCallback(mjModel* model, mjData* data) override + { + std::vector measured_position(kDof, 0.0); + std::vector measured_velocity(kDof, 0.0); + for (std::size_t i = 0; i < kDof; ++i) { + if (qpos_ids_[i] >= 0) { + measured_position[i] = data->qpos[qpos_ids_[i]]; + } + if (qvel_ids_[i] >= 0) { + measured_velocity[i] = data->qvel[qvel_ids_[i]]; + } + } + bridge_->publishMeasured(measured_position, measured_velocity); + + const auto commands = bridge_->commands(); + for (std::size_t i = 0; i < kDof; ++i) { + const int actuator_id = position_actuator_ids_[i]; + if (actuator_id < 0) { + continue; + } + + if (commands.mode[i] == msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { + if (last_mode_[i] != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY && qpos_ids_[i] >= 0) { + position_reference_[i] = data->qpos[qpos_ids_[i]]; + } + position_reference_[i] += commands.velocity[i] * model->opt.timestep; + } else { + position_reference_[i] = commands.position[i]; + } + + const double lower = model->actuator_ctrlrange[2 * actuator_id]; + const double upper = model->actuator_ctrlrange[2 * actuator_id + 1]; + position_reference_[i] = std::clamp(position_reference_[i], lower, upper); + data->ctrl[actuator_id] = position_reference_[i]; + last_mode_[i] = commands.mode[i]; + } + } + +private: + std::shared_ptr bridge_; + std::array position_actuator_ids_{}; + std::array qpos_ids_{}; + std::array qvel_ids_{}; + std::array position_reference_{}; + std::array last_mode_{}; +}; + +double maxPositionError(const std::vector& actual, + const std::vector& expected) +{ + if (actual.size() != expected.size()) { + return std::numeric_limits::infinity(); + } + double error = 0.0; + for (std::size_t i = 0; i < actual.size(); ++i) { + error = std::max(error, std::abs(actual[i] - expected[i])); + } + return error; +} + +double translationError(const CartesianPose& lhs, const CartesianPose& rhs) +{ + return std::sqrt(std::pow(lhs.x - rhs.x, 2.0) + + std::pow(lhs.y - rhs.y, 2.0) + + std::pow(lhs.z - rhs.z, 2.0)); +} + +template +void waitFor(Predicate predicate, const std::chrono::milliseconds timeout) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (!predicate() && std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } +} + +struct ScenarioOutcome { + Result move_j_initial{Result::failure(ArmErrorCode::UnknownError, "not run")}; + Result move_l{Result::failure(ArmErrorCode::UnknownError, "not run")}; + Result speed_l{Result::failure(ArmErrorCode::UnknownError, "not run")}; + double move_j_initial_error{std::numeric_limits::infinity()}; + double move_l_error{std::numeric_limits::infinity()}; + double speed_l_command_x{0.0}; + double speed_l_delta_x{0.0}; + std::string worker_error; +}; + +struct ArmMujocoConfigCase { + const char* name; + const char* config_file; +}; + +void PrintTo(const ArmMujocoConfigCase& value, std::ostream* os) +{ + *os << value.name << " (" << value.config_file << ")"; +} + +void setKinematicsUrdfPath(config::RobotArmConfig& arm_config, + const std::string& urdf_path) +{ + switch (arm_config.kinematics().algorithm_case()) { + case config::ArmKinematicsConfig::kPinocchioDlsIkSolver: + arm_config.mutable_kinematics() + ->mutable_pinocchio_dls_ik_solver() + ->set_urdf_path(urdf_path); + break; + case config::ArmKinematicsConfig::kPinocchioQpIkSolver: + arm_config.mutable_kinematics() + ->mutable_pinocchio_qp_ik_solver() + ->set_urdf_path(urdf_path); + break; + default: + break; + } +} + +class MotorRobotArmMujocoTest : public ::testing::TestWithParam { +protected: + void SetUp() override + { + project_root_ = findProjectRoot(); + ASSERT_FALSE(project_root_.empty()); + + config::MotorRootConfig motor_root_config; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root_ / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt").string(), + &motor_root_config)); + + std::unordered_set right_arm_joints; + for (const auto* joint_name : kJointNames) { + right_arm_joints.insert(joint_name); + } + MotorSystem::clearActiveJoints(); + MotorSystem::setActiveJoints( + "mujoco_motors", {{"right_arm_mujoco", std::move(right_arm_joints)}}); + + motor_system_ = std::make_shared("mujoco_motors", motor_root_config.motor()); + ASSERT_NO_THROW(motor_system_->init()); + bridge_ = MotorSystem::mujocoBridgeFor("mujoco_motors"); + ASSERT_TRUE(bridge_); + + config::ArmRootConfig root_config; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root_ / "cmvr-es/config/devices/arm" / GetParam().config_file).string(), + &root_config)); + ASSERT_GT(root_config.arm().robot_arms_size(), 0); + + config::RobotArmConfig arm_config = root_config.arm().robot_arms(0); + setKinematicsUrdfPath( + arm_config, + (project_root_ / "model/xiaoyan_description/dual_arm.urdf").string()); + + arm_ = std::make_unique(arm_config); + ASSERT_NO_THROW(arm_->init()); + } + + void TearDown() override + { + if (arm_) { + arm_->stop(); + } + if (motor_system_) { + motor_system_->stop(); + } + MotorSystem::clearActiveJoints(); + } + + std::filesystem::path project_root_; + std::shared_ptr motor_system_; + std::shared_ptr bridge_; + std::unique_ptr arm_; +}; + +TEST_P(MotorRobotArmMujocoTest, MoveJ) +{ + MotorRobotArm& arm = *arm_; + + MotorRobotArmViewer viewer( + (project_root_ / "model/xiaoyan_description/dual_arm.xml").string(), bridge_); + ScenarioOutcome outcome; + + std::thread scenario([&] { + try { + if (!bridge_->waitUntilReady(std::chrono::seconds(10))) { + throw std::runtime_error("MuJoCo right-arm joints or actuators are not ready"); + } + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + const std::vector move_j_initial{ + 0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0.0, 0.0 + }; + MotionOptions joint_options; + joint_options.velocity = 2.8; + joint_options.acceleration = 20.0; + + std::cout << "[MotorRobotArmMujocoTest] moveJ initial pose" << std::endl; + outcome.move_j_initial = arm.moveJ(JointPositionCommand{move_j_initial}, joint_options); + waitFor([&] { + return maxPositionError(arm.getJointState().position, move_j_initial) < 0.04; + }, std::chrono::seconds(2)); + outcome.move_j_initial_error = maxPositionError(arm.getJointState().position, move_j_initial); + } catch (const std::exception& error) { + outcome.worker_error = error.what(); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + viewer.requestStop(); + }); + + viewer.setRunning(true); + viewer.run(); + scenario.join(); + + std::cout << "[MotorRobotArmMujocoTest] moveJ initial max error: " + << outcome.move_j_initial_error << std::endl; + + EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error; + EXPECT_TRUE(outcome.move_j_initial.ok()) << outcome.move_j_initial.message; + EXPECT_LT(outcome.move_j_initial_error, 0.10); +} + +TEST_P(MotorRobotArmMujocoTest, MoveL) +{ + MotorRobotArm& arm = *arm_; + + MotorRobotArmViewer viewer( + (project_root_ / "model/xiaoyan_description/dual_arm.xml").string(), bridge_); + ScenarioOutcome outcome; + + std::thread scenario([&] { + try { + if (!bridge_->waitUntilReady(std::chrono::seconds(10))) { + throw std::runtime_error("MuJoCo right-arm joints or actuators are not ready"); + } + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + const std::vector move_j_initial{ + 0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0.0, 0.0 + }; + MotionOptions joint_options; + joint_options.velocity = 2.8; + joint_options.acceleration = 20.0; + + std::cout << "[MotorRobotArmMujocoTest] moveJ setup pose" << std::endl; + outcome.move_j_initial = arm.moveJ(JointPositionCommand{move_j_initial}, joint_options); + waitFor([&] { + return maxPositionError(arm.getJointState().position, move_j_initial) < 0.04; + }, std::chrono::seconds(2)); + outcome.move_j_initial_error = maxPositionError(arm.getJointState().position, move_j_initial); + + std::this_thread::sleep_for(std::chrono::seconds(2)); + + const CartesianPose move_l_start = arm.getTcpPose(); + CartesianPose move_l_target = move_l_start; + move_l_target.x += 0.15; + + MotionOptions cartesian_options; + cartesian_options.velocity = 0.8; + cartesian_options.acceleration = 20; + cartesian_options.jerk = 100.0; + + std::cout << "[MotorRobotArmMujocoTest] moveL target pose" << std::endl; + outcome.move_l = arm.moveL(move_l_target, cartesian_options, FrameType::Base); + waitFor([&] { + return translationError(arm.getTcpPose(), move_l_target) < 0.01; + }, std::chrono::seconds(2)); + outcome.move_l_error = translationError(arm.getTcpPose(), move_l_target); + } catch (const std::exception& error) { + outcome.worker_error = error.what(); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(10000)); + viewer.requestStop(); + }); + + viewer.setRunning(true); + viewer.run(); + scenario.join(); + + std::cout << "[MotorRobotArmMujocoTest] moveJ setup max error: " + << outcome.move_j_initial_error + << ", moveL translation error: " << outcome.move_l_error << std::endl; + + EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error; + EXPECT_TRUE(outcome.move_j_initial.ok()) << outcome.move_j_initial.message; + EXPECT_LT(outcome.move_j_initial_error, 0.10); + EXPECT_TRUE(outcome.move_l.ok()) << outcome.move_l.message; + EXPECT_LT(outcome.move_l_error, 0.04); +} + +TEST_P(MotorRobotArmMujocoTest, SpeedL) +{ + MotorRobotArm& arm = *arm_; + + MotorRobotArmViewer viewer( + (project_root_ / "model/xiaoyan_description/dual_arm.xml").string(), bridge_); + ScenarioOutcome outcome; + + std::thread scenario([&] { + try { + if (!bridge_->waitUntilReady(std::chrono::seconds(10))) { + throw std::runtime_error("MuJoCo right-arm joints or actuators are not ready"); + } + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + const std::vector move_j_initial{ + 0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0.0, 0.0 + }; + MotionOptions joint_options; + joint_options.velocity = 2.8; + joint_options.acceleration = 20.0; + + std::cout << "[MotorRobotArmMujocoTest] moveJ setup pose" << std::endl; + outcome.move_j_initial = arm.moveJ(JointPositionCommand{move_j_initial}, joint_options); + waitFor([&] { + return maxPositionError(arm.getJointState().position, move_j_initial) < 0.04; + }, std::chrono::seconds(2)); + outcome.move_j_initial_error = maxPositionError(arm.getJointState().position, move_j_initial); + + std::this_thread::sleep_for(std::chrono::seconds(2)); + + const CartesianPose speed_l_start = arm.getTcpPose(); + CartesianVelocity cartesian_velocity; + cartesian_velocity.vx = 0.05; + + std::cout << "[MotorRobotArmMujocoTest] speedL" << std::endl; + outcome.speed_l = arm.speedL(cartesian_velocity, 0.5, 0.0, FrameType::Base); + std::this_thread::sleep_for(std::chrono::seconds(1)); + outcome.speed_l_command_x = arm.getSpeedLCommandTwistBase().vx; + outcome.speed_l_delta_x = arm.getTcpPose().x - speed_l_start.x; + (void)arm.stopL(0.5); + waitFor([&] { return !arm.busy(); }, std::chrono::seconds(3)); + } catch (const std::exception& error) { + outcome.worker_error = error.what(); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + viewer.requestStop(); + }); + + viewer.setRunning(true); + viewer.run(); + scenario.join(); + + std::cout << "[MotorRobotArmMujocoTest] moveJ setup max error: " + << outcome.move_j_initial_error + << ", speedL command x: " << outcome.speed_l_command_x + << ", speedL x delta: " << outcome.speed_l_delta_x << std::endl; + + EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error; + EXPECT_TRUE(outcome.move_j_initial.ok()) << outcome.move_j_initial.message; + EXPECT_LT(outcome.move_j_initial_error, 0.10); + EXPECT_TRUE(outcome.speed_l.ok()) << outcome.speed_l.message; + EXPECT_GT(outcome.speed_l_command_x, 0.01); + EXPECT_GT(outcome.speed_l_delta_x, 0.003); +} + +INSTANTIATE_TEST_SUITE_P( + ArmPlannerCombinations, + MotorRobotArmMujocoTest, + ::testing::Values( + ArmMujocoConfigCase{"DlsIkDlsMotion", "arm_mujoco_dls_ik_dls_motion.pb.txt"}, + ArmMujocoConfigCase{"DlsIkQpMotion", "arm_mujoco_dls_ik_qp_motion.pb.txt"}, + ArmMujocoConfigCase{"QpIkQpMotion", "arm_mujoco_qp_ik_qp_motion.pb.txt"}), + [](const ::testing::TestParamInfo& info) { + return std::string(info.param.name); + }); + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h new file mode 100644 index 00000000..d6c255c9 --- /dev/null +++ b/cmvr-es/devices/arm/robot_arm.h @@ -0,0 +1,102 @@ +#ifndef CMVR_ES_ROBOT_ARM_H +#define CMVR_ES_ROBOT_ARM_H + +#include +#include +#include +#include + +#include "algorithms/kinematics/ik_solver/common/include/ik_solver.h" +#include "common/types/arm/arm_types.h" +#include "devices/abstract_device.h" + +namespace cmvr::device { + +class RobotArm : public AbstractDevice { +public: + ~RobotArm() override = default; + + DeviceKind kind() const noexcept override { return DeviceKind::Arm; } + + virtual RobotModel getRobotModel() const = 0; + virtual std::size_t getDof() const = 0; + virtual ArmState getRobotState() const = 0; + virtual JointGroupState getJointState() const = 0; + virtual CartesianPose getTcpPose(FrameType frame = FrameType::Base) const = 0; + virtual RobotMode getRobotMode() const = 0; + virtual SafetyMode getSafetyMode() const = 0; + virtual ControlMode getControlMode() const = 0; + + virtual Result torqueOn() = 0; + virtual Result torqueOff() = 0; + virtual Result calibrateZeroQ(const std::string& joint_name) = 0; + + virtual Result emergencyStop() = 0; + virtual Result protectiveStop() = 0; + virtual Result setSpeedScaling(double scaling) = 0; + virtual double getSpeedScaling() const = 0; + virtual bool isProtectiveStopped() const = 0; + virtual bool isEmergencyStopped() const = 0; + virtual bool isFault() const = 0; + + virtual Result moveJ(const JointPositionCommand& target, + const MotionOptions& options) = 0; + virtual Result speedJ(const JointVelocityCommand& velocity, + double acceleration, + double duration) = 0; + virtual Result stopJ(double acceleration) = 0; + + virtual Result moveL(const CartesianPose& target, + const MotionOptions& options, + FrameType frame = FrameType::Base) = 0; + virtual Result speedL(const CartesianVelocity& velocity, + double acceleration, + double duration, + FrameType frame = FrameType::Base) = 0; + virtual Result stopL(double acceleration) = 0; + virtual Result stopMotion() = 0; + + virtual Result moveP(const CartesianPose& target, + const MotionOptions& options, + FrameType frame = FrameType::Base) + { + return moveL(target, options, frame); + } + + virtual Result startServoMode(const ServoOptions& options) = 0; + virtual Result servoJ(const JointPositionCommand& target) = 0; + virtual Result servoL(const CartesianPose& target, + FrameType frame = FrameType::Base) = 0; + virtual Result servoSpeedJ(const JointVelocityCommand& velocity) = 0; + virtual Result servoSpeedL(const CartesianVelocity& velocity, + FrameType frame = FrameType::Base) = 0; + virtual Result stopServoMode() = 0; + + virtual Result connect(const std::string& ip, int port) = 0; + virtual Result disconnect() = 0; + virtual bool isConnected() const = 0; + virtual Result powerOn() = 0; + virtual Result powerOff() = 0; + virtual Result brakeRelease() = 0; + virtual Result shutdown() = 0; + virtual Result clearFault() = 0; + virtual Result unlockProtectiveStop() = 0; + virtual Result loadProgram(const std::string& program_name) = 0; + virtual Result playProgram() = 0; + virtual Result pauseProgram() = 0; + virtual Result stopProgram() = 0; + + virtual std::vector ik(const std::string& base_link, + const std::string& ee_link, + const CartesianPose& pose) = 0; + virtual std::shared_ptr kinematicsSolver() const = 0; + virtual CartesianPose fk(const std::string& base_link, + const std::string& ee_link) = 0; + virtual CartesianPose fk(bool is_tcp = true) = 0; + virtual CartesianVelocity getSpeedLCommandTwistBase() const = 0; + virtual bool busy() const = 0; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_ROBOT_ARM_H diff --git a/cmvr-es/devices/arm/robot_arm_factory.h b/cmvr-es/devices/arm/robot_arm_factory.h new file mode 100644 index 00000000..bd73f19a --- /dev/null +++ b/cmvr-es/devices/arm/robot_arm_factory.h @@ -0,0 +1,49 @@ +#ifndef CMVR_ES_ROBOT_ARM_FACTORY_H +#define CMVR_ES_ROBOT_ARM_FACTORY_H + +#include +#include + +#include "cmvr/config/arm_config/arm_config.pb.h" +#include "common/base/logging/logger.h" +#include "devices/arm/aubo_arm/include/aubo_arm.h" +#include "devices/arm/motor_robot_arm/include/motor_robot_arm.h" + +namespace cmvr::device { + +class RobotArmFactory { +public: + static std::shared_ptr create(const config::RobotArmConfig& cfg) + { + switch (cfg.backend_case()) { + case config::RobotArmConfig::kMotor: + return std::make_shared(cfg); + + case config::RobotArmConfig::kVendor: + { + const auto& vendor = cfg.vendor(); + if (vendor.brand() == config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM) { + return std::make_shared(cfg); + } + const std::string error = + "[RobotArmFactory]: Vendor RobotArm is not implemented yet: " + cfg.id() + + ", brand=" + config::VendorRobotArmBrand_Name(vendor.brand()); + CMVR_LOG(ERROR) << error; + return nullptr; + } + + case config::RobotArmConfig::BACKEND_NOT_SET: + default: + { + const std::string error = + "[RobotArmFactory]: RobotArm backend is not configured: " + cfg.id(); + CMVR_LOG(ERROR) << error; + return nullptr; + } + } + } +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_ROBOT_ARM_FACTORY_H diff --git a/cmvr-es/devices/battery/abstract_battery.h b/cmvr-es/devices/battery/abstract_battery.h index bc0ed727..135479ea 100644 --- a/cmvr-es/devices/battery/abstract_battery.h +++ b/cmvr-es/devices/battery/abstract_battery.h @@ -11,9 +11,10 @@ namespace cmvr::device{ class AbstractBattery: public AbstractDevice { public: - explicit AbstractBattery(const XmlNode &config): AbstractDevice(config) {} + AbstractBattery() = default; ~AbstractBattery() override =default; + DeviceKind kind() const noexcept override { return DeviceKind::Battery; } virtual void getState(BatteryState &state) {} protected: @@ -22,4 +23,4 @@ namespace cmvr::device{ }; } -#endif //CMVR_ES_ABSTRACT_BATTERY_H \ No newline at end of file +#endif //CMVR_ES_ABSTRACT_BATTERY_H diff --git a/cmvr-es/devices/biohead/abstract_biohead.h b/cmvr-es/devices/biohead/abstract_biohead.h index 93e0eb20..aaf28658 100644 --- a/cmvr-es/devices/biohead/abstract_biohead.h +++ b/cmvr-es/devices/biohead/abstract_biohead.h @@ -58,9 +58,10 @@ namespace cmvr::device { // 抽象头部类 class AbstractBiohead : public AbstractDevice { public: - explicit AbstractBiohead(const XmlNode &config) : AbstractDevice(config) {} + AbstractBiohead() = default; ~AbstractBiohead() override = default; + DeviceKind kind() const noexcept override { return DeviceKind::BioHead; } virtual void getState(RobotState &state){}; virtual void eStop() {}; virtual void setExpressionPose(FacialExpressionState& expression_state, double vel=0, double acc=0) {} @@ -92,5 +93,3 @@ namespace cmvr::device { - - diff --git a/cmvr-es/devices/biohead/biohead_esp32/CMakeLists.txt b/cmvr-es/devices/biohead/biohead_esp32/CMakeLists.txt index c21364f2..a99ff3c2 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/CMakeLists.txt +++ b/cmvr-es/devices/biohead/biohead_esp32/CMakeLists.txt @@ -5,6 +5,7 @@ add_library(head_esp32 SHARED src/biohead_esp32.cpp) target_include_directories(head_esp32 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(head_esp32 PRIVATE cmvr_es::hardware + cmvr_es::proto ) # 创建命名空间化的 ALIAS（建议使用项目命名空间） add_library(cmvr::device::head_esp32 ALIAS head_esp32) @@ -13,4 +14,3 @@ install(TARGETS head_esp32 LIBRARY DESTINATION lib) - diff --git a/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h b/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h index 52b34d5d..61d301ba 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h +++ b/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h @@ -3,6 +3,7 @@ #include "../../abstract_biohead.h" #include "../../../../hardware/include/esp32_serial_port.h" +#include "cmvr/config/biohead_config/biohead_config.pb.h" #include #include #include @@ -17,9 +18,11 @@ namespace cmvr::device { class BioHeadRobot : public AbstractBiohead { public: - explicit BioHeadRobot(const XmlNode &config); + explicit BioHeadRobot(const config::BioHeadRobotConfig &config); ~BioHeadRobot() override = default; + std::string typeName() const override { return "BioHeadRobot"; } + bool init() override; // 接口实现 void getState(RobotState &state) override; void eStop() override; @@ -39,7 +42,7 @@ namespace cmvr::device { private: // 内部方法 - void parseXmlConfig(const XmlNode &node); + void parseConfig(const config::BioHeadRobotConfig &config); void sendServoCommands( const std::vector& targets, uint16_t duration_ms); uint16_t angleToRaw(double angle); double normalizeToAngle(double normalized, size_t index); @@ -51,6 +54,7 @@ namespace cmvr::device { std::shared_ptr serial_; std::string port_name_; + int baud_rate_{115200}; std::vector channels_; std::vector current_joints_; @@ -73,5 +77,3 @@ namespace cmvr::device { } // namespace cmvr::device #endif // CMVR_ES_BIOHEAD_ESP32_H - - diff --git a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp b/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp index c3b67887..5e86e722 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp +++ b/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp @@ -1,5 +1,5 @@ #include "../include/biohead_esp32.h" -#include +#include "common/base/logging/logger.h" #include #include #include @@ -8,15 +8,10 @@ namespace cmvr::device { -BioHeadRobot::BioHeadRobot(const XmlNode &config) : AbstractBiohead(config) { - parseXmlConfig(config); +BioHeadRobot::BioHeadRobot(const config::BioHeadRobotConfig &config) { + parseConfig(config); serial_ = std::make_shared(); - if (!serial_->open(port_name_, 115200)) { - LOG(ERROR) << "[BioHeadRobot] Failed to open serial port: " << port_name_; - } else { - serial_->wakeupESP32(port_name_); - LOG(INFO) << "[BioHeadRobot] Serial port opened and ESP32 woken up."; - } + baud_rate_ = config.baud_rate() > 0 ? config.baud_rate() : 115200; current_joints_.resize(channels_.size(), 90.0); // 默认初始化为90° last_joints_ = current_joints_; // 死区缓存同步 @@ -28,47 +23,37 @@ BioHeadRobot::BioHeadRobot(const XmlNode &config) : AbstractBiohead(config) { -void BioHeadRobot::parseXmlConfig(const XmlNode &node) { - port_name_ = node.getAttrString("serial"); - for (const auto &child : node.getChildren()) { - std::string type = child.getNodeName(); - std::string serial_str = child.getAttrString("serial"); - std::string offset_str = child.getAttrString("offset"); - std::string min_angles_str = child.getAttrDefault("jLmtLow", ""); - std::string max_angles_str = child.getAttrDefault("jLmtUp", ""); +bool BioHeadRobot::init() { + if (!serial_) { + serial_ = std::make_shared(); + } + if (!serial_->open(port_name_, baud_rate_)) { + CMVR_LOG(ERROR) << "[BioHeadRobot] (init): Failed to open serial port: " << port_name_; + return false; + } + serial_->wakeupESP32(port_name_); + CMVR_LOG(INFO) << "[BioHeadRobot] (init): Serial port opened and ESP32 woken up."; - // 解析偏移角 - std::vector offsets; - std::istringstream oss(offset_str); - double val; - while (oss >> val) offsets.push_back(val); + if (!serial_->is_open()) { + CMVR_LOG(ERROR) << "[BioHeadRobot] (init): Serial port is not open: " << port_name_; + return false; + } + return true; +} - // 解析角度限制 - std::vector min_angles; - if (!min_angles_str.empty()) { - std::istringstream min_ss(min_angles_str); - - while (min_ss >> val) min_angles.push_back(val); +void BioHeadRobot::parseConfig(const config::BioHeadRobotConfig &config) { + id_ = config.id(); + port_name_ = config.serial_port(); + for (const auto &group : config.servo_groups()) { + const uint8_t addr = static_cast(group.address()); + int ch_start = group.channel_start(); + int ch_end = group.channel_end(); + if (ch_end < ch_start) { + continue; } - std::vector max_angles; - if (!max_angles_str.empty()) { - std::istringstream max_ss(max_angles_str); - while (max_ss >> val) max_angles.push_back(val); - } - - // 解析 channel，如 0:4~9 - size_t colon = serial_str.find(':'); - if (colon == std::string::npos) continue; - uint8_t addr = std::stoi(serial_str.substr(0, colon)); - std::string range = serial_str.substr(colon + 1); - size_t tilde = range.find('~'); - if (tilde == std::string::npos) continue; - int ch_start = std::stoi(range.substr(0, tilde)); - int ch_end = std::stoi(range.substr(tilde + 1)); - int channel_count = ch_end - ch_start + 1; // 添加舵机通道 @@ -77,18 +62,18 @@ void BioHeadRobot::parseXmlConfig(const XmlNode &node) { } // 添加角度限制 - if (min_angles.size() == 1) { - min_angles_.insert(min_angles_.end(), channel_count, min_angles[0]); - } else if (min_angles.size() >= channel_count) { - min_angles_.insert(min_angles_.end(), min_angles.begin(), min_angles.begin() + channel_count); + if (group.min_angles_size() == 1) { + min_angles_.insert(min_angles_.end(), channel_count, group.min_angles(0)); + } else if (group.min_angles_size() >= channel_count) { + min_angles_.insert(min_angles_.end(), group.min_angles().begin(), group.min_angles().begin() + channel_count); } else { min_angles_.insert(min_angles_.end(), channel_count, 0.0); } - if (max_angles.size() == 1) { - max_angles_.insert(max_angles_.end(), channel_count, max_angles[0]); - } else if (max_angles.size() >= channel_count) { - max_angles_.insert(max_angles_.end(), max_angles.begin(), max_angles.begin() + channel_count); + if (group.max_angles_size() == 1) { + max_angles_.insert(max_angles_.end(), channel_count, group.max_angles(0)); + } else if (group.max_angles_size() >= channel_count) { + max_angles_.insert(max_angles_.end(), group.max_angles().begin(), group.max_angles().begin() + channel_count); } else { max_angles_.insert(max_angles_.end(), channel_count, 180.0); } @@ -107,7 +92,7 @@ void BioHeadRobot::parseXmlConfig(const XmlNode &node) { double BioHeadRobot::normalizeToAngle(double normalized, size_t index) { if (index >= min_angles_.size() || index >= max_angles_.size()) { - LOG(ERROR) << "Invalid index for angle conversion: " << index; + CMVR_LOG(ERROR) << "Invalid index for angle conversion: " << index; return 90.0; // 默认值 } @@ -132,7 +117,7 @@ void BioHeadRobot::getState(RobotState &state) { } void BioHeadRobot::eStop() { - LOG(WARNING) << "[BioHeadRobot] Emergency stop: hold current joint positions."; + CMVR_LOG(WARNING) << "[BioHeadRobot] Emergency stop: hold current joint positions."; sendServoCommands(current_joints_, 100); // 快速下发当前角度 } @@ -173,7 +158,7 @@ void BioHeadRobot::setExpressionPose(FacialExpressionState& expression_state, do joints[18] = normalizeToAngle(expression_state.jaw_x, 18); joints[19] = normalizeToAngle(expression_state.jaw_y, 19); for (size_t i = 0; i < joints.size(); ++i) { - LOG(INFO) << "Joint[" << i << "] = " << joints[i]; // 打印每个关节的角度 + CMVR_LOG(INFO) << "Joint[" << i << "] = " << joints[i]; // 打印每个关节的角度 } uint16_t duration = static_cast(1000.0 / vel); sendServoCommands(joints, duration); @@ -218,10 +203,10 @@ void BioHeadRobot::streamFacialPose(FacialExpressionState& expression_state, dou double jaw_value = expression_state.jaw_x; joints[18] = normalizeToAngle(jaw_value, 18); joints[19] = normalizeToAngle(1-jaw_value,19); - std::cout << "嘴角1=： " << ": " << joints[12] << std::endl; - std::cout << "嘴角2=： " << ": " << joints[13] << std::endl; - std::cout << "嘴角3=： " << ": " << joints[15] << std::endl; - std::cout << "嘴角4=： " << ": " << joints[16] << std::endl; + CMVR_LOG(INFO) << "嘴角1=： " << ": " << joints[12]; + CMVR_LOG(INFO) << "嘴角2=： " << ": " << joints[13]; + CMVR_LOG(INFO) << "嘴角3=： " << ": " << joints[15]; + CMVR_LOG(INFO) << "嘴角4=： " << ": " << joints[16]; uint16_t duration = static_cast(1000.0 / vel); sendServoCommands(joints, duration); @@ -232,7 +217,7 @@ void BioHeadRobot::streamFacialPose(FacialExpressionState& expression_state, dou void BioHeadRobot::speakstart() { if (speak_running_.load()) { - LOG(INFO) << "[BioHeadRobot] speak thread already running."; + CMVR_LOG(INFO) << "[BioHeadRobot] speak thread already running."; return; } @@ -243,7 +228,7 @@ void BioHeadRobot::speakstart() { if (channels_[i].addr == 65 && channels_[i].channel == 9) found9 = true; } if (!found8 || !found9) { - LOG(ERROR) << "[BioHeadRobot] Required servo channels not found (addr 65 ch 8/9). speakstart aborted."; + CMVR_LOG(ERROR) << "[BioHeadRobot] Required servo channels not found (addr 65 ch 8/9). speakstart aborted."; return; } @@ -255,19 +240,19 @@ void BioHeadRobot::speakstart() { try { speak_thread_->join(); } catch (...) { - LOG(WARNING) << "[BioHeadRobot] exception joining previous speak thread."; + CMVR_LOG(WARNING) << "[BioHeadRobot] exception joining previous speak thread."; } speak_thread_.reset(); } speak_thread_ = std::make_shared(&BioHeadRobot::speakthread, this); - LOG(INFO) << "[BioHeadRobot] speak thread started."; + CMVR_LOG(INFO) << "[BioHeadRobot] speak thread started."; } void BioHeadRobot::speakstop() { { if (!speak_running_.load()) { - LOG(INFO) << "[BioHeadRobot] speak thread not running."; + CMVR_LOG(INFO) << "[BioHeadRobot] speak thread not running."; return; } speak_running_.store(false); @@ -280,18 +265,18 @@ void BioHeadRobot::speakstop() { try { speak_thread_->join(); } catch (const std::exception &e) { - LOG(WARNING) << "[BioHeadRobot] exception when joining speak thread: " << e.what(); + CMVR_LOG(WARNING) << "[BioHeadRobot] exception when joining speak thread: " << e.what(); } catch (...) { - LOG(WARNING) << "[BioHeadRobot] unknown exception when joining speak thread."; + CMVR_LOG(WARNING) << "[BioHeadRobot] unknown exception when joining speak thread."; } } speak_thread_.reset(); } - LOG(INFO) << "[BioHeadRobot] speak thread stopped."; + CMVR_LOG(INFO) << "[BioHeadRobot] speak thread stopped."; } void BioHeadRobot::speakthread() { - LOG(INFO) << "[BioHeadRobot] speakthread running."; + CMVR_LOG(INFO) << "[BioHeadRobot] speakthread running."; // 固定参数 const double freq = 3.0; // Hz @@ -322,7 +307,7 @@ void BioHeadRobot::speakthread() { } if (idx8 < 0 || idx9 < 0) { - LOG(ERROR) << "[BioHeadRobot] speakthread: required channels (65:8/9) not found. Exiting thread."; + CMVR_LOG(ERROR) << "[BioHeadRobot] speakthread: required channels (65:8/9) not found. Exiting thread."; speak_running_.store(false); return; } @@ -373,7 +358,7 @@ void BioHeadRobot::speakthread() { blink_start_time = now; blink_duration = simple_rand(0.1, 0.3); // 眨眼持续时间0.1-0.3秒 eyes_open = false; - LOG(INFO) << "[BioHeadRobot] Starting blink, duration: " << blink_duration << "s"; + CMVR_LOG(INFO) << "[BioHeadRobot] Starting blink, duration: " << blink_duration << "s"; } } @@ -387,7 +372,7 @@ void BioHeadRobot::speakthread() { eyes_open = true; last_blink_time = now; next_blink_interval = simple_rand(2.0, 8.0); - LOG(INFO) << "[BioHeadRobot] Blink completed, next blink in: " << next_blink_interval << "s"; + CMVR_LOG(INFO) << "[BioHeadRobot] Blink completed, next blink in: " << next_blink_interval << "s"; } } @@ -463,7 +448,7 @@ void BioHeadRobot::speakthread() { if (i < raw_data.size() - 1) json_output += ", "; } json_output += "] }"; - std::cout << "Sending data: " << json_output << std::endl; + CMVR_LOG(INFO) << "Sending data: " << json_output; } // 下发 @@ -499,7 +484,7 @@ void BioHeadRobot::speakthread() { } serial_->sendRawServoData(restore_data); - LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose."; + CMVR_LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose."; } @@ -651,7 +636,7 @@ void BioHeadRobot::sendServoCommands(const std::vector& targets, uint16_ json_output += "] }"; // 打印发送的数据包 - std::cout << "Sending data at step " << ": " << json_output << std::endl; + CMVR_LOG(INFO) << "Sending data at step " << ": " << json_output; @@ -667,6 +652,3 @@ uint16_t BioHeadRobot::angleToRaw(double angle) { } // namespace cmvr::device - - - diff --git a/cmvr-es/devices/camera/CMakeLists.txt b/cmvr-es/devices/camera/CMakeLists.txt index 30677cdd..8013eb6a 100644 --- a/cmvr-es/devices/camera/CMakeLists.txt +++ b/cmvr-es/devices/camera/CMakeLists.txt @@ -2,3 +2,16 @@ add_subdirectory(uvc_camera) add_subdirectory(realsense_camera) add_subdirectory(mujoco_camera) + +add_library(camera INTERFACE) + +target_include_directories(camera INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(camera + INTERFACE + cmvr_es::device::uvc_camera + cmvr_es::device::realsense_camera + cmvr_es::proto +) + +add_library(cmvr_es::device::camera ALIAS camera) diff --git a/cmvr-es/devices/camera/abstract_camera.h b/cmvr-es/devices/camera/abstract_camera.h index 0830385a..9a9bd728 100644 --- a/cmvr-es/devices/camera/abstract_camera.h +++ b/cmvr-es/devices/camera/abstract_camera.h @@ -46,9 +46,9 @@ namespace cmvr::device { }; public: AbstractCamera() = default; - explicit AbstractCamera(const XmlNode &cfg): AbstractDevice(cfg) {} ~AbstractCamera() override = default; + DeviceKind kind() const noexcept override { return DeviceKind::Camera; } inline void getState(CameraState &state) {state = state_;} virtual void getRGBImage(cv::Mat &color, Rs2Intrinsics& intrinsics) {} virtual void getDepthImage(cv::Mat &depth, Rs2Intrinsics& intrinsics) {} diff --git a/cmvr-es/devices/camera/camera_factory.h b/cmvr-es/devices/camera/camera_factory.h new file mode 100644 index 00000000..600c0bb5 --- /dev/null +++ b/cmvr-es/devices/camera/camera_factory.h @@ -0,0 +1,80 @@ +#ifndef CMVR_ES_CAMERA_FACTORY_H +#define CMVR_ES_CAMERA_FACTORY_H + +#include +#include +#include + +#include "cmvr/config/camera_config/camera_config.pb.h" +#include "common/base/logging/logger.h" +#include "devices/camera/abstract_camera.h" +#include "devices/camera/realsense_camera/include/realsense_camera.h" +#include "devices/camera/uvc_camera/include/uvc_camera.h" + +namespace cmvr::device { + +class CameraFactory { +public: + static std::shared_ptr create(const config::CameraDeviceConfig& cfg) + { + try { + if (cfg.id().empty()) { + const std::string error = "[CameraFactory]: Camera id is empty"; + CMVR_LOG(ERROR) << error; + return nullptr; + } + + switch (cfg.backend_case()) { + case config::CameraDeviceConfig::kUvc: + return std::make_shared(backendWithId_(cfg.id(), cfg.uvc())); + + case config::CameraDeviceConfig::kRealsense: + return std::make_shared( + backendWithId_(cfg.id(), cfg.realsense())); + + case config::CameraDeviceConfig::kMechmind: + { + const std::string error = + "[CameraFactory]: MechMind camera is not enabled in this build: " + cfg.id(); + CMVR_LOG(ERROR) << error; + return nullptr; + } + + case config::CameraDeviceConfig::BACKEND_NOT_SET: + default: + { + const std::string error = + "[CameraFactory]: Camera backend is not configured: " + cfg.id(); + CMVR_LOG(ERROR) << error; + return nullptr; + } + } + } catch (const std::exception& e) { + CMVR_LOG(ERROR) << "[CameraFactory]: Failed to create camera " << cfg.id() + << ": " << e.what(); + return nullptr; + } catch (...) { + CMVR_LOG(ERROR) << "[CameraFactory]: Failed to create camera " << cfg.id() + << ": unknown exception"; + return nullptr; + } + } + +private: + template + static BackendConfig backendWithId_(const std::string& id, const BackendConfig& source) + { + if (!source.id().empty() && source.id() != id) { + const std::string error = "[CameraFactory]: Camera id does not match backend id: " + id; + CMVR_LOG(ERROR) << error; + return BackendConfig{}; + } + BackendConfig backend = source; + backend.set_id(id); + return backend; + } +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_CAMERA_FACTORY_H diff --git a/cmvr-es/devices/camera/mechmind/CMakeLists.txt b/cmvr-es/devices/camera/mechmind/CMakeLists.txt index 736e2a02..80d926f5 100644 --- a/cmvr-es/devices/camera/mechmind/CMakeLists.txt +++ b/cmvr-es/devices/camera/mechmind/CMakeLists.txt @@ -10,6 +10,6 @@ target_include_directories(mechmind PUBLIC target_include_directories(mechmind PUBLIC ${MECH_EYE_SDK_PATH}/lib) file (GLOB LIBS ${MECH_EYE_SDK_PATH}/lib/*.so) -target_link_libraries(mechmind PUBLIC ${LIBS} glog::glog) +target_link_libraries(mechmind PUBLIC ${LIBS} glog::glog cmvr_es::proto) add_library(cmvr_es::device::mechmind ALIAS mechmind) diff --git a/cmvr-es/devices/camera/mechmind/include/mechmind_camera.h b/cmvr-es/devices/camera/mechmind/include/mechmind_camera.h index 3cb9f149..6a8c225b 100644 --- a/cmvr-es/devices/camera/mechmind/include/mechmind_camera.h +++ b/cmvr-es/devices/camera/mechmind/include/mechmind_camera.h @@ -23,16 +23,16 @@ namespace cmvr::device //工业级摄像机 class MechmindCamera: public AbstractCamera { public: - explicit MechmindCamera(const XmlNode& config); + explicit MechmindCamera(const config::MechMindCameraConfig& config); ~MechmindCamera() override = default; - void init() override; - void start() override; - void stop() override; + std::string typeName() const override { return "MechmindCamera"; } + bool init() override; + bool start() override; + bool stop() override; void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override; void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override; void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override; - void updateParams(const std::pair& param) override; void startRecording(const std::string &video_path) override; void stopRecording() override; @@ -50,4 +50,4 @@ namespace cmvr::device -#endif //MECHMIND_CAMERA_H \ No newline at end of file +#endif //MECHMIND_CAMERA_H diff --git a/cmvr-es/devices/camera/mechmind/src/mechmind_camera.cpp b/cmvr-es/devices/camera/mechmind/src/mechmind_camera.cpp index 56b2e63e..39e30539 100644 --- a/cmvr-es/devices/camera/mechmind/src/mechmind_camera.cpp +++ b/cmvr-es/devices/camera/mechmind/src/mechmind_camera.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by xtkuang on 2025/6/4. // @@ -8,42 +9,48 @@ using namespace std; using namespace cmvr::device; using namespace mmind::eye; -MechmindCamera::MechmindCamera(const XmlNode& config) : AbstractCamera(config) { - try { - ip_ = config.getAttrString("ip"); - if (ip_.empty()) { - state_.is_error = true; - state_.error_message = "ip attribute not set"; - LOG(ERROR) << "[MechmindCamera] (MechmindCamera): ip attribute not set"; - throw std::runtime_error("[MechmindCamera] (MechmindCamera): ip attribute not set"); - } - - align_ = cfg_.getAttrDefault("align", true); - - const string temp = cfg_.getAttrDefault("_2dtype", "color"); - if (temp == "color") { - image2d_type_ = COLOR; - } - else if (temp == "gray") { - image2d_type_ = GRAY; - } +MechmindCamera::MechmindCamera(const config::MechMindCameraConfig& config) { + id_ = config.id(); + ip_ = config.ip(); + if (ip_.empty()) { + state_.is_error = true; + state_.error_message = "ip attribute not set"; + CMVR_LOG(ERROR) << "[MechmindCamera] (MechmindCamera): " << state_.error_message; + return; } - catch (const exception& e) { - const string error_msg = "[MechmindCamera] (start): " + string(e.what()); - LOG(ERROR) << error_msg; - throw std::runtime_error(error_msg); + + align_ = config.has_align() ? config.align() : true; + + const string temp = config.image2d_type().empty() ? "color" : config.image2d_type(); + if (temp == "color") { + image2d_type_ = COLOR; + } + else if (temp == "gray") { + image2d_type_ = GRAY; + } + else { + state_.is_error = true; + state_.error_message = "unsupported image2d_type: " + temp; + CMVR_LOG(ERROR) << "[MechmindCamera] (MechmindCamera): " << state_.error_message; } } -void MechmindCamera::init() { +bool MechmindCamera::init() { std::lock_guard lock(dev_mtx_); clear_error_(); state_.is_initialized = false; + if (ip_.empty()) { + state_.is_error = true; + state_.error_message = "ip attribute not set"; + CMVR_LOG(ERROR) << "[MechmindCamera] (init): " << state_.error_message; + return false; + } state_.is_initialized = true; + return true; } -void MechmindCamera::start() { +bool MechmindCamera::start() { try { std::lock_guard lock(dev_mtx_); clear_error_(); @@ -53,28 +60,36 @@ void MechmindCamera::start() { } else { const string error_msg = "start camera failed, error_code=" + to_string(status.errorCode); - LOG(ERROR) << "[MechmindCamera] (start): "<< error_msg; - throw std::runtime_error(error_msg); + CMVR_LOG(ERROR) << "[MechmindCamera] (start): "<< error_msg; + state_.is_error = true; + state_.error_message = error_msg; + return false; } + return true; } catch (const std::exception& e) { const string error_msg = "[MechmindCamera] (start): " + string(e.what()); - LOG(ERROR) << error_msg; - throw std::runtime_error(error_msg); + CMVR_LOG(ERROR) << error_msg; + state_.is_error = true; + state_.error_message = error_msg; + return false; } } -void MechmindCamera::stop() { +bool MechmindCamera::stop() { try { std::lock_guard lock(dev_mtx_); clear_error_(); camera_.disconnect(); state_.is_opened = false; + return true; } catch (const exception &e) { const string error_msg = "[MechmindCamera] (stop): " + string(e.what()); - LOG(ERROR) << error_msg; - throw std::runtime_error(error_msg); + CMVR_LOG(ERROR) << error_msg; + state_.is_error = true; + state_.error_message = error_msg; + return false; } } @@ -83,21 +98,30 @@ void MechmindCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) { std::lock_guard lock(dev_mtx_); clear_error_(); if (!state_.is_initialized || !state_.is_opened) { - LOG(ERROR) << "[MechmindCamera] (getRGBImage): camera not opened"; - throw runtime_error("[MechmindCamera] (getRGBImage): camera not opened"); + state_.is_error = true; + state_.error_message = "camera not opened"; + CMVR_LOG(ERROR) << "[MechmindCamera] (getRGBImage): " << state_.error_message; + color.release(); + return; } if (image2d_type_ != COLOR) { - LOG(ERROR) << "[MechmindCamera] (getRGBImage): image 2d type not color"; - throw runtime_error("[MechmindCamera] (getRGBImage): image 2d type not color"); + state_.is_error = true; + state_.error_message = "image 2d type not color"; + CMVR_LOG(ERROR) << "[MechmindCamera] (getRGBImage): " << state_.error_message; + color.release(); + return; } Frame2D frame2D; auto ret = camera_.capture2D(frame2D); if (!ret.isOK()) { const string error_msg = "get color image failed, error_code=" + to_string(ret.errorCode); - LOG(ERROR) << "[MechmindCamera] (getRGBImage): "<< error_msg; - throw std::runtime_error(error_msg); + CMVR_LOG(ERROR) << "[MechmindCamera] (getRGBImage): "<< error_msg; + state_.is_error = true; + state_.error_message = error_msg; + color.release(); + return; } switch (frame2D.colorType()) { case ColorTypeOf2DCamera::Monochrome: @@ -118,8 +142,10 @@ void MechmindCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) { } catch (const exception &e) { const string error_msg = "[MechmindCamera] (getRGBImage): " + string(e.what()); - LOG(ERROR) << error_msg; - throw std::runtime_error(error_msg); + CMVR_LOG(ERROR) << error_msg; + state_.is_error = true; + state_.error_message = error_msg; + color.release(); } } @@ -128,24 +154,32 @@ void MechmindCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { std::lock_guard lock(dev_mtx_); clear_error_(); if (!state_.is_initialized || !state_.is_opened) { - LOG(ERROR) << "[MechmindCamera] (getDepthImage): camera not opened"; - throw runtime_error("[MechmindCamera] (getDepthImage): camera not opened"); + state_.is_error = true; + state_.error_message = "camera not opened"; + CMVR_LOG(ERROR) << "[MechmindCamera] (getDepthImage): " << state_.error_message; + depth.release(); + return; } Frame3D frame3D; auto ret = camera_.capture3D(frame3D); if (!ret.isOK()) { const string error_msg = "get depth image failed, error_code=" + to_string(ret.errorCode); - LOG(ERROR) << "[MechmindCamera] (getDepthImage): "<< error_msg; - throw std::runtime_error(error_msg); + CMVR_LOG(ERROR) << "[MechmindCamera] (getDepthImage): "<< error_msg; + state_.is_error = true; + state_.error_message = error_msg; + depth.release(); + return; } DepthMap depthMap = frame3D.getDepthMap(); depth = cv::Mat(depthMap.height(), depthMap.width(), CV_32FC1, depthMap.data()); } catch (const exception &e) { const string error_msg = "[MechmindCamera] (getDepthImage): " + string(e.what()); - LOG(ERROR) << error_msg; - throw std::runtime_error(error_msg); + CMVR_LOG(ERROR) << error_msg; + state_.is_error = true; + state_.error_message = error_msg; + depth.release(); } } @@ -154,8 +188,12 @@ void MechmindCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics std::lock_guard lock(dev_mtx_); clear_error_(); if (!state_.is_initialized || !state_.is_opened) { - LOG(ERROR) << "[MechmindCamera] (getDepthImage): camera not opened"; - throw runtime_error("[MechmindCamera] (getDepthImage): camera not opened"); + state_.is_error = true; + state_.error_message = "camera not opened"; + CMVR_LOG(ERROR) << "[MechmindCamera] (getRGBDImages): " << state_.error_message; + color.release(); + depth.release(); + return; } Frame2D frame_2d; @@ -163,16 +201,24 @@ void MechmindCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics auto ret = camera_.capture2D(frame_2d); if (!ret.isOK()) { const string error_msg = "get color failed, error_code=" + to_string(ret.errorCode); - LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg; - throw std::runtime_error(error_msg); + CMVR_LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg; + state_.is_error = true; + state_.error_message = error_msg; + color.release(); + depth.release(); + return; } Color2DImage color_image = frame_2d.getColorImage(); ret = camera_.capture3D(frame_3d); if (!ret.isOK()) { const string error_msg = "get depth failed, error_code=" + to_string(ret.errorCode); - LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg; - throw std::runtime_error(error_msg); + CMVR_LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg; + state_.is_error = true; + state_.error_message = error_msg; + color.release(); + depth.release(); + return; } DepthMap depth_map = frame_3d.getDepthMap(); @@ -189,41 +235,49 @@ void MechmindCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics ret = getPointCloudAfterMapping(depth_map, mask, intrinsics, pcl); if (!ret.isOK()) { const string error_msg = "get pcl after mapping failed, error_code=" + to_string(ret.errorCode); - LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg; - throw std::runtime_error(error_msg); + CMVR_LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg; + state_.is_error = true; + state_.error_message = error_msg; + color.release(); + depth.release(); + return; } TexturedPointCloud textured_pcl; ret = getPointCloudAfterMapping(depth_map, mask, color_image, intrinsics, textured_pcl); if (!ret.isOK()) { const string error_msg = "get pcl after mapping failed, error_code=" + to_string(ret.errorCode); - LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg; - throw std::runtime_error(error_msg); + CMVR_LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg; + state_.is_error = true; + state_.error_message = error_msg; + color.release(); + depth.release(); + return; } textured_plc_to_rgbd_(textured_pcl, color, depth); } catch (const exception &e) { const string error_msg = "[MechmindCamera] (getRGBDImages): " + string(e.what()); - LOG(ERROR) << error_msg; - throw std::runtime_error(error_msg); + CMVR_LOG(ERROR) << error_msg; + state_.is_error = true; + state_.error_message = error_msg; + color.release(); + depth.release(); } } -void MechmindCamera::updateParams(const std::pair& param) -{ - //mmind::eye::UserSet currentUserSet = camera_.currentUserSet(); - //std::vector parameters = currentUserSet.getAvailableParameters(); - throw runtime_error("[MechmindCamera] (startRecording): Not implemented"); -} - void MechmindCamera::startRecording(const std::string& video_path) { - throw runtime_error("[MechmindCamera] (startRecording): Not implemented"); + state_.is_error = true; + state_.error_message = "startRecording is not implemented"; + CMVR_LOG(ERROR) << "[MechmindCamera] (startRecording): " << state_.error_message; } void MechmindCamera::stopRecording() { - throw runtime_error("[MechmindCamera] (startRecording): Not implemented"); + state_.is_error = true; + state_.error_message = "stopRecording is not implemented"; + CMVR_LOG(ERROR) << "[MechmindCamera] (stopRecording): " << state_.error_message; } void MechmindCamera::textured_plc_to_rgbd_(const TexturedPointCloud& cloud, cv::Mat& color, cv::Mat& depth) { diff --git a/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h index fe0076d0..220e9936 100644 --- a/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h +++ b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h @@ -24,6 +24,7 @@ public: explicit MujocoCamera(FetchRgbdFn fetch_rgbd_fn); ~MujocoCamera() override = default; + std::string typeName() const override { return "MujocoCamera"; } void setFovyDeg(double fovy_deg); void setConsumeNewFrameOnly(bool enable); diff --git a/cmvr-es/devices/camera/realsense_camera/CMakeLists.txt b/cmvr-es/devices/camera/realsense_camera/CMakeLists.txt index e2fa3277..0fd4da5f 100644 --- a/cmvr-es/devices/camera/realsense_camera/CMakeLists.txt +++ b/cmvr-es/devices/camera/realsense_camera/CMakeLists.txt @@ -5,7 +5,7 @@ target_include_directories(realsense_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) add_library(cmvr_es::device::realsense_camera ALIAS realsense_camera) # ✅ 命名空间别名 OK # 链接 librealsense2 -target_link_libraries(realsense_camera PRIVATE realsense2 opencv_core opencv_imgproc opencv_videoio) +target_link_libraries(realsense_camera PRIVATE realsense2 opencv_core opencv_imgproc opencv_videoio cmvr_es::proto) target_link_libraries(realsense_camera PRIVATE realsense2 avcodec diff --git a/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h b/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h index 8ac6624d..50a8eb7b 100644 --- a/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h +++ b/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h @@ -15,17 +15,16 @@ namespace cmvr::device{ class RealsenseCamera final : public AbstractCamera { public: - explicit RealsenseCamera(const XmlNode& config); RealsenseCamera(const config::RealSenseCameraConfig& cam); ~RealsenseCamera() override; - void init() override; - void start() override; - void stop() override; + std::string typeName() const override { return "RealSenseCamera"; } + bool init() override; + bool start() override; + bool stop() override; void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override; void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override; void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override; - void updateParams(const std::pair& param) override; void startRecording(const std::string &video_path) override; void stopRecording() override; void pauseRecording() override; diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp index 1d88c87f..f5afa71d 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp +++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by linbo on 2025/6/18. // @@ -20,19 +21,19 @@ bool checkRealSenseCamera(const std::string& serialNumber = "") { // 检查设备数量 size_t device_count = devices.size(); if (device_count == 0) { - std::cout << "未检测到 RealSense 摄像头" << std::endl; + CMVR_LOG(INFO) << "No RealSense camera detected"; return false; } - std::cout << "检测到 " << device_count << " 个 RealSense 设备" << std::endl; + CMVR_LOG(INFO) << "Detected " << device_count << " RealSense device(s)"; // 如果未指定序列号，仅确认是否有设备连接 if (serialNumber.empty()) { for (size_t i = 0; i < device_count; i++) { rs2::device device = devices[i]; - std::cout << "设备 " << i + 1 << ": " + CMVR_LOG(INFO) << "Device " << i + 1 << ": " << device.get_info(RS2_CAMERA_INFO_NAME) - << " (序列号: " << device.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER) << ")" << std::endl; + << " (serial: " << device.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER) << ")"; } return true; } @@ -42,93 +43,38 @@ bool checkRealSenseCamera(const std::string& serialNumber = "") { rs2::device device = devices[i]; std::string currentSerial = device.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); - std::cout << "设备 " << i + 1 << ": " + CMVR_LOG(INFO) << "Device " << i + 1 << ": " << device.get_info(RS2_CAMERA_INFO_NAME) - << " (序列号: " << currentSerial << ")" << std::endl; + << " (serial: " << currentSerial << ")"; if (currentSerial == serialNumber) { - std::cout << "找到指定序列号的设备: " << serialNumber << std::endl; + CMVR_LOG(INFO) << "Found RealSense device with serial: " << serialNumber; return true; } } - std::cout << "未找到序列号为 " << serialNumber << " 的设备" << std::endl; + CMVR_LOG(INFO) << "RealSense device not found, serial=" << serialNumber; return false; } catch (const rs2::error& e) { - std::cerr << "RealSense 错误: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "RealSense error: " << e.what(); return false; } catch (const std::exception& e) { - std::cerr << "常规错误: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "General error: " << e.what(); return false; } } -RealsenseCamera::RealsenseCamera(const XmlNode& config) : AbstractCamera(config) { - try { - serial_ = cfg_.getAttrString("serial"); - if (serial_.empty()){ - LOG(ERROR) << "[RealsenseCamera] (RealsenseCamera): empty device serial number"; - throw runtime_error("[RealsenseCamera] (RealsenseCamera): empty device serial number"); - } - if (!checkRealSenseCamera(serial_)) { - LOG(ERROR) << "[RealsenseCamera] (RealsenseCamera): Device with serial number " << serial_ << " not found"; - throw std::runtime_error("[RealsenseCamera] (RealsenseCamera): Device with serial number " + serial_ + " not found"); - } - fps_ = cfg_.getAttrDefault("fps", 30); - width_ = cfg_.getAttrDefault("w", 640); - height_ = cfg_.getAttrDefault("h", 480); - encode_width_ = cfg_.getAttrDefault("encode_w", width_); - encode_height_ = cfg_.getAttrDefault("encode_h", height_); - buffer_size_ = cfg_.getAttrDefault("bs", 30); - - max_retry_ = cfg_.getAttrDefault("max_retry", 5); - is_sync_ = cfg_.getAttrDefault("sync", true); - - align_mode_ = cfg_.getAttrDefault("align_mode", "color"); - state_.fps = fps_; - state_.width = width_; - state_.height = height_; - string mode_str = cfg_.getAttrDefault("stream_mode", "rgbd"); - if (mode_str == "rgbd") { - stream_mode_ = RGBD_MODE; - } - else if (mode_str == "color") { - stream_mode_ = COLOR_MODE; - } - else if (mode_str == "depth") { - stream_mode_ = DEPTH_MODE; - } - else { - LOG(ERROR) << "[RealsenseCamera] (RealsenseCamera): invalid argument stream_mode=" << mode_str; - throw runtime_error("invalid argument mode=" + mode_str); - } - - mode_str = cfg_.getAttrDefault("mode", "photo"); - if (mode_str == "photo") { - mode_ = PHOTO_MODE; - } - else if (mode_str == "video"){ - mode_ = VIDEO_MODE; - } - else { - LOG(ERROR) << "[RealsenseCamera] (RealsenseCamera): invalid argument mode=" << mode_str; - throw runtime_error("invalid argument mode=" + mode_str); - } - codec_ = cfg_.getAttrDefault("codec", "H265"); - } - catch (exception &e) { - throw runtime_error(e.what()); - } -} - RealsenseCamera::RealsenseCamera(const config::RealSenseCameraConfig& camera):camera_(camera) { + id_ = camera_.id(); serial_ = camera_.serialnumber(); if (serial_.empty()){ - LOG(ERROR) << "[UVCCamera] (UVCCamera): empty device serial number"; - throw runtime_error("[UVCCamera] (UVCCamera): empty device serial number"); + CMVR_LOG(ERROR) << "[RealsenseCamera] (RealsenseCamera): empty device serial number"; + state_.is_error = true; + state_.error_message = "empty device serial number"; + return; } fps_ = camera_.fps(); width_ = camera_.width(); @@ -148,8 +94,10 @@ RealsenseCamera::RealsenseCamera(const config::RealSenseCameraConfig& camera):ca mode_ = VIDEO_MODE; } else { - LOG(ERROR) << "[UVCCamera] (UVCCamera): invalid argument mode"; - throw runtime_error("invalid argument mode"); + CMVR_LOG(ERROR) << "[RealsenseCamera] (RealsenseCamera): invalid argument mode"; + state_.is_error = true; + state_.error_message = "invalid argument mode"; + return; } auto stream_mode = camera_.stream_mode(); @@ -186,13 +134,13 @@ RealsenseCamera::~RealsenseCamera() { try { stop(); } catch (const std::exception& e) { - LOG(WARNING) << "[RealsenseCamera] (~RealsenseCamera): " << e.what(); + CMVR_LOG(WARNING) << "[RealsenseCamera] (~RealsenseCamera): " << e.what(); } catch (...) { - LOG(WARNING) << "[RealsenseCamera] (~RealsenseCamera): unknown exception"; + CMVR_LOG(WARNING) << "[RealsenseCamera] (~RealsenseCamera): unknown exception"; } } -void RealsenseCamera::init() { +bool RealsenseCamera::init() { try { std::lock_guard lock(ctrl_mtx_); clear_error_(); @@ -201,6 +149,13 @@ void RealsenseCamera::init() { stream_frame_buffer_ = std::make_shared>(buffer_size_); stream_frame_buffer_->clear(); + if (!checkRealSenseCamera(serial_)) { + CMVR_LOG(ERROR) << "[RealsenseCamera] (init): RealSense camera not found, serial=" << serial_; + state_.is_error = true; + state_.error_message = "RealSense camera not found: " + serial_; + return false; + } + rs_cfg_.enable_device(serial_); if (stream_mode_ == RGBD_MODE){ rs_cfg_.enable_stream(RS2_STREAM_COLOR, state_.width, state_.height, RS2_FORMAT_BGR8, state_.fps); @@ -217,36 +172,40 @@ void RealsenseCamera::init() { //初始化编码器 // 初始化RGB编码器（示例参数：640x480，30fps，H.264） if (!initSingleEncoder(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) { - LOG(ERROR) << "[RealsenseCamera] (start): Failed to init RGB encoder!"; - throw runtime_error("Failed to init RGB encoder!"); + CMVR_LOG(ERROR) << "[RealsenseCamera] (start): Failed to init RGB encoder!"; + state_.is_initialized = false; + state_.is_error = true; + state_.error_message = "Failed to init RGB encoder!"; + return false; } // 初始化深度图编码器（示例参数：640x480，15fps，H.265） - // if (!initSingleEncoder(depthEncoder_, codec_, width_, height_, fps_)) { - // LOG(ERROR) << "[RealsenseCamera] (start): Failed to init depth encoder!"; - // throw runtime_error("Failed to init depth encoder!"); - // } - LOG(INFO) << "[RealsenseCamera] (init): realsense camera initialized at index " << serial_; // 记录初始化成功日志 + CMVR_LOG(INFO) << "[RealsenseCamera] (init): realsense camera initialized at index " << serial_; + return true; } catch (const exception &error){ state_.is_initialized = false; - LOG(ERROR) << "realsense camera connect error:" << error.what(); - throw runtime_error(error.what()); + CMVR_LOG(ERROR) << "realsense camera connect error:" << error.what(); + state_.is_error = true; + state_.error_message = error.what(); + return false; } } -void RealsenseCamera::start() { +bool RealsenseCamera::start() { std::lock_guard lock(ctrl_mtx_); clear_error_(); if (!state_.is_initialized) { - LOG(ERROR) << "[RealsenseCamera] (init): Camera not initialized"; - throw runtime_error("Camera not initialized"); + CMVR_LOG(ERROR) << "[RealsenseCamera] (init): Camera not initialized"; + state_.is_error = true; + state_.error_message = "Camera not initialized"; + return false; } if (state_.is_opened) { - LOG(WARNING) << "[RealsenseCamera] (start): camera already started"; - return; + CMVR_LOG(WARNING) << "[RealsenseCamera] (start): camera already started"; + return true; } constexpr int kStartMaxRetry = 5; @@ -295,20 +254,30 @@ void RealsenseCamera::start() { // 启动后做一次首帧探测，避免“start 成功但长期无帧”假阳性。 rs2::frameset probe = pipe_.wait_for_frames(kProbeTimeoutMs); if (!probe.get_color_frame()) { - throw runtime_error("no color frame after start"); + last_error = "no color frame after start"; + CMVR_LOG(WARNING) << "[RealsenseCamera] (start): " << last_error; + pipe_.stop(); + align_.reset(); + std::this_thread::sleep_for(std::chrono::milliseconds(kRetrySleepMs)); + continue; } if (stream_mode_ == RGBD_MODE && !probe.get_depth_frame()) { - throw runtime_error("no depth frame after start"); + last_error = "no depth frame after start"; + CMVR_LOG(WARNING) << "[RealsenseCamera] (start): " << last_error; + pipe_.stop(); + align_.reset(); + std::this_thread::sleep_for(std::chrono::milliseconds(kRetrySleepMs)); + continue; } state_.is_opened = true; - LOG(INFO) << "realsense start streaming successfully"; - return; + CMVR_LOG(INFO) << "realsense start streaming successfully"; + return true; } catch (const exception &error) { last_error = error.what(); state_.is_opened = false; - LOG(WARNING) << "[RealsenseCamera] (start): attempt " + CMVR_LOG(WARNING) << "[RealsenseCamera] (start): attempt " << attempt << "/" << kStartMaxRetry << " failed: " << last_error; try { @@ -321,33 +290,36 @@ void RealsenseCamera::start() { } } - LOG(ERROR) << "realsense start streaming error: " << last_error; - throw runtime_error("realsense start streaming error: " + last_error); + CMVR_LOG(ERROR) << "realsense start streaming error: " << last_error; + state_.is_error = true; + state_.error_message = "realsense start streaming error: " + last_error; + return false; } -void RealsenseCamera::stop() { +bool RealsenseCamera::stop() { //先停止录制再关闭摄像头 if (state_.is_recording) { try { stopRecording(); } catch (const std::exception& e) { - LOG(WARNING) << "[RealsenseCamera] (stop): stopRecording failed: " << e.what(); + CMVR_LOG(WARNING) << "[RealsenseCamera] (stop): stopRecording failed: " << e.what(); } } std::lock_guard lock(ctrl_mtx_); clear_error_(); if (!state_.is_opened || !state_.is_initialized) { state_.is_opened = false; - return; + return true; } try { pipe_.stop(); } catch (const std::exception& e) { - LOG(WARNING) << "[RealsenseCamera] (stop): " << e.what(); + CMVR_LOG(WARNING) << "[RealsenseCamera] (stop): " << e.what(); } align_.reset(); pipe_ = rs2::pipeline(); state_.is_opened = false; + return true; } void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) { @@ -362,7 +334,11 @@ void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) { std::lock_guard lock(ctrl_mtx_); clear_error_(); if (!state_.is_opened) { - throw runtime_error("camera not opened"); + state_.is_error = true; + state_.error_message = "camera not opened"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (getRGBImage): " << state_.error_message; + color.release(); + return; } // 先尝试从队列取数据，没有的话再直接从摄像头读取 auto frame = stream_frame_buffer_->pop(getImageIndex_); @@ -391,10 +367,13 @@ void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) { latest_depth_ = depth; } } - // LOG(INFO) << "realsense get rgb frame successfully"; + // CMVR_LOG(INFO) << "realsense get rgb frame successfully"; } catch (const std::exception& e) { - throw std::runtime_error("[RealsenseCamera] (getRGBImage): Failed to get image: " + std::string(e.what())); + state_.is_error = true; + state_.error_message = "[RealsenseCamera] (getRGBImage): Failed to get image: " + std::string(e.what()); + CMVR_LOG(ERROR) << state_.error_message; + color.release(); } } @@ -416,12 +395,20 @@ void RealsenseCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { // 获取帧（带同步） frames = get_frameset(true); } catch (const std::exception& e) { - throw std::runtime_error("[RealsenseCamera] (getDepthImage): Failed to get frames: " + std::string(e.what())); + state_.is_error = true; + state_.error_message = "[RealsenseCamera] (getDepthImage): Failed to get frames: " + std::string(e.what()); + CMVR_LOG(ERROR) << state_.error_message; + depth.release(); + return; } rs2::depth_frame depth_frame = frames.get_depth_frame(); if (!depth_frame) { - throw std::runtime_error("[RealsenseCamera] (getDepthImage): No depth frame available"); + state_.is_error = true; + state_.error_message = "[RealsenseCamera] (getDepthImage): No depth frame available"; + CMVR_LOG(ERROR) << state_.error_message; + depth.release(); + return; } depth = cv::Mat(cv::Size(depth_frame.get_width(), depth_frame.get_height()), @@ -454,14 +441,24 @@ void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsic try { frames = get_frameset(true); } catch (const std::exception& e) { - throw std::runtime_error("[RealsenseCamera] (getRGBDImages): Failed to get frames: " + std::string(e.what())); + state_.is_error = true; + state_.error_message = "[RealsenseCamera] (getRGBDImages): Failed to get frames: " + std::string(e.what()); + CMVR_LOG(ERROR) << state_.error_message; + color.release(); + depth.release(); + return; } rs2::video_frame color_frame = frames.get_color_frame(); // rs2::frame color_frame = frames.get_color_frame(); rs2::depth_frame depth_frame = frames.get_depth_frame(); if (!color_frame || !depth_frame) { - throw std::runtime_error("[RealsenseCamera] (getRGBDImages): Missing frames"); + state_.is_error = true; + state_.error_message = "[RealsenseCamera] (getRGBDImages): Missing frames"; + CMVR_LOG(ERROR) << state_.error_message; + color.release(); + depth.release(); + return; } color = cv::Mat(cv::Size(color_frame.get_width(), color_frame.get_height()), @@ -478,43 +475,40 @@ void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsic } -void RealsenseCamera::updateParams(const std::pair& param) { - std::lock_guard lock(ctrl_mtx_); - try { - if (param.first == "serial") { - serial_ = param.second; - } - else if (param.first == "width") { - width_ = stoi(param.second); - } - else if (param.first == "height") { - height_ = stoi(param.second); - } - else if (param.first == "fps") { - fps_ = stoi(param.second); - } - stop(); - init(); - start(); - } - catch (exception &e) { - throw runtime_error(e.what()); - } -} - void RealsenseCamera::startRecording(const std::string &video_path) { std::lock_guard lock(ctrl_mtx_); clear_error_(); if (mode_ != VIDEO_MODE) { - throw runtime_error("[RealsenseCamera] (startRecording): 仅支持VIDEO_MODE"); + state_.is_error = true; + state_.error_message = "startRecording only supports VIDEO_MODE"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + return; } if (!state_.is_opened) { - throw runtime_error("[RealsenseCamera] (startRecording): 没有打开设备"); + state_.is_error = true; + state_.error_message = "camera not opened"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + return; } if (state_.is_recording) { - throw runtime_error("[RealsenseCamera] (startRecording): 已在录像中"); + state_.is_error = true; + state_.error_message = "already recording"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + return; } + auto cleanup_recording_resources = [this]() { + if (packet_) av_packet_free(&packet_); + if (stream_ && stream_->codecpar->extradata) av_free(stream_->codecpar->extradata); + if (format_context_) { + if (!(format_context_->oformat->flags & AVFMT_NOFILE) && format_context_->pb) avio_closep(&format_context_->pb); + avformat_free_context(format_context_); + } + stream_ = nullptr; + format_context_ = nullptr; + state_.is_recording = false; + }; + try { current_video_path_ = video_path; std::string temp_path = current_video_path_ + ".temp"; // 临时文件 @@ -526,19 +520,29 @@ void RealsenseCamera::startRecording(const std::string &video_path) { } else if (codec_ == "H265" || codec_ == "hevc") { codec_id = AV_CODEC_ID_HEVC; } else { - throw runtime_error("不支持的编码格式: " + codec_); + state_.is_error = true; + state_.error_message = "unsupported codec: " + codec_; + CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + return; } const char* format_name = (codec_ == "H265" || codec_ == "h265" || codec_ == "HEVC" || codec_ == "hevc") ? "mp4" : "avi"; auto ret = avformat_alloc_output_context2(&format_context_, nullptr, format_name, output_path_.c_str()); // 2. 创建输出格式上下文（封装器核心） if (ret < 0) { - throw runtime_error("创建输出格式上下文失败"); + state_.is_error = true; + state_.error_message = "failed to create output format context"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + return; } // 3. 添加视频流并配置参数 AVStream* stream = avformat_new_stream(format_context_, nullptr); if (!stream) { - throw runtime_error("创建视频流失败"); + state_.is_error = true; + state_.error_message = "failed to create video stream"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + cleanup_recording_resources(); + return; } stream_ = stream; AVCodecParameters* codecpar = stream->codecpar; @@ -552,7 +556,11 @@ void RealsenseCamera::startRecording(const std::string &video_path) { if (rgbEncoder_ && rgbEncoder_->codec_context && rgbEncoder_->codec_context->extradata) { codecpar->extradata = static_cast(av_mallocz(rgbEncoder_->codec_context->extradata_size + AV_INPUT_BUFFER_PADDING_SIZE)); if (!codecpar->extradata) { - throw runtime_error("分配extradata内存失败"); + state_.is_error = true; + state_.error_message = "failed to allocate extradata"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + cleanup_recording_resources(); + return; } memcpy(codecpar->extradata, rgbEncoder_->codec_context->extradata, rgbEncoder_->codec_context->extradata_size); codecpar->extradata_size = rgbEncoder_->codec_context->extradata_size; @@ -561,19 +569,31 @@ void RealsenseCamera::startRecording(const std::string &video_path) { // 5. 打开输出文件 if (!(format_context_->oformat->flags & AVFMT_NOFILE)) { if (avio_open(&format_context_->pb, temp_path.c_str(), AVIO_FLAG_WRITE) < 0) { - throw runtime_error("打开输出文件失败: " + temp_path); + state_.is_error = true; + state_.error_message = "failed to open output file: " + temp_path; + CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + cleanup_recording_resources(); + return; } } // 6. 写入文件头 if (avformat_write_header(format_context_, nullptr) < 0) { - throw runtime_error("写入文件头失败"); + state_.is_error = true; + state_.error_message = "failed to write file header"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + cleanup_recording_resources(); + return; } // 7. 初始化数据包（用于封装编码帧） packet_ = av_packet_alloc(); if (!packet_) { - throw runtime_error("分配AVPacket失败"); + state_.is_error = true; + state_.error_message = "failed to allocate AVPacket"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + cleanup_recording_resources(); + return; } //不在录像也不在流传输，但是采集线程没有退出时。 if (!state_.is_streaming && !state_.is_recording) { @@ -605,17 +625,10 @@ void RealsenseCamera::startRecording(const std::string &video_path) { recording_thread_ = make_shared(&RealsenseCamera::recording_worker_, this); } catch (const std::exception& e) { - // 异常清理 - if (packet_) av_packet_free(&packet_); - if (stream_ && stream_->codecpar->extradata) av_free(stream_->codecpar->extradata); - if (format_context_) { - if (!(format_context_->oformat->flags & AVFMT_NOFILE) && format_context_->pb) avio_closep(&format_context_->pb); - avformat_free_context(format_context_); - } - stream_ = nullptr; - format_context_ = nullptr; - state_.is_recording = false; - throw runtime_error("[RealsenseCamera] (startRecording): " + std::string(e.what())); + cleanup_recording_resources(); + state_.is_error = true; + state_.error_message = "[RealsenseCamera] (startRecording): " + std::string(e.what()); + CMVR_LOG(ERROR) << state_.error_message; } } @@ -623,10 +636,14 @@ void RealsenseCamera::stopRecording() { std::lock_guard lock(ctrl_mtx_); clear_error_(); if (mode_ != VIDEO_MODE) { - throw runtime_error("[RealsenseCamera] (stopRecording): 仅支持VIDEO_MODE"); + state_.is_error = true; + state_.error_message = "stopRecording only supports VIDEO_MODE"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (stopRecording): " << state_.error_message; + return; } if (!state_.is_recording) { - throw runtime_error("[RealsenseCamera] (stopRecording): 未在录像中"); + CMVR_LOG(WARNING) << "[RealsenseCamera] (stopRecording): not recording"; + return; } // 1. 停止录像线程 @@ -658,7 +675,10 @@ void RealsenseCamera::stopRecording() { // 3. 重命名临时文件为目标文件 std::string temp_path = current_video_path_ + ".temp"; if (rename(temp_path.c_str(), current_video_path_.c_str()) != 0) { - throw runtime_error("重命名临时文件失败: " + temp_path + " -> " + current_video_path_); + state_.is_error = true; + state_.error_message = "failed to rename temp file: " + temp_path + " -> " + current_video_path_; + CMVR_LOG(ERROR) << "[RealsenseCamera] (stopRecording): " << state_.error_message; + return; } current_video_path_.clear(); } @@ -701,10 +721,16 @@ void RealsenseCamera::streaming_worker_() { rs2::frame color_frame = frames.get_color_frame(); rs2::frame depth_frame = frames.get_depth_frame(); if (!color_frame) { - throw std::runtime_error("[RealsenseCamera]streaming_worker_: missing color frame"); + state_.is_error = true; + state_.error_message = "missing color frame"; + CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message; + break; } if (stream_mode_ == RGBD_MODE && !depth_frame) { - throw std::runtime_error("[RealsenseCamera]streaming_worker_: missing depth frame in RGBD mode"); + state_.is_error = true; + state_.error_message = "missing depth frame in RGBD mode"; + CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message; + break; } // 以下为编码部分，用于流模式 @@ -782,7 +808,7 @@ void RealsenseCamera::streaming_worker_() { is_streaming_running = false; state_.is_error = true; state_.error_message = e.what(); - LOG(ERROR) << "[RealsenseCamera]streaming_worker_ error:" << state_.error_message; + CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_ error:" << state_.error_message; } } @@ -847,7 +873,7 @@ void RealsenseCamera::recording_worker_() { // 写入帧 if (av_interleaved_write_frame(format_context_, packet_) < 0) { - std::cerr << "写入第" << frame_count_ << "帧失败" << std::endl; + CMVR_LOG(ERROR) << "Failed to write frame " << frame_count_; } std::this_thread::sleep_for(std::chrono::milliseconds(33)); @@ -857,7 +883,7 @@ void RealsenseCamera::recording_worker_() { av_write_trailer(format_context_); } catch (const std::exception& e) { - std::cerr << "录像线程错误: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "Recording thread error: " << e.what(); } is_recording_running = false; @@ -885,14 +911,14 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr& enco if (!codec) codec = avcodec_find_encoder(AV_CODEC_ID_HEVC); } if (!codec) { - std::cerr << "Failed to find " << codec_name << " encoder!" << std::endl; + CMVR_LOG(ERROR) << "Failed to find " << codec_name << " encoder!"; return false; } // 3. 初始化编码器上下文（不变） encoder->codec_context = avcodec_alloc_context3(codec); if (!encoder->codec_context) { - std::cerr << "Failed to allocate codec context!" << std::endl; + CMVR_LOG(ERROR) << "Failed to allocate codec context!"; return false; } @@ -938,11 +964,11 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr& enco // 6. 设置像素格式（不变） const enum AVPixelFormat* pix_fmts = codec->pix_fmts; if (!pix_fmts) { - std::cout << "Using default pixel format: YUV420P" << std::endl; + CMVR_LOG(INFO) << "Using default pixel format: YUV420P"; ctx->pix_fmt = AV_PIX_FMT_YUV420P; } else { ctx->pix_fmt = pix_fmts[0]; - std::cout << "Selected pixel format: " << av_get_pix_fmt_name(ctx->pix_fmt) << std::endl; + CMVR_LOG(INFO) << "Selected pixel format: " << av_get_pix_fmt_name(ctx->pix_fmt); } // 7. 打开编码器（不变） @@ -950,21 +976,21 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr& enco if (ret < 0) { char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0}; av_strerror(ret, errbuf, sizeof(errbuf)); - std::cerr << "Failed to open " << codec_name << " encoder: " << errbuf << std::endl; + CMVR_LOG(ERROR) << "Failed to open " << codec_name << " encoder: " << errbuf; return false; } // 8. 分配AVFrame（不变） encoder->frame = av_frame_alloc(); if (!encoder->frame) { - std::cerr << "Failed to allocate AVFrame!" << std::endl; + CMVR_LOG(ERROR) << "Failed to allocate AVFrame!"; return false; } encoder->frame->format = ctx->pix_fmt; encoder->frame->width = ctx->width; encoder->frame->height = ctx->height; if (av_frame_get_buffer(encoder->frame, 0) < 0) { - std::cerr << "Failed to allocate AVFrame buffer!" << std::endl; + CMVR_LOG(ERROR) << "Failed to allocate AVFrame buffer!"; av_frame_free(&encoder->frame); return false; } @@ -972,23 +998,23 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr& enco // 9. 分配AVPacket（不变） encoder->packet = av_packet_alloc(); if (!encoder->packet) { - std::cerr << "Failed to allocate AVPacket!" << std::endl; + CMVR_LOG(ERROR) << "Failed to allocate AVPacket!"; return false; } // 添加调试信息，确认设置生效 - std::cout << "Encoder settings:" << std::endl; - std::cout << " GOP size: " << ctx->gop_size << std::endl; + CMVR_LOG(INFO) << "Encoder settings:"; + CMVR_LOG(INFO) << " GOP size: " << ctx->gop_size; if (codec->id == AV_CODEC_ID_HEVC) { char* params = nullptr; if (av_opt_get(ctx->priv_data, "x265-params", 0, (uint8_t**)¶ms) >= 0) { - std::cout << " x265-params: " << params << std::endl; + CMVR_LOG(INFO) << " x265-params: " << params; av_free(params); } } - std::cout << "Successfully initialized " << codec_name << " encoder ( " - << width << "x" << height << "@" << fps << "fps )" << std::endl; + CMVR_LOG(INFO) << "Successfully initialized " << codec_name << " encoder ( " + << width << "x" << height << "@" << fps << "fps )"; return true; } @@ -1047,7 +1073,7 @@ bool RealsenseCamera::encodeFrameWithEncoder(std::shared_ptr& // 确保输入帧尺寸匹配（不变） if (frame.cols != encoder->width || frame.rows != encoder->height) { - std::cerr << "Frame size does not match encoder dimensions" << std::endl; + CMVR_LOG(ERROR) << "Frame size does not match encoder dimensions"; return false; } @@ -1068,7 +1094,7 @@ bool RealsenseCamera::encodeFrameWithEncoder(std::shared_ptr& } else if (dateImage.channels() == 1) { src_pix_fmt = AV_PIX_FMT_GRAY8; // 单通道灰度图 } else { - std::cerr << "Unsupported number of channels: " << dateImage.channels() << std::endl; + CMVR_LOG(ERROR) << "Unsupported number of channels: " << dateImage.channels(); return false; } @@ -1082,7 +1108,7 @@ bool RealsenseCamera::encodeFrameWithEncoder(std::shared_ptr& SWS_BILINEAR, nullptr, nullptr, nullptr ); if (!encoder->sws_context) { - std::cerr << "Failed to create SwsContext" << std::endl; + CMVR_LOG(ERROR) << "Failed to create SwsContext"; return false; } @@ -1093,7 +1119,7 @@ bool RealsenseCamera::encodeFrameWithEncoder(std::shared_ptr& int ret = sws_scale(encoder->sws_context, src_data, src_linesize, 0, dateImage.rows, encoder->frame->data, encoder->frame->linesize); if (ret < 0) { - std::cerr << "Error scaling frame" << std::endl; + CMVR_LOG(ERROR) << "Error scaling frame"; return false; } // 发送帧到编码器（不变） @@ -1101,7 +1127,7 @@ bool RealsenseCamera::encodeFrameWithEncoder(std::shared_ptr& if (ret < 0) { char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0}; av_strerror(ret, errbuf, sizeof(errbuf)); - std::cerr << "Error sending frame to encoder: " << errbuf << std::endl; + CMVR_LOG(ERROR) << "Error sending frame to encoder: " << errbuf; return false; } while (true) @@ -1113,17 +1139,17 @@ bool RealsenseCamera::encodeFrameWithEncoder(std::shared_ptr& } else if (ret < 0) { char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0}; av_strerror(ret, errbuf, sizeof(errbuf)); - std::cerr << "Error receiving packet from encoder: " << errbuf << std::endl; + CMVR_LOG(ERROR) << "Error receiving packet from encoder: " << errbuf; break; } // 调试：打印帧类型（I帧/P帧） if (encoder->packet->flags & AV_PKT_FLAG_KEY) { is_key = true; - //std::cout << "Encoded I frame (size: " << encoder->packet->size << " bytes)" << std::endl; + //CMVR_LOG(INFO) << "Encoded I frame (size: " << encoder->packet->size << " bytes)"; } else { is_key = false; - //std::cout << "Encoded P frame (size: " << encoder->packet->size << " bytes)" << std::endl; + //CMVR_LOG(INFO) << "Encoded P frame (size: " << encoder->packet->size << " bytes)"; } // 预留足够空间，避免多次内存分配 @@ -1145,11 +1171,11 @@ bool RealsenseCamera::encodeFrameWithEncoder(std::shared_ptr& has_start_code = true; } if (!has_start_code) { - std::cerr << "Invalid frame: no NALU start code!" << std::endl; + CMVR_LOG(ERROR) << "Invalid frame: no NALU start code!"; return false; } } else { - //std::cerr << "Encoded frame is empty!" << std::endl; + //CMVR_LOG(ERROR) << "Encoded frame is empty!"; return false; } return true; @@ -1226,14 +1252,14 @@ Eigen::Vector3f RealsenseCamera::get3DPointFromPixel(int u, int v) { std::lock_guard lock(frame_mtx_); if (latest_depth_.empty()) { - LOG(WARNING) << " depth data empty"; + CMVR_LOG(WARNING) << " depth data empty"; return {0,0,0}; } // 获取深度值，pixel float depth_m = latest_depth_.at(v, u) * 0.001f; if (depth_m <= 0) { - LOG(WARNING) << " depth data is < zero"; + CMVR_LOG(WARNING) << " depth data is < zero"; return {0,0,0}; } diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp index 803ee75a..d7e939c6 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp +++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by lgv on 2025/9/1. // @@ -5,25 +6,41 @@ #include "../../abstract_camera.h" #include "../include/realsense_camera.h" #include -#include "../../../../device_manager/include/device_manager.h" +#include "../../../../manager/device_manager/include/device_manager.h" +#include "common/config/config_files.h" #include #include #include using namespace cmvr::device; + +namespace { + +DeviceManager& getTestDeviceManager() { + static bool initialized = [] { + cmvr::ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt"); + cmvr::config::DeviceManagerRootConfig root_cfg; + if (!cmvr::ConfigHelper::loadConfigFile("manager/device_manager.pb.txt", root_cfg)) { + throw std::runtime_error("Failed to load test DeviceManagerConfig"); + } + DeviceManager::getInstance(root_cfg.device_manager()); + return true; + }(); + (void)initialized; + return DeviceManager::getInstance(); +} + +} // namespace + // 测试: 实时显示 RGB + Depth TEST(RealsenseCameraRealDeviceTest, SaveFrames) { - std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"; - const XmlNode config(config_path); + auto &dmgr = getTestDeviceManager(); - if (!config.hasChild("DeviceManager")){ - LOG(ERROR) << "Device Manager node not found"; + auto cam = dmgr.getDevice("right_hand_cam"); + if (!cam) { + CMVR_LOG(ERROR) << "[RealsenseCameraRealDeviceTest] Camera device not found: right_hand_cam"; return; } - auto dmgr_cfg = config.getChild("DeviceManager"); - auto &dmgr = DeviceManager::getInstance(dmgr_cfg); - - auto cam = dmgr.getDevice("cam4"); cam->start(); cv::Mat color, depth; @@ -36,7 +53,7 @@ TEST(RealsenseCameraRealDeviceTest, SaveFrames) { auto pose = cam->get3DPointFromPixel(700,377); - LOG(INFO) << "Pose : " << pose ; + CMVR_LOG(INFO) << "Pose : " << pose ; cam->stop(); } @@ -62,7 +79,6 @@ cmvr::config::RealSenseCameraConfig makeRsConfig() { cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR); cfg.set_buffer_size(30); cfg.set_sync(true); - cfg.set_enable(true); return cfg; } diff --git a/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt b/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt index 2079ddb6..77596fdf 100644 --- a/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt +++ b/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt @@ -2,7 +2,7 @@ add_library(uvc_camera SHARED src/uvc_camera.cpp) target_include_directories(uvc_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(uvc_camera PUBLIC glog opencv_core opencv_imgproc) +target_link_libraries(uvc_camera PUBLIC glog opencv_core opencv_imgproc cmvr_es::proto) add_library(cmvr_es::device::uvc_camera ALIAS uvc_camera) -install(TARGETS uvc_camera LIBRARY DESTINATION lib) \ No newline at end of file +install(TARGETS uvc_camera LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h b/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h index 41a7e4f5..9c035efa 100644 --- a/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h +++ b/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h @@ -5,7 +5,7 @@ #ifndef CMVR_ES_UVC_CAMERA_H #define CMVR_ES_UVC_CAMERA_H -#include "utils/base/include/ring_buffer.h" +#include "common/base/ring_buffer.h" #include "camera/abstract_camera.h" //使用ffmpeg来编码保存视频文件 @@ -69,17 +69,16 @@ namespace cmvr::device { //USB摄像机 class UVCCamera final : public AbstractCamera { public: - explicit UVCCamera(const XmlNode& config); UVCCamera(const cmvr::config::UVCCameraConfig& camera); ~UVCCamera() override; - void init() override; - void start() override; - void stop() override; + std::string typeName() const override { return "UVCCamera"; } + bool init() override; + bool start() override; + bool stop() override; void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override; void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override; void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override; - void updateParams(const std::pair& param) override; void startRecording(const std::string &video_path) override; void stopRecording() override; void pauseRecording() override; diff --git a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp index 30663784..dd6e80f3 100644 --- a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp +++ b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by xtkuang on 2025/5/30. // @@ -9,49 +10,15 @@ using namespace cmvr::device; #define USE_LIST_IMAGE 1 -UVCCamera::UVCCamera(const XmlNode& config) : AbstractCamera(config) { - try { - serial_ = cfg_.getAttrString("serial"); - if (serial_.empty()){ - LOG(ERROR) << "[UVCCamera] (UVCCamera): empty device serial number"; - throw runtime_error("[UVCCamera] (UVCCamera): empty device serial number"); - } - fps_ = cfg_.getAttrDefault("fps", 30); - width_ = cfg_.getAttrDefault("w", 640); - height_ = cfg_.getAttrDefault("h", 480); - encode_width_ = cfg_.getAttrDefault("encode_w", width_); - encode_height_ = cfg_.getAttrDefault("encode_h", height_); - buffer_size_ = cfg_.getAttrDefault("bs", 30); - - state_.fps = fps_; - state_.width = width_; - state_.height = height_; - string mode_str = cfg_.getAttrDefault("mode", "photo"); - if (mode_str == "photo") { - mode_ = PHOTO_MODE; - } - else if (mode_str == "video"){ - mode_ = VIDEO_MODE; - } - else { - LOG(ERROR) << "[UVCCamera] (UVCCamera): invalid argument mode=" << mode_str; - throw runtime_error("invalid argument mode=" + mode_str); - } - - codec_ = cfg_.getAttrDefault("codec", "H265"); - - //initFFmpeg(); - } - catch (exception &e) { - throw runtime_error(e.what()); - } -} UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera) { + id_ = camera_.id(); serial_ = camera_.usb(); if (serial_.empty()){ - LOG(ERROR) << "[UVCCamera] (UVCCamera): empty device serial number"; - throw runtime_error("[UVCCamera] (UVCCamera): empty device serial number"); + CMVR_LOG(ERROR) << "[UVCCamera] (UVCCamera): empty device serial number"; + state_.is_error = true; + state_.error_message = "empty device serial number"; + return; } fps_ = camera_.fps(); width_ = camera_.width(); @@ -71,8 +38,10 @@ UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera) mode_ = VIDEO_MODE; } else { - LOG(ERROR) << "[UVCCamera] (UVCCamera): invalid argument mode"; - throw runtime_error("invalid argument mode"); + CMVR_LOG(ERROR) << "[UVCCamera] (UVCCamera): invalid argument mode"; + state_.is_error = true; + state_.error_message = "invalid argument mode"; + return; } codec_ = camera_.codec(); @@ -85,7 +54,7 @@ UVCCamera::~UVCCamera() { recording_thread_->join(); } -void UVCCamera::init() { +bool UVCCamera::init() { std::lock_guard lock(ctrl_mtx_); clear_error_(); state_.is_initialized = false; @@ -102,8 +71,8 @@ void UVCCamera::init() { if (!cap_.isOpened()) { state_.is_error = true; state_.error_message = "Failed to open USB camera at index " + serial_; - LOG(ERROR) << "[UVCCamera] (init)" << state_.error_message; - throw runtime_error(state_.error_message); + CMVR_LOG(ERROR) << "[UVCCamera] (init)" << state_.error_message; + return false; } // 设置格式为MJPG @@ -112,45 +81,50 @@ void UVCCamera::init() { if (!cap_.set(cv::CAP_PROP_FRAME_WIDTH, width_)) { state_.is_error = true; state_.error_message = "set width failed"; - LOG(ERROR) << "[UVCCamera] (init): set width failed"; - throw runtime_error("[UVCCamera] (init): set width failed"); + CMVR_LOG(ERROR) << "[UVCCamera] (init): set width failed"; + return false; } state_.width = width_; if (!cap_.set(cv::CAP_PROP_FRAME_HEIGHT, height_)) { state_.is_error = true; state_.error_message = "set height failed"; - LOG(ERROR) << "[UVCCamera] (init): set height failed"; - throw runtime_error("[UVCCamera] (init): set height failed"); + CMVR_LOG(ERROR) << "[UVCCamera] (init): set height failed"; + return false; } state_.height = height_; if (!cap_.set(cv::CAP_PROP_FPS, fps_)) { state_.is_error = true; state_.error_message = "set fps failed"; - LOG(ERROR) << "[UVCCamera] (init): set fps failed"; - throw runtime_error("[UVCCamera] (init): set fps failed"); + CMVR_LOG(ERROR) << "[UVCCamera] (init): set fps failed"; + return false; } //初始化编码器 // 初始化RGB编码器（示例参数：640x480，30fps，H.264） if (!initSingleEncoder(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) { - LOG(ERROR) << "[UVCCamera] (start): Failed to init RGB encoder!"; - throw runtime_error("Failed to init RGB encoder!"); + CMVR_LOG(ERROR) << "[UVCCamera] (start): Failed to init RGB encoder!"; + state_.is_error = true; + state_.error_message = "Failed to init RGB encoder!"; + return false; } state_.fps = fps_; state_.is_initialized = true; state_.is_error = false; - LOG(INFO) << "[UVCCamera] (init): UVC camera initialized at index " << serial_; // 记录初始化成功日志 + CMVR_LOG(INFO) << "[UVCCamera] (init): UVC camera initialized at index " << serial_; // 记录初始化成功日志 + return true; } -void UVCCamera::start() { +bool UVCCamera::start() { std::lock_guard lock(ctrl_mtx_); clear_error_(); if (!state_.is_initialized) { - LOG(ERROR) << "[UVCCamera] (init): Camera not initialized"; - throw runtime_error("Camera not initialized"); + CMVR_LOG(ERROR) << "[UVCCamera] (init): Camera not initialized"; + state_.is_error = true; + state_.error_message = "Camera not initialized"; + return false; } if (state_.is_opened) { - LOG(WARNING) << "[RealsenseCamera] (start): camera already started"; - return; + CMVR_LOG(WARNING) << "[RealsenseCamera] (start): camera already started"; + return true; } cap_.open(serial_, cv::CAP_V4L2); this_thread::sleep_for(chrono::milliseconds(100)); @@ -158,8 +132,8 @@ void UVCCamera::start() { if (!cap_.isOpened()) { state_.is_error = true; state_.error_message = "Failed to open USB camera at index " + serial_; - LOG(ERROR) << "[UVCCamera] (init)" << state_.error_message; - throw runtime_error(state_.error_message); + CMVR_LOG(ERROR) << "[UVCCamera] (init)" << state_.error_message; + return false; } // 设置格式为MJPG @@ -168,27 +142,28 @@ void UVCCamera::start() { if (!cap_.set(cv::CAP_PROP_FRAME_WIDTH, width_)) { state_.is_error = true; state_.error_message = "set width failed"; - LOG(ERROR) << "[UVCCamera] (init): set width failed"; - throw runtime_error("[UVCCamera] (init): set width failed"); + CMVR_LOG(ERROR) << "[UVCCamera] (init): set width failed"; + return false; } state_.width = width_; if (!cap_.set(cv::CAP_PROP_FRAME_HEIGHT, height_)) { state_.is_error = true; state_.error_message = "set height failed"; - LOG(ERROR) << "[UVCCamera] (init): set height failed"; - throw runtime_error("[UVCCamera] (init): set height failed"); + CMVR_LOG(ERROR) << "[UVCCamera] (init): set height failed"; + return false; } state_.height = height_; if (!cap_.set(cv::CAP_PROP_FPS, fps_)) { state_.is_error = true; state_.error_message = "set fps failed"; - LOG(ERROR) << "[UVCCamera] (init): set fps failed"; - throw runtime_error("[UVCCamera] (init): set fps failed"); + CMVR_LOG(ERROR) << "[UVCCamera] (init): set fps failed"; + return false; } state_.is_opened = true; + return true; } -void UVCCamera::stop() { +bool UVCCamera::stop() { //先停止录制再关闭摄像头 if (state_.is_recording) { stopRecording(); @@ -197,7 +172,8 @@ void UVCCamera::stop() { clear_error_(); try { if (!state_.is_opened || !state_.is_initialized) { - throw runtime_error("Camera already closed"); + CMVR_LOG(WARNING) << "[UVCCamera] (stop): Camera already closed"; + return true; } if (mode_ == VIDEO_MODE){ state_.is_streaming = false; @@ -212,10 +188,13 @@ void UVCCamera::stop() { cap_.release(); } state_.is_opened = false; + return true; } catch (exception &e) { - LOG(ERROR) << "[UVCCamera] (stop): " << e.what(); - throw runtime_error(e.what()); + CMVR_LOG(ERROR) << "[UVCCamera] (stop): " << e.what(); + state_.is_error = true; + state_.error_message = e.what(); + return false; } } @@ -225,65 +204,81 @@ void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) clear_error_(); if (mode_ == PHOTO_MODE) { if (!state_.is_opened) { - throw runtime_error("camera not opened"); + state_.is_error = true; + state_.error_message = "camera not opened"; + CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message; + color.release(); + return; } if (!cap_.read(color)) { - throw runtime_error("read color image failed"); + state_.is_error = true; + state_.error_message = "read color image failed"; + CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message; + color.release(); + return; } } else if (mode_ == VIDEO_MODE) { if (!state_.is_opened) { - throw runtime_error("camera not opened"); + state_.is_error = true; + state_.error_message = "camera not opened"; + CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message; + color.release(); + return; } } } void UVCCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { - throw runtime_error("[UVCCamera] (getDepthImage): unsupported usage"); + state_.is_error = true; + state_.error_message = "getDepthImage unsupported usage"; + CMVR_LOG(ERROR) << "[UVCCamera] (getDepthImage): " << state_.error_message; + depth.release(); } void UVCCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) { - throw runtime_error("[UVCCamera] (getDepthImage): unsupported usage"); -} - -void UVCCamera::updateParams(const std::pair& param) { - std::lock_guard lock(ctrl_mtx_); - try { - if (param.first == "serial") { - serial_ = param.second; - } - else if (param.first == "width") { - width_ = stoi(param.second); - } - else if (param.first == "height") { - height_ = stoi(param.second); - } - else if (param.first == "fps") { - fps_ = stoi(param.second); - } - stop(); - init(); - start(); - } - catch (exception &e) { - throw runtime_error(e.what()); - } + state_.is_error = true; + state_.error_message = "getRGBDImages unsupported usage"; + CMVR_LOG(ERROR) << "[UVCCamera] (getRGBDImages): " << state_.error_message; + color.release(); + depth.release(); } void UVCCamera::startRecording(const std::string &video_path) { std::lock_guard lock(ctrl_mtx_); clear_error_(); if (mode_ != VIDEO_MODE) { - throw runtime_error("[UVCCamera] (startRecording): 仅支持VIDEO_MODE"); + state_.is_error = true; + state_.error_message = "startRecording only supports VIDEO_MODE"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + return; } if (!state_.is_opened) { - throw runtime_error("[UVCCamera] (startRecording): 没有打开设备"); + state_.is_error = true; + state_.error_message = "camera not opened"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + return; } if (state_.is_recording) { - throw runtime_error("[UVCCamera] (startRecording): 已在录像中"); + state_.is_error = true; + state_.error_message = "already recording"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + return; } + auto cleanup_recording_resources = [this]() { + if (packet_) av_packet_free(&packet_); + if (stream_ && stream_->codecpar->extradata) av_free(stream_->codecpar->extradata); + if (format_context_) { + if (!(format_context_->oformat->flags & AVFMT_NOFILE) && format_context_->pb) avio_closep(&format_context_->pb); + avformat_free_context(format_context_); + } + stream_ = nullptr; + format_context_ = nullptr; + state_.is_recording = false; + }; + try { current_video_path_ = video_path; std::string temp_path = current_video_path_ + ".temp"; // 临时文件 @@ -295,19 +290,29 @@ void UVCCamera::startRecording(const std::string &video_path) { } else if (codec_ == "H265" || codec_ == "hevc") { codec_id = AV_CODEC_ID_HEVC; } else { - throw runtime_error("不支持的编码格式: " + codec_); + state_.is_error = true; + state_.error_message = "unsupported codec: " + codec_; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + return; } const char* format_name = (codec_ == "H265" || codec_ == "h265" || codec_ == "HEVC" || codec_ == "hevc") ? "mp4" : "avi"; auto ret = avformat_alloc_output_context2(&format_context_, nullptr, format_name, output_path_.c_str()); // 2. 创建输出格式上下文（封装器核心） if (ret < 0) { - throw runtime_error("创建输出格式上下文失败"); + state_.is_error = true; + state_.error_message = "failed to create output format context"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + return; } // 3. 添加视频流并配置参数 AVStream* stream = avformat_new_stream(format_context_, nullptr); if (!stream) { - throw runtime_error("创建视频流失败"); + state_.is_error = true; + state_.error_message = "failed to create video stream"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + cleanup_recording_resources(); + return; } stream_ = stream; AVCodecParameters* codecpar = stream->codecpar; @@ -321,7 +326,11 @@ void UVCCamera::startRecording(const std::string &video_path) { if (rgbEncoder_ && rgbEncoder_->codec_context && rgbEncoder_->codec_context->extradata) { codecpar->extradata = static_cast(av_mallocz(rgbEncoder_->codec_context->extradata_size + AV_INPUT_BUFFER_PADDING_SIZE)); if (!codecpar->extradata) { - throw runtime_error("分配extradata内存失败"); + state_.is_error = true; + state_.error_message = "failed to allocate extradata"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + cleanup_recording_resources(); + return; } memcpy(codecpar->extradata, rgbEncoder_->codec_context->extradata, rgbEncoder_->codec_context->extradata_size); codecpar->extradata_size = rgbEncoder_->codec_context->extradata_size; @@ -330,19 +339,31 @@ void UVCCamera::startRecording(const std::string &video_path) { // 5. 打开输出文件 if (!(format_context_->oformat->flags & AVFMT_NOFILE)) { if (avio_open(&format_context_->pb, temp_path.c_str(), AVIO_FLAG_WRITE) < 0) { - throw runtime_error("打开输出文件失败: " + temp_path); + state_.is_error = true; + state_.error_message = "failed to open output file: " + temp_path; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + cleanup_recording_resources(); + return; } } // 6. 写入文件头 if (avformat_write_header(format_context_, nullptr) < 0) { - throw runtime_error("写入文件头失败"); + state_.is_error = true; + state_.error_message = "failed to write file header"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + cleanup_recording_resources(); + return; } // 7. 初始化数据包（用于封装编码帧） packet_ = av_packet_alloc(); if (!packet_) { - throw runtime_error("分配AVPacket失败"); + state_.is_error = true; + state_.error_message = "failed to allocate AVPacket"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + cleanup_recording_resources(); + return; } //不在录像也不在流传输，但是采集线程没有退出时。 if (!state_.is_streaming && !state_.is_recording) { @@ -374,17 +395,10 @@ void UVCCamera::startRecording(const std::string &video_path) { recording_thread_ = make_shared(&UVCCamera::recording_worker_, this); } catch (const std::exception& e) { - // 异常清理 - if (packet_) av_packet_free(&packet_); - if (stream_ && stream_->codecpar->extradata) av_free(stream_->codecpar->extradata); - if (format_context_) { - if (!(format_context_->oformat->flags & AVFMT_NOFILE) && format_context_->pb) avio_closep(&format_context_->pb); - avformat_free_context(format_context_); - } - stream_ = nullptr; - format_context_ = nullptr; - state_.is_recording = false; - throw runtime_error("[UVCCamera] (startRecording): " + std::string(e.what())); + cleanup_recording_resources(); + state_.is_error = true; + state_.error_message = "[UVCCamera] (startRecording): " + std::string(e.what()); + CMVR_LOG(ERROR) << state_.error_message; } } @@ -392,10 +406,14 @@ void UVCCamera::stopRecording() { std::lock_guard lock(ctrl_mtx_); clear_error_(); if (mode_ != VIDEO_MODE) { - throw runtime_error("[UVCCamera] (stopRecording): 仅支持VIDEO_MODE"); + state_.is_error = true; + state_.error_message = "stopRecording only supports VIDEO_MODE"; + CMVR_LOG(ERROR) << "[UVCCamera] (stopRecording): " << state_.error_message; + return; } if (!state_.is_recording) { - throw runtime_error("[UVCCamera] (stopRecording): 未在录像中"); + CMVR_LOG(WARNING) << "[UVCCamera] (stopRecording): not recording"; + return; } // 1. 停止录像线程 @@ -427,7 +445,10 @@ void UVCCamera::stopRecording() { // 3. 重命名临时文件为目标文件 std::string temp_path = current_video_path_ + ".temp"; if (rename(temp_path.c_str(), current_video_path_.c_str()) != 0) { - throw runtime_error("重命名临时文件失败: " + temp_path + " -> " + current_video_path_); + state_.is_error = true; + state_.error_message = "failed to rename temp file: " + temp_path + " -> " + current_video_path_; + CMVR_LOG(ERROR) << "[UVCCamera] (stopRecording): " << state_.error_message; + return; } current_video_path_.clear(); } @@ -455,7 +476,10 @@ void UVCCamera::streaming_worker_() { // 记录当前帧处理开始时间 auto frame_start_time = std::chrono::high_resolution_clock::now(); if (!cap_.read(frame) || frame.empty()) { - throw runtime_error("[UVCCamera]: Failed to read frame."); + state_.is_error = true; + state_.error_message = "failed to read frame"; + CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << state_.error_message; + break; } // 以下为编码部分，用于流模式 @@ -515,7 +539,7 @@ void UVCCamera::streaming_worker_() { is_streaming_running = false; state_.is_error = true; state_.error_message = e.what(); - LOG(ERROR) << "[UVCCamera]streaming_worker_ error:" << state_.error_message; + CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_ error:" << state_.error_message; } } @@ -580,7 +604,7 @@ void UVCCamera::recording_worker_() { // 写入帧 if (av_interleaved_write_frame(format_context_, packet_) < 0) { - std::cerr << "写入第" << frame_count_ << "帧失败" << std::endl; + CMVR_LOG(ERROR) << "写入第" << frame_count_ << "帧失败"; } std::this_thread::sleep_for(std::chrono::milliseconds(33)); @@ -590,7 +614,7 @@ void UVCCamera::recording_worker_() { av_write_trailer(format_context_); } catch (const std::exception& e) { - std::cerr << "录像线程错误: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "录像线程错误: " << e.what(); } is_recording_running = false; @@ -618,14 +642,14 @@ bool UVCCamera::initSingleEncoder(std::shared_ptr& encoder, if (!codec) codec = avcodec_find_encoder(AV_CODEC_ID_HEVC); } if (!codec) { - std::cerr << "Failed to find " << codec_name << " encoder!" << std::endl; + CMVR_LOG(ERROR) << "Failed to find " << codec_name << " encoder!"; return false; } // 3. 初始化编码器上下文（不变） encoder->codec_context = avcodec_alloc_context3(codec); if (!encoder->codec_context) { - std::cerr << "Failed to allocate codec context!" << std::endl; + CMVR_LOG(ERROR) << "Failed to allocate codec context!"; return false; } @@ -654,11 +678,11 @@ bool UVCCamera::initSingleEncoder(std::shared_ptr& encoder, // 6. 设置像素格式（不变） const enum AVPixelFormat* pix_fmts = codec->pix_fmts; if (!pix_fmts) { - std::cout << "Using default pixel format: YUV420P" << std::endl; + CMVR_LOG(INFO) << "Using default pixel format: YUV420P"; ctx->pix_fmt = AV_PIX_FMT_YUV420P; } else { ctx->pix_fmt = pix_fmts[0]; - std::cout << "Selected pixel format: " << av_get_pix_fmt_name(ctx->pix_fmt) << std::endl; + CMVR_LOG(INFO) << "Selected pixel format: " << av_get_pix_fmt_name(ctx->pix_fmt); } // 7. 打开编码器（不变） @@ -666,21 +690,21 @@ bool UVCCamera::initSingleEncoder(std::shared_ptr& encoder, if (ret < 0) { char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0}; av_strerror(ret, errbuf, sizeof(errbuf)); - std::cerr << "Failed to open " << codec_name << " encoder: " << errbuf << std::endl; + CMVR_LOG(ERROR) << "Failed to open " << codec_name << " encoder: " << errbuf; return false; } // 8. 分配AVFrame（不变） encoder->frame = av_frame_alloc(); if (!encoder->frame) { - std::cerr << "Failed to allocate AVFrame!" << std::endl; + CMVR_LOG(ERROR) << "Failed to allocate AVFrame!"; return false; } encoder->frame->format = ctx->pix_fmt; encoder->frame->width = ctx->width; encoder->frame->height = ctx->height; if (av_frame_get_buffer(encoder->frame, 0) < 0) { - std::cerr << "Failed to allocate AVFrame buffer!" << std::endl; + CMVR_LOG(ERROR) << "Failed to allocate AVFrame buffer!"; av_frame_free(&encoder->frame); return false; } @@ -688,12 +712,12 @@ bool UVCCamera::initSingleEncoder(std::shared_ptr& encoder, // 9. 分配AVPacket（不变） encoder->packet = av_packet_alloc(); if (!encoder->packet) { - std::cerr << "Failed to allocate AVPacket!" << std::endl; + CMVR_LOG(ERROR) << "Failed to allocate AVPacket!"; return false; } - std::cout << "Successfully initialized " << codec_name << " encoder ( " - << width << "x" << height << "@" << fps << "fps )" << std::endl; + CMVR_LOG(INFO) << "Successfully initialized " << codec_name << " encoder ( " + << width << "x" << height << "@" << fps << "fps )"; return true; } @@ -705,7 +729,7 @@ bool UVCCamera::encodeFrameWithEncoder(std::shared_ptr& encod // 确保输入帧尺寸匹配（不变） if (frame.cols != encoder->width || frame.rows != encoder->height) { - std::cerr << "Frame size does not match encoder dimensions" << std::endl; + CMVR_LOG(ERROR) << "Frame size does not match encoder dimensions"; return false; } @@ -721,7 +745,7 @@ bool UVCCamera::encodeFrameWithEncoder(std::shared_ptr& encod } else if (frame.channels() == 1) { src_pix_fmt = AV_PIX_FMT_GRAY8; // 单通道灰度图 } else { - std::cerr << "Unsupported number of channels: " << frame.channels() << std::endl; + CMVR_LOG(ERROR) << "Unsupported number of channels: " << frame.channels(); return false; } // 【修复4：动态创建sws_context（匹配当前输入格式）】 @@ -734,7 +758,7 @@ bool UVCCamera::encodeFrameWithEncoder(std::shared_ptr& encod SWS_BILINEAR, nullptr, nullptr, nullptr ); if (!encoder->sws_context) { - std::cerr << "Failed to create SwsContext" << std::endl; + CMVR_LOG(ERROR) << "Failed to create SwsContext"; return false; } @@ -745,7 +769,7 @@ bool UVCCamera::encodeFrameWithEncoder(std::shared_ptr& encod int ret = sws_scale(encoder->sws_context, src_data, src_linesize, 0, frame.rows, encoder->frame->data, encoder->frame->linesize); if (ret < 0) { - std::cerr << "Error scaling frame" << std::endl; + CMVR_LOG(ERROR) << "Error scaling frame"; return false; } // 发送帧到编码器（不变） @@ -753,7 +777,7 @@ bool UVCCamera::encodeFrameWithEncoder(std::shared_ptr& encod if (ret < 0) { char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0}; av_strerror(ret, errbuf, sizeof(errbuf)); - std::cerr << "Error sending frame to encoder: " << errbuf << std::endl; + CMVR_LOG(ERROR) << "Error sending frame to encoder: " << errbuf; return false; } while (true) @@ -765,17 +789,17 @@ bool UVCCamera::encodeFrameWithEncoder(std::shared_ptr& encod } else if (ret < 0) { char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0}; av_strerror(ret, errbuf, sizeof(errbuf)); - std::cerr << "Error receiving packet from encoder: " << errbuf << std::endl; + CMVR_LOG(ERROR) << "Error receiving packet from encoder: " << errbuf; break; } // 调试：打印帧类型（I帧/P帧） if (encoder->packet->flags & AV_PKT_FLAG_KEY) { is_key = true; - //std::cout << "Encoded I frame (size: " << encoder->packet->size << " bytes)" << std::endl; + //CMVR_LOG(INFO) << "Encoded I frame (size: " << encoder->packet->size << " bytes)"; } else { is_key = false; - //std::cout << "Encoded P frame (size: " << encoder->packet->size << " bytes)" << std::endl; + //CMVR_LOG(INFO) << "Encoded P frame (size: " << encoder->packet->size << " bytes)"; } // 预留足够空间，避免多次内存分配 @@ -797,11 +821,11 @@ bool UVCCamera::encodeFrameWithEncoder(std::shared_ptr& encod has_start_code = true; } if (!has_start_code) { - std::cerr << "Invalid frame: no NALU start code!" << std::endl; + CMVR_LOG(ERROR) << "Invalid frame: no NALU start code!"; return false; } } else { - //std::cerr << "Encoded frame is empty!" << std::endl; + //CMVR_LOG(ERROR) << "Encoded frame is empty!"; return false; } return true; diff --git a/cmvr-es/devices/canbus/abstract_canbus.h b/cmvr-es/devices/canbus/abstract_canbus.h index ca0399d0..b8649125 100644 --- a/cmvr-es/devices/canbus/abstract_canbus.h +++ b/cmvr-es/devices/canbus/abstract_canbus.h @@ -54,8 +54,10 @@ namespace cmvr::device { class AbstractCanbus : public AbstractDevice { public: - AbstractCanbus(const XmlNode &cfg) : AbstractDevice(cfg) {} + AbstractCanbus() = default; ~AbstractCanbus() {} + + DeviceKind kind() const noexcept override { return DeviceKind::CanBus; } /** * @brief Send messages * @param frames The messages to send. @@ -72,8 +74,9 @@ namespace cmvr::device { */ virtual cmvr::msgs::ErrorCode sendSingleFrame( const std::vector &frames) { - CHECK_EQ(frames.size(), 1U) - << "frames size not equal to 1, actual frame size :" << frames.size(); + if (frames.size() != 1U) { + CMVR_LOG(FATAL) << "frames size not equal to 1, actual frame size: " << frames.size(); + } int32_t n = 1; return send(frames, &n); } diff --git a/cmvr-es/devices/canbus/can_client/pcan/pcan_client.cc b/cmvr-es/devices/canbus/can_client/pcan/pcan_client.cc index badf4854..0dc99860 100644 --- a/cmvr-es/devices/canbus/can_client/pcan/pcan_client.cc +++ b/cmvr-es/devices/canbus/can_client/pcan/pcan_client.cc @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" #include "pcan_client.h" #include @@ -14,16 +15,9 @@ namespace cmvr { using cmvr::msgs::ErrorCode; using cmvr::msgs::CANCardParameter; - PcanClient::PcanClient(const XmlNode &cfg) : AbstractCanbus(cfg) { - // 从 XML 读取配置 - // if (cfg.HasAttribute("port")) { - // port_ = static_cast( - // std::stoi(cfg.Attribute("port"))); - // } - // if (cfg.HasAttribute("interface")) { - // interface_ = static_cast( - // std::stoi(cfg.Attribute("interface"))); - // } + PcanClient::PcanClient(const config::SocketCanConfig &cfg) { + port_ = static_cast(cfg.channel_id()); + interface_ = CANCardParameter::NATIVE; enable_can_err_check_ = false; } @@ -31,7 +25,7 @@ namespace cmvr { stop(); } - void PcanClient::init() { + bool PcanClient::init() { #define LPCSTR const char* @@ -91,40 +85,49 @@ namespace cmvr { TPCANStatus stsResult = CAN_LookUpChannel((LPSTR)sParameters, &handle); TPCANStatus ret = CAN_Initialize(PcanHandle, Bitrate); if (ret != PCAN_ERROR_OK) { - LOG(ERROR) << "send message failed, error code: " << ret; + CMVR_LOG(ERROR) << "send message failed, error code: " << ret; + return false; } TPCANStatus state = CAN_GetStatus(PcanHandle); if (state & PCAN_ERROR_BUSOFF) { // 进入 bus-off，尝试复位总线 - LOG(ERROR) << "send message failed, error code: " << ret; + CMVR_LOG(ERROR) << "send message failed, error code: " << ret; + return false; } + return true; } - void PcanClient::start() { + bool PcanClient::start() { is_started_ = true; + return true; } - void PcanClient::stop() { + bool PcanClient::stop() { if (is_started_) { is_started_ = false; CAN_Uninitialize(dev_handler_); - LOG(INFO) << "close socket can ok. port:" << port_; + CMVR_LOG(INFO) << "close socket can ok. port:" << port_; } + return true; } cmvr::msgs::ErrorCode PcanClient::send(const std::vector &frames, int32_t *const frame_num) { - CHECK_NOTNULL(frame_num); - CHECK_EQ(frames.size(), static_cast(*frame_num)); + if (frame_num == nullptr) { + CMVR_LOG(FATAL) << "frame_num is null"; + } + if (frames.size() != static_cast(*frame_num)) { + CMVR_LOG(FATAL) << "frames size does not match frame_num"; + } if (!is_started_) { - LOG(ERROR) << "Nvidia can client has not been initiated! Please init first!"; + CMVR_LOG(ERROR) << "Nvidia can client has not been initiated! Please init first!"; return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } for (size_t i = 0; i < frames.size() && i < MAX_CAN_SEND_FRAME_LEN; ++i) { if (frames[i].len > CANBUS_MESSAGE_LENGTH || frames[i].len < 0) { - LOG(ERROR) << "frames[" << i << "].len = " << frames[i].len + CMVR_LOG(ERROR) << "frames[" << i << "].len = " << frames[i].len << ", which is not equal to can message data length (" << CANBUS_MESSAGE_LENGTH << ")."; return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; @@ -134,14 +137,14 @@ namespace cmvr { } else { send_frames_[i].ID = (frames[i].id & CAN_SFF_MASK); } - // LOG(INFO) << "send can id is " << send_frames_[i].can_id; + // CMVR_LOG(INFO) << "send can id is " << send_frames_[i].can_id; send_frames_[i].LEN = frames[i].len; std::memcpy(send_frames_[i].DATA, frames[i].data, frames[i].len); send_frames_[i].MSGTYPE = PCAN_MESSAGE_EXTENDED; // Synchronous transmission of CAN messages TPCANStatus ret = CAN_Write(PcanHandle, &send_frames_[i]); if (ret != PCAN_ERROR_OK) { - LOG(ERROR) << "send message failed, error code: " << ret; + CMVR_LOG(ERROR) << "send message failed, error code: " << ret; return ErrorCode::CAN_CLIENT_ERROR_BASE; } } @@ -151,12 +154,12 @@ namespace cmvr { cmvr::msgs::ErrorCode PcanClient::receive(std::vector *const frames, int32_t *const frame_num) { if (!is_started_) { - LOG(ERROR) << "Nvidia can client is not init! Please init first!"; + CMVR_LOG(ERROR) << "Nvidia can client is not init! Please init first!"; return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; } if (*frame_num > MAX_CAN_RECV_FRAME_LEN || *frame_num < 0) { - LOG(ERROR) << "recv can frame num not in range[0, " << MAX_CAN_RECV_FRAME_LEN + CMVR_LOG(ERROR) << "recv can frame num not in range[0, " << MAX_CAN_RECV_FRAME_LEN << "], frame_num:" << *frame_num; // TODO(Authors): check the difference of returning frame_num/error_code return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM; @@ -169,12 +172,12 @@ namespace cmvr { TPCANStatus ret = CAN_Read(PcanHandle, &recv_frames_[i], &CANTimeStamp); if (ret != PCAN_ERROR_OK) { - LOG(ERROR) << "receive message failed, error code: " << ret; + CMVR_LOG(ERROR) << "receive message failed, error code: " << ret; return ErrorCode::CAN_CLIENT_ERROR_BASE; } if (recv_frames_[i].LEN > CANBUS_MESSAGE_LENGTH || recv_frames_[i].LEN < 0) { - LOG(ERROR) << "recv_frames_[" << i + CMVR_LOG(ERROR) << "recv_frames_[" << i << "].can_dlc = " << recv_frames_[i].LEN << ", which is not equal to can message data length (" << CANBUS_MESSAGE_LENGTH << ")."; @@ -187,7 +190,7 @@ namespace cmvr { } else { cf.id = (recv_frames_[i].ID & CAN_SFF_MASK); } - LOG(INFO) << "Socket can receive can id is " << recv_frames_[i].ID; + CMVR_LOG(INFO) << "Socket can receive can id is " << recv_frames_[i].ID; cf.len = recv_frames_[i].LEN; std::memcpy(cf.data, recv_frames_[i].DATA, recv_frames_[i].LEN); frames->push_back(cf); diff --git a/cmvr-es/devices/canbus/can_client/pcan/pcan_client.h b/cmvr-es/devices/canbus/can_client/pcan/pcan_client.h index ba302c4c..94401e65 100644 --- a/cmvr-es/devices/canbus/can_client/pcan/pcan_client.h +++ b/cmvr-es/devices/canbus/can_client/pcan/pcan_client.h @@ -26,6 +26,7 @@ #include "cmvr/msgs/error_code.pb.h" #include "cmvr/msgs/can_card_parameter.pb.h" +#include "cmvr/config/motor_config/motor_config.pb.h" #include "gflags/gflags.h" #include "../../abstract_canbus.h" @@ -38,11 +39,12 @@ namespace cmvr { class PcanClient final : public AbstractCanbus { public: - explicit PcanClient(const XmlNode &cfg); + explicit PcanClient(const config::SocketCanConfig &cfg); ~PcanClient(); - void init() override; - void start() override; - void stop() override; + std::string typeName() const override { return "PcanClient"; } + bool init() override; + bool start() override; + bool stop() override; /** * @brief Send messages diff --git a/cmvr-es/devices/canbus/can_client/pcan/pcan_client_test.cpp b/cmvr-es/devices/canbus/can_client/pcan/pcan_client_test.cpp index 41e01e99..e493b851 100644 --- a/cmvr-es/devices/canbus/can_client/pcan/pcan_client_test.cpp +++ b/cmvr-es/devices/canbus/can_client/pcan/pcan_client_test.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by lgv on 2025/8/7. // @@ -15,8 +16,9 @@ namespace cmvr { param.set_brand(CANCardParameter::SOCKET_CAN_RAW); param.set_channel_id(CANCardParameter::CHANNEL_ID_ZERO); - XmlNode xml; - PcanClient socket_can_client(xml); + config::SocketCanConfig cfg; + cfg.set_channel_id(0); + PcanClient socket_can_client(cfg); socket_can_client.init(); @@ -29,7 +31,7 @@ namespace cmvr { // ++num; // EXPECT_EQ(socket_can_client.receive(&frames, &num), // ErrorCode::OK); - // LOG(INFO) << frames.at(0).CanFrameString(); + // CMVR_LOG(INFO) << frames.at(0).CanFrameString(); CanFrame can_frame; can_frame.id = 0x123; can_frame.len = 8; @@ -41,4 +43,4 @@ namespace cmvr { socket_can_client.stop(); } } -} \ No newline at end of file +} diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc index aef0af2c..21d9340e 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by lgv on 2025/7/14. // @@ -19,26 +20,18 @@ namespace cmvr { using cmvr::msgs::ErrorCode; using cmvr::msgs::CANCardParameter; - SocketCanClientRaw::SocketCanClientRaw(const XmlNode &cfg) : AbstractCanbus(cfg) { - - auto channel_id = cfg.getAttrDefault("channelId", 1); + SocketCanClientRaw::SocketCanClientRaw(const config::SocketCanConfig &cfg) { + auto channel_id = cfg.channel_id(); port_ = static_cast(channel_id); interface_ = CANCardParameter::NATIVE; enable_can_err_check_ = false; - - - // try { - // interface_ = - // - // }catch (const exception& e) - // LOG(ERROR) << "[SocketCanClientRaw] ([SocketCanClientRaw]): Failed to parse XML: " << e.what(); } - void SocketCanClientRaw::init() { + bool SocketCanClientRaw::init() { // if (!parameter.has_channel_id()) { - // LOG(ERROR) << "Init CAN failed: parameter does not have channel id. The " + // CMVR_LOG(ERROR) << "Init CAN failed: parameter does not have channel id. The " // "parameter is " // << parameter.DebugString(); // // return false; @@ -48,11 +41,11 @@ namespace cmvr { // interface_ = parameter.interface(); // auto num_ports = parameter.num_ports(); // if (port_ > static_cast(num_ports) || port_ < 0) { - // LOG(ERROR) << "Can port number [" << port_ << "] is out of range [0, " + // CMVR_LOG(ERROR) << "Can port number [" << port_ << "] is out of range [0, " // << num_ports << ") !"; // // return false; // } - // // return true; + return true; } SocketCanClientRaw::~SocketCanClientRaw() { @@ -61,9 +54,10 @@ namespace cmvr { } } - void SocketCanClientRaw::start() { + bool SocketCanClientRaw::start() { if (is_started_) { status_ = ErrorCode::OK; + return true; } struct sockaddr_can addr; struct ifreq ifr; @@ -75,8 +69,9 @@ namespace cmvr { // &dev_handler_); dev_handler_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); if (dev_handler_ < 0) { - LOG(ERROR) << "open device error code [" << dev_handler_ << "]: "; + CMVR_LOG(ERROR) << "open device error code [" << dev_handler_ << "]: "; status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + return false; } // init config and state @@ -94,8 +89,9 @@ namespace cmvr { ret = setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)); if (ret < 0) { - LOG(ERROR) << "add receive msg id filter error code: " << ret; + CMVR_LOG(ERROR) << "add receive msg id filter error code: " << ret; status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + return false; } } @@ -104,8 +100,9 @@ namespace cmvr { ret = ::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable, sizeof(enable)); if (ret < 0) { - LOG(ERROR) << "enable reception of can frame error code: " << ret; + CMVR_LOG(ERROR) << "enable reception of can frame error code: " << ret; status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + return false; } std::string interface_prefix; @@ -121,8 +118,9 @@ namespace cmvr { const std::string can_name = absl::StrCat(interface_prefix, port_); std::strncpy(ifr.ifr_name, can_name.c_str(), IFNAMSIZ); if (ioctl(dev_handler_, SIOCGIFINDEX, &ifr) < 0) { - LOG(ERROR) << "ioctl error"; + CMVR_LOG(ERROR) << "ioctl error"; status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + return false; } // bind socket to network interface @@ -133,40 +131,48 @@ namespace cmvr { sizeof(addr)); if (ret < 0) { - LOG(ERROR) << "bind socket to network interface error code: " << ret; + CMVR_LOG(ERROR) << "bind socket to network interface error code: " << ret; status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + return false; } is_started_ = true; status_ = ErrorCode::OK; + return true; } - void SocketCanClientRaw::stop() { + bool SocketCanClientRaw::stop() { if (is_started_) { is_started_ = false; int ret = close(dev_handler_); if (ret < 0) { - LOG(ERROR) << "close error code:" << ret << ", " << getErrorString(ret); + CMVR_LOG(ERROR) << "close error code:" << ret << ", " << getErrorString(ret); + return false; } else { - LOG(INFO) << "close socket can ok. port:" << port_; + CMVR_LOG(INFO) << "close socket can ok. port:" << port_; } } + return true; } // Synchronous transmission of CAN messages ErrorCode SocketCanClientRaw::send(const std::vector &frames, int32_t *const frame_num) { - CHECK_NOTNULL(frame_num); - CHECK_EQ(frames.size(), static_cast(*frame_num)); + if (frame_num == nullptr) { + CMVR_LOG(FATAL) << "frame_num is null"; + } + if (frames.size() != static_cast(*frame_num)) { + CMVR_LOG(FATAL) << "frames size does not match frame_num"; + } if (!is_started_) { - LOG(ERROR) << "Nvidia can client has not been initiated! Please init first!"; + CMVR_LOG(ERROR) << "Nvidia can client has not been initiated! Please init first!"; return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } for (size_t i = 0; i < frames.size() && i < MAX_CAN_SEND_FRAME_LEN; ++i) { if (frames[i].len > CANBUS_MESSAGE_LENGTH || frames[i].len < 0) { - LOG(ERROR) << "frames[" << i << "].len = " << frames[i].len + CMVR_LOG(ERROR) << "frames[" << i << "].len = " << frames[i].len << ", which is not equal to can message data length (" << CANBUS_MESSAGE_LENGTH << ")."; return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; @@ -176,7 +182,7 @@ namespace cmvr { } else { send_frames_[i].can_id = (frames[i].id & CAN_SFF_MASK); } - // LOG(INFO) << "send can id is " << send_frames_[i].can_id; + // CMVR_LOG(INFO) << "send can id is " << send_frames_[i].can_id; send_frames_[i].can_dlc = frames[i].len; std::memcpy(send_frames_[i].data, frames[i].data, frames[i].len); @@ -184,7 +190,7 @@ namespace cmvr { int ret = static_cast( write(dev_handler_, &send_frames_[i], sizeof(send_frames_[i]))); if (ret <= 0) { - LOG(ERROR) << "can " << port_ << " send message failed, error code: " << ret; + CMVR_LOG(ERROR) << "can " << port_ << " send message failed, error code: " << ret; return ErrorCode::CAN_CLIENT_ERROR_BASE; } } @@ -196,12 +202,12 @@ namespace cmvr { ErrorCode SocketCanClientRaw::receive(std::vector *const frames, int32_t *const frame_num) { if (!is_started_) { - LOG(ERROR) << "Nvidia can client is not init! Please init first!"; + CMVR_LOG(ERROR) << "Nvidia can client is not init! Please init first!"; return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; } if (*frame_num > MAX_CAN_RECV_FRAME_LEN || *frame_num < 0) { - LOG(ERROR) << "recv can frame num not in range[0, " << MAX_CAN_RECV_FRAME_LEN + CMVR_LOG(ERROR) << "recv can frame num not in range[0, " << MAX_CAN_RECV_FRAME_LEN << "], frame_num:" << *frame_num; // TODO(Authors): check the difference of returning frame_num/error_code return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM; @@ -212,12 +218,12 @@ namespace cmvr { auto ret = read(dev_handler_, &recv_frames_[i], sizeof(recv_frames_[i])); if (ret < 0) { - LOG(ERROR) << "receive message failed, error code: " << ret; + CMVR_LOG(ERROR) << "receive message failed, error code: " << ret; return ErrorCode::CAN_CLIENT_ERROR_BASE; } if (recv_frames_[i].can_dlc > CANBUS_MESSAGE_LENGTH || recv_frames_[i].can_dlc < 0) { - LOG(ERROR) << "recv_frames_[" << i + CMVR_LOG(ERROR) << "recv_frames_[" << i << "].can_dlc = " << recv_frames_[i].can_dlc << ", which is not equal to can message data length (" << CANBUS_MESSAGE_LENGTH << ")."; @@ -230,7 +236,7 @@ namespace cmvr { } else { cf.id = (recv_frames_[i].can_id & CAN_SFF_MASK); } - // LOG(INFO) << "Socket can receive can id is " << recv_frames_[i].can_id; + // CMVR_LOG(INFO) << "Socket can receive can id is " << recv_frames_[i].can_id; cf.len = recv_frames_[i].can_dlc; std::memcpy(cf.data, recv_frames_[i].data, recv_frames_[i].can_dlc); frames->push_back(cf); diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h index 3de5b42c..7f478c86 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h @@ -22,6 +22,7 @@ #include "cmvr/msgs/error_code.pb.h" #include "cmvr/msgs/can_card_parameter.pb.h" +#include "cmvr/config/motor_config/motor_config.pb.h" #include "gflags/gflags.h" #include "../../abstract_canbus.h" @@ -35,11 +36,12 @@ namespace cmvr { */ class SocketCanClientRaw final : public AbstractCanbus { public: - explicit SocketCanClientRaw(const XmlNode &cfg); + explicit SocketCanClientRaw(const config::SocketCanConfig &cfg); ~SocketCanClientRaw(); - void init() override; - void start() override; - void stop() override; + std::string typeName() const override { return "SocketCanClientRaw"; } + bool init() override; + bool start() override; + bool stop() override; /** * @brief Send messages diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc index 41a3a440..391be9d3 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by lgv on 2025/7/16. // @@ -15,8 +16,9 @@ namespace device { param.set_brand(CANCardParameter::SOCKET_CAN_RAW); param.set_channel_id(CANCardParameter::CHANNEL_ID_ZERO); - XmlNode xml; - SocketCanClientRaw socket_can_client(xml); + cmvr::config::SocketCanConfig cfg; + cfg.set_channel_id(0); + SocketCanClientRaw socket_can_client(cfg); // EXPECT_EQ(socket_can_client.start(), ErrorCode::CAN_CLIENT_ERROR_BASE); socket_can_client.start(); @@ -27,7 +29,7 @@ namespace device { ++num; EXPECT_EQ(socket_can_client.receive(&frames, &num), ErrorCode::OK); - LOG(INFO) << frames.at(0).CanFrameString(); + CMVR_LOG(INFO) << frames.at(0).CanFrameString(); CanFrame can_frame; can_frame.id = 0x123; can_frame.len = 8; @@ -39,4 +41,4 @@ namespace device { socket_can_client.stop(); } } -} \ No newline at end of file +} diff --git a/cmvr-es/devices/canbus/can_comm/can_receiver.h b/cmvr-es/devices/canbus/can_comm/can_receiver.h index 7c80806e..f6a2512e 100644 --- a/cmvr-es/devices/canbus/can_comm/can_receiver.h +++ b/cmvr-es/devices/canbus/can_comm/can_receiver.h @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" /****************************************************************************** * Copyright 2017 The Apollo Authors. All Rights Reserved. * @@ -113,11 +114,11 @@ namespace cmvr { pt_manager_ = pt_manager; enable_log_ = enable_log; if (can_client_ == nullptr) { - LOG(ERROR) << "Invalid can client."; + CMVR_LOG(ERROR) << "Invalid can client."; return cmvr::msgs::ErrorCode::CANBUS_ERROR; } if (pt_manager_ == nullptr) { - LOG(ERROR) << "Invalid protocol manager."; + CMVR_LOG(ERROR) << "Invalid protocol manager."; return cmvr::msgs::ErrorCode::CANBUS_ERROR; } is_init_ = true; @@ -126,9 +127,13 @@ namespace cmvr { template void CanReceiver::RecvThreadFunc() { - LOG(INFO) << "Can client receiver thread starts."; - CHECK_NOTNULL(can_client_); - CHECK_NOTNULL(pt_manager_); + CMVR_LOG(INFO) << "Can client receiver thread starts."; + if (can_client_ == nullptr) { + CMVR_LOG(FATAL) << "CAN receiver client is null"; + } + if (pt_manager_ == nullptr) { + CMVR_LOG(FATAL) << "CAN receiver protocol manager is null"; + } int32_t receive_error_count = 0; int32_t receive_none_count = 0; @@ -140,7 +145,7 @@ namespace cmvr { int32_t frame_num = MAX_CAN_RECV_FRAME_LEN; if (can_client_->receive(&buf, &frame_num) != cmvr::msgs::ErrorCode::OK) { - LOG_IF_EVERY_N(ERROR, receive_error_count++ > ERROR_COUNT_MAX, + CMVR_LOG_IF_EVERY_N(ERROR, receive_error_count++ > ERROR_COUNT_MAX, ERROR_COUNT_MAX) << "Received " << receive_error_count << " error messages."; std::this_thread::sleep_for(std::chrono::microseconds(default_period)); @@ -149,13 +154,13 @@ namespace cmvr { receive_error_count = 0; if (buf.size() != static_cast(frame_num)) { - LOG_EVERY_N(ERROR,100) << "Receiver buf size [" << buf.size() + CMVR_LOG_EVERY_N(ERROR,100) << "Receiver buf size [" << buf.size() << "] does not match can_client returned length[" << frame_num << "]."; } if (frame_num == 0) { - LOG_IF_EVERY_N(ERROR, receive_none_count++ > ERROR_COUNT_MAX, + CMVR_LOG_IF_EVERY_N(ERROR, receive_none_count++ > ERROR_COUNT_MAX, ERROR_COUNT_MAX) << "Received " << receive_none_count << " empty messages."; std::this_thread::sleep_for(std::chrono::microseconds(default_period)); @@ -169,12 +174,12 @@ namespace cmvr { const uint8_t *data = frame.data; pt_manager_->Parse(uid, data, len); if (enable_log_) { - LOG(INFO) << "recv_can_frame#" << frame.CanFrameString(); + CMVR_LOG(INFO) << "recv_can_frame#" << frame.CanFrameString(); } } std::this_thread::yield(); } - LOG(INFO) << "Can client receiver thread stopped."; + CMVR_LOG(INFO) << "Can client receiver thread stopped."; } template @@ -197,13 +202,13 @@ namespace cmvr { template void CanReceiver::Stop() { if (IsRunning()) { - LOG(INFO) << "Stopping can client receiver ..."; + CMVR_LOG(INFO) << "Stopping can client receiver ..."; is_running_.exchange(false); async_result_.wait(); } else { - LOG(INFO) << "Can client receiver is not running."; + CMVR_LOG(INFO) << "Can client receiver is not running."; } - LOG(INFO) << "Can client receiver stopped [ok]."; + CMVR_LOG(INFO) << "Can client receiver stopped [ok]."; } } } diff --git a/cmvr-es/devices/canbus/can_comm/can_receiver_test.cc b/cmvr-es/devices/canbus/can_comm/can_receiver_test.cc index 4a8acf81..eade3843 100644 --- a/cmvr-es/devices/canbus/can_comm/can_receiver_test.cc +++ b/cmvr-es/devices/canbus/can_comm/can_receiver_test.cc @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by lgv on 2025/7/16. // @@ -84,8 +85,9 @@ namespace cmvr { TEST(CanReceiverTest, ReceiveTest) { - XmlNode xml_node; - SocketCanClientRaw can_client(xml_node); + cmvr::config::SocketCanConfig cfg; + cfg.set_channel_id(0); + SocketCanClientRaw can_client(cfg); can_client.start(); TestMessageManager pm; @@ -99,7 +101,7 @@ namespace cmvr { while (true) { MySensorData data; pm.GetSensorData(&data); - LOG(INFO) << "Received data: " << data.speed << " " << data.enable; + CMVR_LOG(INFO) << "Received data: " << data.speed << " " << data.enable; std::this_thread::sleep_for(std::chrono::milliseconds(100)); } @@ -110,4 +112,4 @@ namespace cmvr { } } -} \ No newline at end of file +} diff --git a/cmvr-es/devices/canbus/can_comm/can_sender.h b/cmvr-es/devices/canbus/can_comm/can_sender.h index ae2e8336..63e4b42c 100644 --- a/cmvr-es/devices/canbus/can_comm/can_sender.h +++ b/cmvr-es/devices/canbus/can_comm/can_sender.h @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" /****************************************************************************** * Copyright 2017 The Apollo Authors. All Rights Reserved. * @@ -259,7 +260,7 @@ namespace cmvr { template void SenderMessage::Update() { if (protocol_data_ == nullptr) { - LOG(ERROR) << "Attention: ProtocolData is nullptr!"; + CMVR_LOG(ERROR) << "Attention: ProtocolData is nullptr!"; return; } protocol_data_->UpdateData(can_frame_to_update_.data); @@ -289,7 +290,9 @@ namespace cmvr { template void CanSender::PowerSendThreadFunc() { - CHECK_NOTNULL(can_client_); + if (can_client_ == nullptr) { + CMVR_LOG(FATAL) << "CAN sender client is null"; + } sched_param sch; sch.sched_priority = 99; pthread_setschedparam(pthread_self(), SCHED_FIFO, &sch); @@ -302,7 +305,7 @@ namespace cmvr { int64_t tm_end = 0; int64_t sleep_interval = 0; - LOG(INFO) << "Can client sender thread starts."; + CMVR_LOG(INFO) << "Can client sender thread starts."; while (is_running_) { tm_start = std::chrono::duration_cast( @@ -323,10 +326,10 @@ namespace cmvr { CanFrame can_frame = message.CanFrame(); can_frames.push_back(can_frame); if (can_client_->sendSingleFrame(can_frames) != cmvr::msgs::ErrorCode::OK) { - LOG(ERROR) << "Send msg failed:" << can_frame.CanFrameString(); + CMVR_LOG(ERROR) << "Send msg failed:" << can_frame.CanFrameString(); } if (enable_log()) { - LOG(INFO) << "send_can_frame#" << can_frame.CanFrameString(); + CMVR_LOG(INFO) << "send_can_frame#" << can_frame.CanFrameString(); } if (message.send_once()) { @@ -342,11 +345,11 @@ namespace cmvr { std::this_thread::sleep_for(std::chrono::microseconds(sleep_interval)); } else { // do not sleep - LOG(WARNING) << "Too much time for calculation: " << tm_end - tm_start + CMVR_LOG(WARNING) << "Too much time for calculation: " << tm_end - tm_start << "us is more than minimum period: " << delta_period << "us"; } } - LOG(INFO) << "Can client sender thread stopped!"; + CMVR_LOG(INFO) << "Can client sender thread stopped!"; } template @@ -357,10 +360,10 @@ namespace cmvr { can_frame.len = 0; can_frames.push_back(can_frame); if (can_client_->sendSingleFrame(can_frames) != cmvr::msgs::ErrorCode::OK) { - LOG(ERROR) << "Send msg failed:" << can_frame.CanFrameString(); + CMVR_LOG(ERROR) << "Send msg failed:" << can_frame.CanFrameString(); } if (enable_log()) { - LOG(INFO) << "send_can_frame#" << can_frame.CanFrameString(); + CMVR_LOG(INFO) << "send_can_frame#" << can_frame.CanFrameString(); } } @@ -368,11 +371,11 @@ namespace cmvr { msgs::ErrorCode CanSender::Init(AbstractCanbus *can_client, bool enable_log) { if (is_init_) { - LOG(ERROR) << "Duplicated Init request."; + CMVR_LOG(ERROR) << "Duplicated Init request."; return msgs::ErrorCode::CANBUS_ERROR; } if (can_client == nullptr) { - LOG(ERROR) << "Invalid can client."; + CMVR_LOG(ERROR) << "Invalid can client."; return msgs::ErrorCode::CANBUS_ERROR; } is_init_ = true; @@ -386,20 +389,20 @@ namespace cmvr { ProtocolData *protocol_data, bool send_with_one) { if (protocol_data == nullptr) { - LOG(ERROR) << "invalid protocol data."; + CMVR_LOG(ERROR) << "invalid protocol data."; return; } // send_messages_.emplace_back( // SenderMessage(message_id, protocol_data, send_with_one)); send_messages_[message_id] = SenderMessage(message_id, protocol_data, send_with_one); - LOG(INFO) << "Add send message:" << std::hex << message_id; + CMVR_LOG(INFO) << "Add send message:" << std::hex << message_id; } template msgs::ErrorCode CanSender::Start() { if (is_running_) { - LOG(ERROR) << "Cansender has already started."; + CMVR_LOG(ERROR) << "Cansender has already started."; return msgs::ErrorCode::CANBUS_ERROR; } is_running_ = true; @@ -421,7 +424,7 @@ namespace cmvr { if (it != send_messages_.end()) { it->second.Update(); } else { - LOG(WARNING) << "Update failed: message_id 0x" << std::hex << message_id << " not found."; + CMVR_LOG(WARNING) << "Update failed: message_id 0x" << std::hex << message_id << " not found."; } } @@ -432,7 +435,7 @@ namespace cmvr { it->second.Update(); it->second.mark_as_sent(); } else { - LOG(WARNING) << "Update failed: message_id 0x" << std::hex << message_id << " not found."; + CMVR_LOG(WARNING) << "Update failed: message_id 0x" << std::hex << message_id << " not found."; } @@ -440,10 +443,10 @@ namespace cmvr { CanFrame can_frame = it->second.CanFrame(); can_frames.push_back(can_frame); if (can_client_->sendSingleFrame(can_frames) != cmvr::msgs::ErrorCode::OK) { - LOG(ERROR) << "Send msg failed:" << can_frame.CanFrameString(); + CMVR_LOG(ERROR) << "Send msg failed:" << can_frame.CanFrameString(); } if (enable_log()) { - LOG(INFO) << "send_can_frame#" << can_frame.CanFrameString(); + CMVR_LOG(INFO) << "send_can_frame#" << can_frame.CanFrameString(); } @@ -452,17 +455,17 @@ namespace cmvr { template void CanSender::Stop() { if (is_running_) { - LOG(INFO) << "Stopping can sender ..."; + CMVR_LOG(INFO) << "Stopping can sender ..."; is_running_ = false; if (thread_ != nullptr && thread_->joinable()) { thread_->join(); } thread_.reset(); } else { - LOG(ERROR) << "CanSender is not running."; + CMVR_LOG(ERROR) << "CanSender is not running."; } - LOG(INFO) << "Can client sender stopped [ok]."; + CMVR_LOG(INFO) << "Can client sender stopped [ok]."; } template diff --git a/cmvr-es/devices/canbus/can_comm/can_sender_test.cc b/cmvr-es/devices/canbus/can_comm/can_sender_test.cc index 73216247..39e941ad 100644 --- a/cmvr-es/devices/canbus/can_comm/can_sender_test.cc +++ b/cmvr-es/devices/canbus/can_comm/can_sender_test.cc @@ -79,8 +79,9 @@ namespace cmvr { TEST(CanSenderTest, OneRunCase) { - XmlNode xml_node; - SocketCanClientRaw can_client(xml_node); + cmvr::config::SocketCanConfig cfg; + cfg.set_channel_id(0); + SocketCanClientRaw can_client(cfg); can_client.start(); CanSender sender; @@ -114,4 +115,4 @@ namespace cmvr { } -} \ No newline at end of file +} diff --git a/cmvr-es/devices/canbus/can_comm/message_manager.h b/cmvr-es/devices/canbus/can_comm/message_manager.h index 409d9983..44189301 100644 --- a/cmvr-es/devices/canbus/can_comm/message_manager.h +++ b/cmvr-es/devices/canbus/can_comm/message_manager.h @@ -31,7 +31,7 @@ #include #include #include -#include +#include "common/base/logging/logger.h" #include "cmvr/msgs/error_code.pb.h" @@ -215,7 +215,7 @@ namespace cmvr { *MessageManager::GetMutableProtocolDataById( const uint32_t message_id) { if (protocol_data_map_.find(message_id) == protocol_data_map_.end()) { - // LOG(WARNING) << "Unable to get protocol data because of invalid message_id:" + // CMVR_LOG(WARNING) << "Unable to get protocol data because of invalid message_id:" // << Byte::byte_to_hex(message_id); return nullptr; } @@ -270,7 +270,7 @@ namespace cmvr { ErrorCode MessageManager::GetSensorData( SensorType *const sensor_data) { if (sensor_data == nullptr) { - LOG(ERROR) << "Failed to get sensor_data due to nullptr."; + CMVR_LOG(ERROR) << "Failed to get sensor_data due to nullptr."; return ErrorCode::CANBUS_ERROR; } std::lock_guard lock(sensor_data_mutex_); @@ -282,7 +282,7 @@ namespace cmvr { void MessageManager::ResetSendMessages() { for (auto &protocol_data: send_protocol_data_) { if (protocol_data == nullptr) { - LOG(ERROR) << "Invalid protocol data."; + CMVR_LOG(ERROR) << "Invalid protocol data."; } else { protocol_data->Reset(); } diff --git a/cmvr-es/devices/canbus/canopen/nmt_request_protocol.h b/cmvr-es/devices/canbus/canopen/nmt_request_protocol.h index 3a2d5bbf..75b6b1a1 100644 --- a/cmvr-es/devices/canbus/canopen/nmt_request_protocol.h +++ b/cmvr-es/devices/canbus/canopen/nmt_request_protocol.h @@ -11,7 +11,7 @@ #include "canbus/can_comm/protocol_data.h" #include "cmvr/msgs/robot_detail.pb.h" #include "sdo_response_protocol.h" -#include "glog/logging.h" +#include "common/base/logging/logger.h" #include namespace cmvr { namespace device { diff --git a/cmvr-es/devices/canbus/canopen/nmt_response_protocol.h b/cmvr-es/devices/canbus/canopen/nmt_response_protocol.h index 08b7490d..3f2b8483 100644 --- a/cmvr-es/devices/canbus/canopen/nmt_response_protocol.h +++ b/cmvr-es/devices/canbus/canopen/nmt_response_protocol.h @@ -13,7 +13,7 @@ #include "canbus/can_comm/protocol_data.h" #include "cmvr/msgs/robot_detail.pb.h" #include "sdo_response_protocol.h" -#include "glog/logging.h" +#include "common/base/logging/logger.h" namespace cmvr { namespace device { template @@ -37,7 +37,7 @@ namespace cmvr { // void Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const override { // if (length < 1) { - // LOG(WARNING) << "NmtResponseProtocol: data length too short: " << length; + // CMVR_LOG(WARNING) << "NmtResponseProtocol: data length too short: " << length; // return; // } // auto &motors_map = *sensor_data->mutable_motors(); @@ -47,7 +47,7 @@ namespace cmvr { void Parse(const std::uint8_t *bytes, int32_t length, SensorType *sensor_data) const override { if (length < 1) { - LOG(WARNING) << "NmtResponseProtocol: data length too short: " << length; + CMVR_LOG(WARNING) << "NmtResponseProtocol: data length too short: " << length; return; } // 子类实现的扩展解析data逻辑 diff --git a/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h b/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h index 7192790a..69366ef4 100644 --- a/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h +++ b/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h @@ -7,7 +7,7 @@ #include "canbus/can_comm/protocol_data.h" #include "cmvr/msgs/robot_detail.pb.h" #include "sdo_response_protocol.h" -#include "glog/logging.h" +#include "common/base/logging/logger.h" namespace cmvr { namespace device { diff --git a/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h b/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h index 8694899d..0a48af3e 100644 --- a/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h +++ b/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h @@ -7,7 +7,7 @@ #include "canbus/can_comm/protocol_data.h" #include "cmvr/msgs/robot_detail.pb.h" #include "sdo_response_protocol.h" -#include "glog/logging.h" +#include "common/base/logging/logger.h" namespace cmvr { namespace device { template @@ -41,7 +41,7 @@ namespace cmvr { template void SdoResponseProtocol::Parse(const std::uint8_t *bytes, int32_t length,SensorType *sensor_data) const { if (length < 8) { - LOG(WARNING) << "MotorSdoResponseProtocol: data length too short: " << length; + CMVR_LOG(WARNING) << "MotorSdoResponseProtocol: data length too short: " << length; return; } @@ -79,7 +79,7 @@ namespace cmvr { break; default: - LOG(WARNING) << "Unknown command specifier: " << std::hex << static_cast(command); + CMVR_LOG(WARNING) << "Unknown command specifier: " << std::hex << static_cast(command); data = 0; break; } diff --git a/cmvr-es/devices/canbus/canopen/sync_protocol.h b/cmvr-es/devices/canbus/canopen/sync_protocol.h index 4794584a..d95bd85f 100644 --- a/cmvr-es/devices/canbus/canopen/sync_protocol.h +++ b/cmvr-es/devices/canbus/canopen/sync_protocol.h @@ -5,7 +5,7 @@ #include "canbus/can_comm/protocol_data.h" #include "cmvr/msgs/robot_detail.pb.h" #include "sdo_response_protocol.h" -#include "glog/logging.h" +#include "common/base/logging/logger.h" #include namespace cmvr { namespace device { diff --git a/cmvr-es/devices/device_types.h b/cmvr-es/devices/device_types.h new file mode 100644 index 00000000..c5c08b9e --- /dev/null +++ b/cmvr-es/devices/device_types.h @@ -0,0 +1,67 @@ +#ifndef CMVR_ES_DEVICE_TYPES_H +#define CMVR_ES_DEVICE_TYPES_H + +#include + +namespace cmvr::device { + + enum class DeviceKind { + Unknown, + AGV, + Arm, + Battery, + BioHead, + Camera, + CanBus, + DexHand, + Gripper, + Microphone, + Motor, + MotorSystem, + Robot, + Speaker, + }; + + inline std::string toString(DeviceKind kind) { + switch (kind) { + case DeviceKind::AGV: + return "AGV"; + case DeviceKind::Arm: + return "Arm"; + case DeviceKind::Battery: + return "Battery"; + case DeviceKind::BioHead: + return "BioHead"; + case DeviceKind::Camera: + return "Camera"; + case DeviceKind::CanBus: + return "CanBus"; + case DeviceKind::DexHand: + return "DexHand"; + case DeviceKind::Gripper: + return "Gripper"; + case DeviceKind::Microphone: + return "Microphone"; + case DeviceKind::Motor: + return "Motor"; + case DeviceKind::MotorSystem: + return "MotorSystem"; + case DeviceKind::Robot: + return "Robot"; + case DeviceKind::Speaker: + return "Speaker"; + case DeviceKind::Unknown: + default: + return "Unknown"; + } + } + + struct DeviceInfo { + std::string id; + DeviceKind kind = DeviceKind::Unknown; + std::string type_name; + }; + +} // namespace cmvr::device + +#endif // CMVR_ES_DEVICE_TYPES_H diff --git a/cmvr-es/devices/dexhand/CMakeLists.txt b/cmvr-es/devices/dexhand/CMakeLists.txt index f383dbb7..41130532 100644 --- a/cmvr-es/devices/dexhand/CMakeLists.txt +++ b/cmvr-es/devices/dexhand/CMakeLists.txt @@ -1,2 +1,15 @@ add_subdirectory(rh56dftp_dexhand) add_subdirectory(px_6ax_gen3) + +add_library(dexhand INTERFACE) + +target_include_directories(dexhand INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(dexhand + INTERFACE + cmvr_es::device::rh56dftp_dexhand + cmvr_es::device::px_6ax_gen3 + cmvr_es::proto +) + +add_library(cmvr_es::device::dexhand ALIAS dexhand) diff --git a/cmvr-es/devices/dexhand/abstract_dexhand.h b/cmvr-es/devices/dexhand/abstract_dexhand.h index 852bd6f4..1cf8b9c7 100644 --- a/cmvr-es/devices/dexhand/abstract_dexhand.h +++ b/cmvr-es/devices/dexhand/abstract_dexhand.h @@ -9,16 +9,18 @@ #include #include #include -#include #include #include #include #include "devices/abstract_device.h" +#include "common/base/logging/logger.h" namespace cmvr::device { class AbstractDexHand : public AbstractDevice { public: + DeviceKind kind() const noexcept override { return DeviceKind::DexHand; } + struct TactilePoint { int32_t fx{0}; int32_t fy{0}; @@ -123,7 +125,6 @@ namespace cmvr::device { }; AbstractDexHand() = default; - explicit AbstractDexHand(const XmlNode& cfg) : AbstractDevice(cfg) {} ~AbstractDexHand() override = default; virtual Status state() const = 0; @@ -148,23 +149,23 @@ namespace cmvr::device { virtual ResultantForce getResultantForce(FingerType finger, TactileRegion region) = 0; virtual void setPositions(const std::vector&) { - throw std::logic_error("setPositions is not supported by this dexhand abstraction."); + CMVR_LOG(ERROR) << "[AbstractDexHand] setPositions is not supported by this dexhand abstraction."; } virtual void setVelocities(const std::vector&) { - throw std::logic_error("setVelocities is not supported by this dexhand abstraction."); + CMVR_LOG(ERROR) << "[AbstractDexHand] setVelocities is not supported by this dexhand abstraction."; } virtual void setPresetAct(int) { - throw std::logic_error("setPresetAct is not supported by this dexhand abstraction."); + CMVR_LOG(ERROR) << "[AbstractDexHand] setPresetAct is not supported by this dexhand abstraction."; } virtual void execPresetAct(int) { - throw std::logic_error("execPresetAct is not supported by this dexhand abstraction."); + CMVR_LOG(ERROR) << "[AbstractDexHand] execPresetAct is not supported by this dexhand abstraction."; } virtual void setForce(const std::vector&) { - throw std::logic_error("setForce is not supported by this dexhand abstraction."); + CMVR_LOG(ERROR) << "[AbstractDexHand] setForce is not supported by this dexhand abstraction."; } }; } diff --git a/cmvr-es/devices/dexhand/dexhand_factory.h b/cmvr-es/devices/dexhand/dexhand_factory.h new file mode 100644 index 00000000..0bba4dfb --- /dev/null +++ b/cmvr-es/devices/dexhand/dexhand_factory.h @@ -0,0 +1,63 @@ +#ifndef CMVR_ES_DEXHAND_FACTORY_H +#define CMVR_ES_DEXHAND_FACTORY_H + +#include +#include +#include + +#include "cmvr/config/dexhand_config/dexhand_config.pb.h" +#include "common/base/logging/logger.h" +#include "devices/dexhand/abstract_dexhand.h" +#include "devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h" +#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h" + +namespace cmvr::device { + +class DexHandFactory { +public: + static std::shared_ptr create(const config::DexHandDeviceConfig& cfg) + { + if (cfg.id().empty()) { + const std::string error = "[DexHandFactory]: DexHand id is empty"; + CMVR_LOG(ERROR) << error; + return nullptr; + } + + switch (cfg.backend_case()) { + case config::DexHandDeviceConfig::kRh56Dftp: + return std::make_shared( + backendWithId_(cfg.id(), cfg.rh56dftp())); + + case config::DexHandDeviceConfig::kPx6AxGen3: + return std::make_shared( + backendWithId_(cfg.id(), cfg.px_6ax_gen3())); + + case config::DexHandDeviceConfig::BACKEND_NOT_SET: + default: + { + const std::string error = + "[DexHandFactory]: DexHand backend is not configured: " + cfg.id(); + CMVR_LOG(ERROR) << error; + return nullptr; + } + } + } + +private: + template + static BackendConfig backendWithId_(const std::string& id, const BackendConfig& source) + { + if (!source.id().empty() && source.id() != id) { + const std::string error = "[DexHandFactory]: DexHand id does not match backend id: " + id; + CMVR_LOG(ERROR) << error; + return BackendConfig{}; + } + BackendConfig backend = source; + backend.set_id(id); + return backend; + } +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_DEXHAND_FACTORY_H diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h b/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h index 5cff0970..163f12c8 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h @@ -38,13 +38,13 @@ namespace cmvr::device { DISTRIBUTED_AND_RESULTANT_FORCE }; - explicit PX6AXGen3(const XmlNode& cfg); explicit PX6AXGen3(const config::PX6AXGen3& cfg); ~PX6AXGen3() override; - void init() override; - void start() override; - void stop() override; + std::string typeName() const override { return "PX6AXGen3"; } + bool init() override; + bool start() override; + bool stop() override; Status state() const override; std::string lastError() const override; @@ -69,7 +69,6 @@ namespace cmvr::device { static PollingReadMode parsePollingReadMode(config::PX6AXGen3PollingReadMode mode); static PollingReadMode parsePollingReadModeName(std::string value); - void parseXmlConfig(); void initializeSnapshot(); void ensureConnected(); void closeConnection(); diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp index 139a5568..d15d4cf6 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by Codex on 2026/4/3. // @@ -58,7 +59,8 @@ namespace { if (value == "PALM") { return FingerType::PALM; } - throw std::invalid_argument("Unsupported tactile finger type: " + value); + CMVR_LOG(ERROR) << "[PX6AXGen3] Unsupported tactile finger type: " << value; + return FingerType::INDEX; } TactileRegion parseTactileRegionName(std::string value) { @@ -81,7 +83,8 @@ namespace { if (value == "PALM_PAD") { return TactileRegion::PALM_PAD; } - throw std::invalid_argument("Unsupported tactile region type: " + value); + CMVR_LOG(ERROR) << "[PX6AXGen3] Unsupported tactile region type: " << value; + return TactileRegion::TIP; } int parseSignedByte(const uint8_t byte) { @@ -127,7 +130,8 @@ namespace { const size_t response_header_bytes, const size_t payload_length) { if (response.size() < frame_offset + response_header_bytes + payload_length) { - throw std::runtime_error("PX-6AX GEN3 response shorter than expected payload window."); + CMVR_LOG(ERROR) << "PX-6AX GEN3 response shorter than expected payload window."; + return {}; } return std::vector( response.begin() + static_cast(frame_offset + response_header_bytes), @@ -195,9 +199,8 @@ namespace { } if (!read_ok && frame_offset == std::string::npos) { - throw std::runtime_error( - "Failed to read " + response_name + ": " + serial.lastError() + - ", raw=" + previewBytesHex(response)); + CMVR_LOG(ERROR) << "Failed to read " << response_name << ": " << serial.lastError() + << ", raw=" << previewBytesHex(response); } return response; @@ -205,7 +208,8 @@ namespace { std::array parseResultantPayload(const std::vector& payload) { if (payload.size() < 3) { - throw std::runtime_error("PX-6AX GEN3 resultant payload must contain at least 3 bytes."); + CMVR_LOG(ERROR) << "PX-6AX GEN3 resultant payload must contain at least 3 bytes."; + return {}; } return { @@ -265,9 +269,9 @@ PX6AXGen3::PollingReadMode PX6AXGen3::parsePollingReadMode(const config::PX6AXGe case config::PX_6AX_GEN3_POLLING_READ_MODE_DISTRIBUTED_AND_RESULTANT_FORCE: return PollingReadMode::DISTRIBUTED_AND_RESULTANT_FORCE; default: - throw std::invalid_argument( - "Unsupported PX6AXGen3 polling_read_mode enum value: " + - std::to_string(static_cast(mode))); + CMVR_LOG(ERROR) << "Unsupported PX6AXGen3 polling_read_mode enum value: " + << static_cast(mode); + return PollingReadMode::DISTRIBUTED_AND_RESULTANT_FORCE; } } @@ -294,14 +298,8 @@ PX6AXGen3::PollingReadMode PX6AXGen3::parsePollingReadModeName(std::string value return PollingReadMode::DISTRIBUTED_AND_RESULTANT_FORCE; } - throw std::invalid_argument("Unsupported PX6AXGen3 polling_read_mode: " + value); -} - -PX6AXGen3::PX6AXGen3(const XmlNode& cfg) - : AbstractDexHand(cfg), - serial_(std::make_unique<::cmvr::PosixSerialTransport>()) { - parseXmlConfig(); - initializeSnapshot(); + CMVR_LOG(ERROR) << "Unsupported PX6AXGen3 polling_read_mode: " << value; + return PollingReadMode::DISTRIBUTED_AND_RESULTANT_FORCE; } PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg) @@ -319,7 +317,8 @@ PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg) } distributed_length_ = config_.distributed_length(); if (distributed_length_ <= 0) { - throw std::invalid_argument("PX6AXGen3 requires config.distributed_length to be explicitly configured."); + enterFault("PX6AXGen3 requires config.distributed_length to be explicitly configured."); + return; } if (config_.resultant_length() > 0) { resultant_length_ = config_.resultant_length(); @@ -357,22 +356,26 @@ PX6AXGen3::~PX6AXGen3() { stop(); } -void PX6AXGen3::init() { +bool PX6AXGen3::init() { try { ensureConnected(); + if (!isOperationalState(state())) { + return false; + } refreshSensorDataWithRetry( 5, std::chrono::milliseconds(std::max(10, response_timeout_ms_ / 2))); + return isOperationalState(state()); } catch (const std::exception& e) { enterFault("[PX6AXGen3](init): " + std::string(e.what())); - throw; + return false; } } -void PX6AXGen3::start() { +bool PX6AXGen3::start() { if (polling_thread_running_.exchange(true, std::memory_order_acq_rel)) { transitionTo(Status::STREAMING); - return; + return true; } try { @@ -381,6 +384,10 @@ void PX6AXGen3::start() { } ensureConnected(); + if (!isOperationalState(state())) { + polling_thread_running_.store(false, std::memory_order_release); + return false; + } if (requested_polling_) { refreshSensorDataWithRetry( 5, @@ -390,14 +397,15 @@ void PX6AXGen3::start() { polling_thread_ = std::thread(&PX6AXGen3::pollingLoop, this); transitionTo(Status::STREAMING); polling_cv_.notify_all(); + return true; } catch (const std::exception& e) { polling_thread_running_.store(false, std::memory_order_release); enterFault("[PX6AXGen3](start): " + std::string(e.what())); - throw; + return false; } } -void PX6AXGen3::stop() { +bool PX6AXGen3::stop() { polling_thread_running_.store(false, std::memory_order_release); polling_cv_.notify_all(); @@ -410,6 +418,7 @@ void PX6AXGen3::stop() { if (state() != Status::FAULT) { transitionTo(Status::STOPPED); } + return true; } PX6AXGen3::Status PX6AXGen3::state() const { @@ -442,13 +451,14 @@ void PX6AXGen3::getState(DexHandState& state_out) { } void PX6AXGen3::setAngles(const std::vector&) { - throw std::logic_error("PX6AXGen3 is a tactile sensor only and does not support setAngles."); + CMVR_LOG(ERROR) << "PX6AXGen3 is a tactile sensor only and does not support setAngles."; } void PX6AXGen3::setTactilePollingRegions(const std::vector& regions) { for (const auto& region : regions) { if (!isSupportedRegion(region.first, region.second)) { - throw std::invalid_argument("PX6AXGen3 only exposes INDEX/TIP tactile data."); + CMVR_LOG(ERROR) << "PX6AXGen3 only exposes INDEX/TIP tactile data."; + return; } } @@ -472,7 +482,8 @@ std::vector PX6AXGen3::getSensorData() { TactileRegionData PX6AXGen3::getSensorData(FingerType finger, TactileRegion region) { if (!isSupportedRegion(finger, region)) { - throw std::invalid_argument("PX6AXGen3 only supports INDEX/TIP tactile data."); + CMVR_LOG(ERROR) << "PX6AXGen3 only supports INDEX/TIP tactile data."; + return {}; } ensureSensorReady(true, true, false); @@ -481,14 +492,16 @@ TactileRegionData PX6AXGen3::getSensorData(FingerType finger, TactileRegion regi PX6AXGen3::ResultantForce PX6AXGen3::getResultantForce(FingerType finger, TactileRegion region) { if (!isSupportedRegion(finger, region)) { - throw std::invalid_argument("PX6AXGen3 only supports INDEX/TIP tactile data."); + CMVR_LOG(ERROR) << "PX6AXGen3 only supports INDEX/TIP tactile data."; + return {}; } ensureSensorReady(true, false, true); std::lock_guard lock(snapshot_mutex_); if (!latest_snapshot_.resultant_valid) { - throw std::runtime_error("PX6AXGen3 resultant-force snapshot is not ready."); + CMVR_LOG(ERROR) << "PX6AXGen3 resultant-force snapshot is not ready."; + return {}; } return ResultantForce{ @@ -498,31 +511,6 @@ PX6AXGen3::ResultantForce PX6AXGen3::getResultantForce(FingerType finger, Tactil }; } -void PX6AXGen3::parseXmlConfig() { - id_ = cfg_.getAttrString("id"); - port_name_ = cfg_.getAttrString("serial"); - sensor_model_ = cfg_.getAttrDefault("sensor_model", sensor_model_); - sensor_name_ = cfg_.getAttrDefault("sensor_name", sensor_name_); - module_id_ = std::max(0, cfg_.getAttrDefault("module_id", module_id_)); - device_address_ = module_id_ + 1; - baud_rate_ = cfg_.getAttrDefault("baud_rate", baud_rate_); - distributed_length_ = cfg_.getAttrDefault("distributed_length", 0); - if (distributed_length_ <= 0) { - throw std::invalid_argument("PX6AXGen3 requires XML attribute distributed_length to be explicitly configured."); - } - resultant_length_ = cfg_.getAttrDefault("resultant_length", resultant_length_); - response_header_bytes_ = cfg_.getAttrDefault("response_header_bytes", response_header_bytes_); - tactile_rows_ = cfg_.getAttrDefault("tactile_rows", tactile_rows_); - tactile_cols_ = cfg_.getAttrDefault("tactile_cols", tactile_cols_); - response_timeout_ms_ = cfg_.getAttrDefault("response_timeout_ms", response_timeout_ms_); - tactile_finger_ = parseFingerTypeName(cfg_.getAttrDefault("tactile_finger", "INDEX")); - tactile_region_ = parseTactileRegionName(cfg_.getAttrDefault("tactile_region", "TIP")); - polling_read_mode_ = parsePollingReadModeName( - cfg_.getAttrDefault("polling_read_mode", "DISTRIBUTED_AND_RESULTANT_FORCE")); - auto_calibrate_ = cfg_.getAttrDefault("auto_calibrate", false); - poll_interval_ = std::chrono::milliseconds(std::max(1, cfg_.getAttrDefault("poll_interval_ms", static_cast(poll_interval_.count())))); -} - void PX6AXGen3::initializeSnapshot() { std::lock_guard lock(snapshot_mutex_); latest_snapshot_ = SensorSnapshot{}; @@ -530,7 +518,8 @@ void PX6AXGen3::initializeSnapshot() { void PX6AXGen3::ensureConnected() { if (port_name_.empty()) { - throw std::runtime_error("PX6AXGen3 serial port is not configured."); + enterFault("PX6AXGen3 serial port is not configured."); + return; } if (!serial_) { @@ -539,7 +528,8 @@ void PX6AXGen3::ensureConnected() { if (!serial_->isOpen()) { if (!serial_->open(::cmvr::AbstractSerialTransport::Config{port_name_, baud_rate_})) { - throw std::runtime_error("Failed to open PX-6AX GEN3 serial transport: " + serial_->lastError()); + enterFault("Failed to open PX-6AX GEN3 serial transport: " + serial_->lastError()); + return; } calibration_performed_ = false; } @@ -553,7 +543,7 @@ void PX6AXGen3::ensureConnected() { void PX6AXGen3::closeConnection() { if (serial_) { if (!serial_->close()) { - LOG(WARNING) << "[PX6AXGen3] Failed to close serial transport: " << serial_->lastError(); + CMVR_LOG(WARNING) << "[PX6AXGen3] Failed to close serial transport: " << serial_->lastError(); } } calibration_performed_ = false; @@ -566,10 +556,12 @@ void PX6AXGen3::calibrateIfRequested() { const auto frame = buildCommandFrame(CommandType::CALIBRATION, device_address_, distributed_length_); if (!serial_->flushInput()) { - throw std::runtime_error("Failed to flush serial input before calibration: " + serial_->lastError()); + enterFault("Failed to flush serial input before calibration: " + serial_->lastError()); + return; } if (!serial_->write(frame)) { - throw std::runtime_error("Failed to send calibration command: " + serial_->lastError()); + enterFault("Failed to send calibration command: " + serial_->lastError()); + return; } const auto response = readFramedResponse( *serial_, @@ -578,9 +570,9 @@ void PX6AXGen3::calibrateIfRequested() { std::chrono::milliseconds(response_timeout_ms_), "calibration response"); if (findResponseFrameOffset(response, 2U) == std::string::npos) { - throw std::runtime_error( - "PX-6AX GEN3 calibration command did not receive a valid acknowledgment. raw=" + - previewBytesHex(response)); + enterFault("PX-6AX GEN3 calibration command did not receive a valid acknowledgment. raw=" + + previewBytesHex(response)); + return; } calibration_performed_ = true; } @@ -592,7 +584,8 @@ void PX6AXGen3::refreshSensorData() { void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_resultant) { if (!read_distributed && !read_resultant) { - throw std::invalid_argument("PX6AXGen3 refreshSensorData requires at least one data type to read."); + CMVR_LOG(ERROR) << "PX6AXGen3 refreshSensorData requires at least one data type to read."; + return; } std::lock_guard refresh_lock(refresh_mutex_); @@ -600,6 +593,9 @@ void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_r try { ensureConnected(); + if (!isOperationalState(state())) { + return; + } std::vector tactile_points; int rows = 0; @@ -609,10 +605,12 @@ void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_r if (read_distributed) { const auto distributed_frame = buildCommandFrame(CommandType::DISTRIBUTED_FORCE, device_address_, distributed_length_); if (!serial_->flushInput()) { - throw std::runtime_error("Failed to flush serial input before distributed-force read: " + serial_->lastError()); + handleRefreshFailure("Failed to flush serial input before distributed-force read: " + serial_->lastError(), had_valid_snapshot); + return; } if (!serial_->write(distributed_frame)) { - throw std::runtime_error("Failed to send distributed-force command: " + serial_->lastError()); + handleRefreshFailure("Failed to send distributed-force command: " + serial_->lastError(), had_valid_snapshot); + return; } const size_t distributed_frame_bytes = static_cast(response_header_bytes_) + static_cast(distributed_length_); @@ -625,8 +623,8 @@ void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_r const auto distributed_frame_offset = findResponseFrameOffset(distributed_response, distributed_frame_bytes); if (distributed_frame_offset == std::string::npos) { - throw std::runtime_error( - "Invalid distributed tactile response header. raw=" + previewBytesHex(distributed_response)); + handleRefreshFailure("Invalid distributed tactile response header. raw=" + previewBytesHex(distributed_response), had_valid_snapshot); + return; } tactile_points = parseDistributedPayload(extractPayload( @@ -644,10 +642,12 @@ void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_r try { const auto resultant_frame = buildCommandFrame(CommandType::RESULTANT_FORCE, device_address_, distributed_length_); if (!serial_->flushInput()) { - throw std::runtime_error("Failed to flush serial input before resultant-force read: " + serial_->lastError()); + handleRefreshFailure("Failed to flush serial input before resultant-force read: " + serial_->lastError(), had_valid_snapshot); + return; } if (!serial_->write(resultant_frame)) { - throw std::runtime_error("Failed to send resultant-force command: " + serial_->lastError()); + handleRefreshFailure("Failed to send resultant-force command: " + serial_->lastError(), had_valid_snapshot); + return; } const size_t resultant_frame_bytes = static_cast(response_header_bytes_) + static_cast(resultant_length_); @@ -667,14 +667,15 @@ void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_r static_cast(resultant_length_))); resultant_valid = true; } else { - throw std::runtime_error( - "Invalid resultant-force response header. raw=" + previewBytesHex(resultant_response)); + handleRefreshFailure("Invalid resultant-force response header. raw=" + previewBytesHex(resultant_response), had_valid_snapshot); + return; } } catch (const std::exception& e) { if (!read_distributed) { - throw; + handleRefreshFailure("[PX6AXGen3](refreshSensorData): " + std::string(e.what()), had_valid_snapshot); + return; } - LOG(WARNING) << "[PX6AXGen3] Failed to refresh resultant force: " << e.what(); + CMVR_LOG(WARNING) << "[PX6AXGen3] Failed to refresh resultant force: " << e.what(); } } @@ -693,7 +694,7 @@ void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_r clearOperationalError(); } catch (const std::exception& e) { handleRefreshFailure("[PX6AXGen3](refreshSensorData): " + std::string(e.what()), had_valid_snapshot); - throw; + return; } } @@ -714,7 +715,7 @@ void PX6AXGen3::refreshSensorDataWithRetry(const int max_attempts, } } - throw std::runtime_error(last_error.empty() ? "PX6AXGen3 refresh retries exhausted." : last_error); + handleRefreshFailure(last_error.empty() ? "PX6AXGen3 refresh retries exhausted." : last_error, isSnapshotReady(true, true)); } void PX6AXGen3::pollingLoop() { @@ -733,7 +734,7 @@ void PX6AXGen3::pollingLoop() { try { refreshSensorData(); } catch (const std::exception& e) { - LOG(ERROR) << e.what(); + CMVR_LOG(ERROR) << e.what(); } lock.lock(); @@ -785,7 +786,8 @@ TactileRegionData PX6AXGen3::buildSupportedRegionSnapshot() const { { std::lock_guard lock(snapshot_mutex_); if (!latest_snapshot_.tactile_valid) { - throw std::runtime_error("PX6AXGen3 tactile snapshot is not ready."); + CMVR_LOG(ERROR) << "PX6AXGen3 tactile snapshot is not ready."; + return {}; } *snapshot = latest_snapshot_.tactile_points; rows = latest_snapshot_.rows; @@ -810,7 +812,8 @@ std::pair PX6AXGen3::resolvePollingReadSelection() const { return {true, true}; } - throw std::logic_error("Unsupported PX6AXGen3 polling read mode."); + CMVR_LOG(ERROR) << "Unsupported PX6AXGen3 polling read mode."; + return {true, true}; } void PX6AXGen3::clearOperationalError() { @@ -830,7 +833,7 @@ void PX6AXGen3::handleRefreshFailure(const std::string& error, const bool had_va last_error_ = error; } } - LOG(WARNING) << error; + CMVR_LOG(WARNING) << error; return; } @@ -856,7 +859,7 @@ void PX6AXGen3::enterFault(const std::string& error) { polling_cv_.notify_all(); closeConnection(); - LOG(ERROR) << error; + CMVR_LOG(ERROR) << error; } bool PX6AXGen3::isOperationalState(const Status lifecycle) const { diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp index 15675d05..bf0b57b3 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp @@ -2,7 +2,7 @@ #include "../include/px_6ax_gen3.h" #include "cmvr/config/dexhand_config/dexhand_config.pb.h" -#include "common/utils/config_helper/include/config_helper.h" +#include "common/config/config_files.h" #include #include @@ -67,19 +67,6 @@ struct StopGuard { } }; -const cmvr::config::PX6AXGen3* findTestConfig(const cmvr::config::DexHandConfig& dexhand_config) { - const cmvr::config::PX6AXGen3* candidate = nullptr; - for (const auto& config : dexhand_config.px_6ax_gen3()) { - if (config.enable() && !config.serial_port().empty()) { - return &config; - } - if (candidate == nullptr && !config.serial_port().empty()) { - candidate = &config; - } - } - return candidate; -} - DexHand::ResultantForce readFirstValidResultantForce(PX6AXGen3& hand, const DexHand::FingerType finger, const DexHand::TactileRegion region, @@ -111,13 +98,19 @@ DexHand::ResultantForce readStateResultantForce(PX6AXGen3& hand) { } // namespace TEST(PX6AXGen3Test, PrintResultantForceOnly) { - cmvr::config::DexHandConfig dexhand_config; - ASSERT_TRUE(cmvr::ConfigHelper::getDexHandsConfig(dexhand_config)); - - const cmvr::config::PX6AXGen3* hand_config = findTestConfig(dexhand_config); - ASSERT_NE(hand_config, nullptr); - - const auto& test_config = *hand_config; + cmvr::config::DexHandRootConfig root_config; + ASSERT_TRUE(cmvr::ConfigHelper::loadConfigFile("devices/dexhand/dexhand.pb.txt", root_config)); + const cmvr::config::DexHandDeviceConfig* device_config_ptr = nullptr; + for (const auto& cfg : root_config.dexhand().dexhands()) { + if (cfg.id() == "paxini_tip_1" && cfg.has_px_6ax_gen3()) { + device_config_ptr = &cfg; + break; + } + } + ASSERT_NE(device_config_ptr, nullptr); + auto test_config = device_config_ptr->px_6ax_gen3(); + test_config.set_id(device_config_ptr->id()); + ASSERT_FALSE(test_config.serial_port().empty()); auto hand = std::make_shared(test_config); ASSERT_NO_THROW(hand->init()); diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt b/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt index 1a235638..8d2c82bc 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt @@ -4,7 +4,7 @@ target_include_directories(rh56dftp_dexhand PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) add_library(cmvr_es::device::rh56dftp_dexhand ALIAS rh56dftp_dexhand) -target_link_libraries(rh56dftp_dexhand PRIVATE cmvr_es::hardware -lmodbus) +target_link_libraries(rh56dftp_dexhand PRIVATE cmvr_es::hardware cmvr_es::proto -lmodbus) install(TARGETS rh56dftp_dexhand LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h index c6556a7e..388790f8 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h @@ -30,12 +30,12 @@ namespace cmvr::device { ModbusController() = default; ~ModbusController(); - void open(const std::string& ip, int port); + bool open(const std::string& ip, int port); void close(); bool isOpen() const; - void writeRegisters(int address, const uint16_t* values, int count); - void readRegisterBlock(int start_address, int count, std::vector& values); + bool writeRegisters(int address, const uint16_t* values, int count); + bool readRegisterBlock(int start_address, int count, std::vector& values); private: void closeUnlocked(); @@ -57,13 +57,13 @@ namespace cmvr::device { static constexpr size_t TACTILE_REGION_SLOT_COUNT = RH56TactileBuffer::REGION_COUNT; using RegionMask = std::bitset; - explicit RH56DFTPDexhand(const XmlNode& cfg); explicit RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg); ~RH56DFTPDexhand() override; - void init() override; - void start() override; - void stop() override; + std::string typeName() const override { return "RH56DFTPDexhand"; } + bool init() override; + bool start() override; + bool stop() override; Status state() const override; std::string lastError() const override; @@ -81,9 +81,8 @@ namespace cmvr::device { const RH56TactileBuffer* buffer{nullptr}; }; - void parseXmlConfig(); void initializeTactileBuffers(); - void ensureConnected(); + bool ensureConnected(); void refreshTactileData(const RegionMask& mask); void tactilePollingLoop(); diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h index 1eebd2d4..412951bb 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h @@ -3,6 +3,8 @@ #pragma once #include + +#include "common/base/logging/logger.h" #include #include #include @@ -130,7 +132,8 @@ namespace cmvr::device { return index; } } - throw std::invalid_argument("Invalid tactile region for selected finger."); + CMVR_LOG(ERROR) << "Invalid tactile region for selected finger."; + return 0; } std::array values_{}; diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp index 24010134..44737981 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by linbo on 2025/6/20. // @@ -172,7 +173,8 @@ namespace { for (const auto& selection : selections) { const int index = findRegionConfigIndex(selection.first, selection.second); if (index < 0) { - throw std::invalid_argument("Invalid tactile region selection."); + CMVR_LOG(ERROR) << "Invalid tactile region selection."; + return {}; } mask.set(static_cast(index)); } @@ -183,7 +185,8 @@ namespace { RegionMask mask; const int index = findRegionConfigIndex(finger, region); if (index < 0) { - throw std::invalid_argument("Invalid tactile region for selected finger."); + CMVR_LOG(ERROR) << "Invalid tactile region for selected finger."; + return {}; } mask.set(static_cast(index)); return mask; @@ -220,7 +223,8 @@ namespace { std::array encodeAngleCommand( const std::vector& finger_joint_angles) { if (finger_joint_angles.size() != RH56DFTPDexhand::ANGLE_COMMAND_COUNT) { - throw std::invalid_argument("RH56DFTPDexhand expects exactly 6 joint angles."); + CMVR_LOG(ERROR) << "RH56DFTPDexhand expects exactly 6 joint angles."; + return {}; } std::array registers{}; @@ -236,20 +240,23 @@ ModbusController::~ModbusController() { close(); } -void ModbusController::open(const std::string& ip, const int port) { +bool ModbusController::open(const std::string& ip, const int port) { std::lock_guard lock(io_mutex_); closeUnlocked(); ctx_ = modbus_new_tcp(ip.c_str(), port); if (ctx_ == nullptr) { - throw std::runtime_error("Failed to create Modbus TCP context."); + CMVR_LOG(ERROR) << "Failed to create Modbus TCP context."; + return false; } if (modbus_connect(ctx_) == -1) { const std::string error = modbus_strerror(errno); closeUnlocked(); - throw std::runtime_error("Failed to connect Modbus TCP: " + error); + CMVR_LOG(ERROR) << "Failed to connect Modbus TCP: " << error; + return false; } + return true; } void ModbusController::close() { @@ -262,30 +269,33 @@ bool ModbusController::isOpen() const { return ctx_ != nullptr; } -void ModbusController::writeRegisters(const int address, const uint16_t* values, const int count) { +bool ModbusController::writeRegisters(const int address, const uint16_t* values, const int count) { std::lock_guard lock(io_mutex_); if (ctx_ == nullptr) { - throw std::runtime_error("Modbus connection is not open."); + CMVR_LOG(ERROR) << "Modbus connection is not open."; + return false; } const int rc = modbus_write_registers(ctx_, address, count, values); if (rc == -1) { - throw std::runtime_error( - "Failed to write register block at " + std::to_string(address) + ": " + modbus_strerror(errno)); + CMVR_LOG(ERROR) << "Failed to write register block at " << address << ": " << modbus_strerror(errno); + return false; } if (rc != count) { - throw std::runtime_error( - "Incomplete register write at " + std::to_string(address) + ", expected " + std::to_string(count) + - ", got " + std::to_string(rc)); + CMVR_LOG(ERROR) << "Incomplete register write at " << address << ", expected " << count + << ", got " << rc; + return false; } + return true; } -void ModbusController::readRegisterBlock(const int start_address, const int count, std::vector& values) { +bool ModbusController::readRegisterBlock(const int start_address, const int count, std::vector& values) { values.assign(static_cast(count), 0); std::lock_guard lock(io_mutex_); if (ctx_ == nullptr) { - throw std::runtime_error("Modbus connection is not open."); + CMVR_LOG(ERROR) << "Modbus connection is not open."; + return false; } for (int offset = 0; offset < count; offset += kMaxRegistersPerRead) { @@ -293,16 +303,17 @@ void ModbusController::readRegisterBlock(const int start_address, const int coun const int current_address = start_address + offset * kRegisterByteWidth; const int rc = modbus_read_registers(ctx_, current_address, current_count, values.data() + offset); if (rc == -1) { - throw std::runtime_error( - "Failed to read register block at " + std::to_string(current_address) + ": " + - modbus_strerror(errno)); + CMVR_LOG(ERROR) << "Failed to read register block at " << current_address << ": " + << modbus_strerror(errno); + return false; } if (rc != current_count) { - throw std::runtime_error( - "Incomplete register read at " + std::to_string(current_address) + ", expected " + - std::to_string(current_count) + ", got " + std::to_string(rc)); + CMVR_LOG(ERROR) << "Incomplete register read at " << current_address << ", expected " + << current_count << ", got " << rc; + return false; } } + return true; } void ModbusController::closeUnlocked() { @@ -313,13 +324,6 @@ void ModbusController::closeUnlocked() { } } -RH56DFTPDexhand::RH56DFTPDexhand(const XmlNode& cfg) - : AbstractDexHand(cfg), - controller_(std::make_unique()) { - parseXmlConfig(); - initializeTactileBuffers(); -} - RH56DFTPDexhand::RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg) : controller_(std::make_unique()), dexhandCfg_(cfg) { @@ -338,19 +342,19 @@ RH56DFTPDexhand::~RH56DFTPDexhand() { stop(); } -void RH56DFTPDexhand::init() { +bool RH56DFTPDexhand::init() { try { - ensureConnected(); + return ensureConnected(); } catch (const std::exception& e) { enterFault("[RH56DFTPDexhand](init): " + std::string(e.what())); - throw; + return false; } } -void RH56DFTPDexhand::start() { +bool RH56DFTPDexhand::start() { if (tactile_thread_running_.exchange(true, std::memory_order_acq_rel)) { transitionTo(Status::STREAMING); - return; + return true; } try { @@ -358,7 +362,9 @@ void RH56DFTPDexhand::start() { tactile_thread_.join(); } - ensureConnected(); + if (!ensureConnected()) { + return false; + } RegionMask initial_mask; { @@ -372,14 +378,15 @@ void RH56DFTPDexhand::start() { tactile_thread_ = std::thread(&RH56DFTPDexhand::tactilePollingLoop, this); transitionTo(Status::STREAMING); polling_cv_.notify_all(); + return true; } catch (const std::exception& e) { tactile_thread_running_.store(false, std::memory_order_release); enterFault("[RH56DFTPDexhand](start): " + std::string(e.what())); - throw; + return false; } } -void RH56DFTPDexhand::stop() { +bool RH56DFTPDexhand::stop() { tactile_thread_running_.store(false, std::memory_order_release); polling_cv_.notify_all(); @@ -394,6 +401,7 @@ void RH56DFTPDexhand::stop() { if (state() != Status::FAULT) { transitionTo(Status::STOPPED); } + return true; } RH56DFTPDexhand::Status RH56DFTPDexhand::state() const { @@ -429,11 +437,16 @@ void RH56DFTPDexhand::setAngles(const std::vector& finger_joint_angles) { const auto registers = encodeAngleCommand(finger_joint_angles); try { - ensureConnected(); - controller_->writeRegisters( - kAngleSetByteAddress, - registers.data(), - static_cast(registers.size())); + if (!ensureConnected()) { + return; + } + if (!controller_->writeRegisters( + kAngleSetByteAddress, + registers.data(), + static_cast(registers.size()))) { + enterFault("[RH56DFTPDexhand](setAngles): writeRegisters failed"); + return; + } std::lock_guard lock(command_mutex_); for (size_t i = 0; i < finger_joint_angles.size(); ++i) { @@ -441,7 +454,7 @@ void RH56DFTPDexhand::setAngles(const std::vector& finger_joint_angles) { } } catch (const std::exception& e) { enterFault("[RH56DFTPDexhand](setAngles): " + std::string(e.what())); - throw; + return; } } @@ -471,7 +484,8 @@ TactileRegionData RH56DFTPDexhand::getSensorData(FingerType finger, TactileRegio RH56DFTPDexhand::ResultantForce RH56DFTPDexhand::getResultantForce(FingerType finger, TactileRegion region) { const auto region_data = getSensorData(finger, region); if (!region_data.valid()) { - throw std::runtime_error("RH56DFTPDexhand tactile region data is not ready."); + CMVR_LOG(ERROR) << "RH56DFTPDexhand tactile region data is not ready."; + return {}; } ResultantForce resultant{}; @@ -486,14 +500,6 @@ RH56DFTPDexhand::ResultantForce RH56DFTPDexhand::getResultantForce(FingerType fi return resultant; } -void RH56DFTPDexhand::parseXmlConfig() { - id_ = cfg_.getAttrString("id"); - ip_address_ = cfg_.getAttrString("ip_address"); - port_ = cfg_.getAttrDefault("port", kDefaultPort); - tactile_poll_interval_ = std::chrono::milliseconds( - std::max(1, cfg_.getAttrDefault("poll_interval_ms", static_cast(tactile_poll_interval_.count())))); -} - void RH56DFTPDexhand::initializeTactileBuffers() { for (auto& tactile_buffer : tactile_buffers_) { tactile_buffer.clear(); @@ -503,9 +509,10 @@ void RH56DFTPDexhand::initializeTactileBuffers() { active_buffer_index_.store(0, std::memory_order_release); } -void RH56DFTPDexhand::ensureConnected() { +bool RH56DFTPDexhand::ensureConnected() { if (ip_address_.empty()) { - throw std::runtime_error("Dexhand IP address is not configured."); + enterFault("Dexhand IP address is not configured."); + return false; } if (!controller_) { @@ -516,11 +523,15 @@ void RH56DFTPDexhand::ensureConnected() { if (!isOperationalState(state())) { transitionTo(Status::INITIALIZED); } - return; + return true; } - controller_->open(ip_address_, port_); + if (!controller_->open(ip_address_, port_)) { + enterFault("Failed to connect Modbus TCP: " + ip_address_); + return false; + } transitionTo(Status::INITIALIZED); + return true; } void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) { @@ -529,7 +540,12 @@ void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) { } try { - ensureConnected(); + if (!ensureConnected()) { + return; + } + if (!isOperationalState(state())) { + return; + } const auto read_plan = buildReadPlan(mask); std::lock_guard refresh_lock(tactile_refresh_mutex_); @@ -541,7 +557,10 @@ void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) { std::vector segment_values; for (const auto& segment : read_plan) { - controller_->readRegisterBlock(segment.start_address, segment.register_count, segment_values); + if (!controller_->readRegisterBlock(segment.start_address, segment.register_count, segment_values)) { + enterFault("[RH56DFTPDexhand](refreshTactileData): readRegisterBlock failed"); + return; + } for (const size_t region_index : segment.region_indexes) { const auto& config = kTactileRegionConfigs[region_index]; const int offset = toRegisterCount(config.start_address - segment.start_address); @@ -559,7 +578,7 @@ void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) { active_buffer_index_.store(write_index, std::memory_order_release); } catch (const std::exception& e) { enterFault("[RH56DFTPDexhand](refreshTactileData): " + std::string(e.what())); - throw; + return; } } @@ -580,7 +599,7 @@ void RH56DFTPDexhand::tactilePollingLoop() { try { refreshTactileData(mask); } catch (const std::exception& e) { - LOG(ERROR) << e.what(); + CMVR_LOG(ERROR) << e.what(); } lock.lock(); @@ -682,7 +701,7 @@ void RH56DFTPDexhand::enterFault(const std::string& error) { controller_->close(); } - LOG(ERROR) << error; + CMVR_LOG(ERROR) << error; } bool RH56DFTPDexhand::isOperationalState(const Status lifecycle) const { diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp index 1cea87b7..3469e22f 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp @@ -2,7 +2,7 @@ #include "../include/rh56dftp_dexhand.h" #include "cmvr/config/dexhand_config/dexhand_config.pb.h" -#include "common/utils/config_helper/include/config_helper.h" +#include "common/config/config_files.h" #include #include @@ -45,21 +45,19 @@ struct StopGuard { } // namespace TEST(RH56DFTPDexhandLatencyTest, ReadConfiguredRegionAndMeasureLatency) { - cmvr::config::DexHandConfig dexhand_config; - ASSERT_TRUE(cmvr::ConfigHelper::getDexHandsConfig(dexhand_config)); - - const cmvr::config::RH56DFTPDexHandConfig* hand_config = nullptr; - for (const auto& config : dexhand_config.rh56dftp_dexhands()) { - if (config.enable() && !config.ip().empty()) { - hand_config = &config; + cmvr::config::DexHandRootConfig root_config; + ASSERT_TRUE(cmvr::ConfigHelper::loadConfigFile("devices/dexhand/dexhand.pb.txt", root_config)); + const cmvr::config::DexHandDeviceConfig* device_config_ptr = nullptr; + for (const auto& cfg : root_config.dexhand().dexhands()) { + if (cfg.id() == "hand2" && cfg.has_rh56dftp()) { + device_config_ptr = &cfg; break; } - if (hand_config == nullptr && !config.ip().empty()) { - hand_config = &config; - } } - - ASSERT_NE(hand_config, nullptr); + ASSERT_NE(device_config_ptr, nullptr); + auto hand_config = device_config_ptr->rh56dftp(); + hand_config.set_id(device_config_ptr->id()); + ASSERT_FALSE(hand_config.ip().empty()); const auto finger = DexHand::FingerType::RING; const auto region = DexHand::TactileRegion::TIP; @@ -67,7 +65,7 @@ TEST(RH56DFTPDexhandLatencyTest, ReadConfiguredRegionAndMeasureLatency) { const int warmup_ms = 200; const int read_interval_ms = 10; - auto hand = std::make_shared(*hand_config); + auto hand = std::make_shared(hand_config); ASSERT_NO_THROW(hand->init()); ASSERT_NO_THROW(hand->start()); StopGuard stop_guard{hand}; @@ -120,9 +118,9 @@ TEST(RH56DFTPDexhandLatencyTest, ReadConfiguredRegionAndMeasureLatency) { } std::cout << std::fixed << std::setprecision(3) - << "[RH56DFTPDexhandLatencyTest] id=" << hand_config->id() - << " ip=" << hand_config->ip() - << " port=" << hand_config->port() + << "[RH56DFTPDexhandLatencyTest] id=" << hand_config.id() + << " ip=" << hand_config.ip() + << " port=" << hand_config.port() << " finger=INDEX" << " region=" << tactileRegionToString(region) << " sensor=" << sensor_name diff --git a/cmvr-es/devices/gripper/abstract_gripper.h b/cmvr-es/devices/gripper/abstract_gripper.h index c8b4d3c2..728d094b 100644 --- a/cmvr-es/devices/gripper/abstract_gripper.h +++ b/cmvr-es/devices/gripper/abstract_gripper.h @@ -12,9 +12,10 @@ namespace cmvr::device{ class AbstractGripper: public AbstractDevice { public: - explicit AbstractGripper(const XmlNode &cfg): AbstractDevice(cfg) {} + AbstractGripper() = default; ~AbstractGripper() override = default; + DeviceKind kind() const noexcept override { return DeviceKind::Gripper; } virtual void getState(GripperState &state) {} virtual void openGripper(float vel) {} @@ -27,4 +28,4 @@ namespace cmvr::device{ }; } -#endif //CMVR_ES_ABSTRACT_GRIPPER_H \ No newline at end of file +#endif //CMVR_ES_ABSTRACT_GRIPPER_H diff --git a/cmvr-es/devices/microphone/CMakeLists.txt b/cmvr-es/devices/microphone/CMakeLists.txt index 431a864a..1312054b 100644 --- a/cmvr-es/devices/microphone/CMakeLists.txt +++ b/cmvr-es/devices/microphone/CMakeLists.txt @@ -1 +1,13 @@ -add_subdirectory(ffmpeg_microphone) \ No newline at end of file +add_subdirectory(ffmpeg_microphone) + +add_library(microphone INTERFACE) + +target_include_directories(microphone INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(microphone + INTERFACE + cmvr_es::device::ffmpeg_microphone + cmvr_es::proto +) + +add_library(cmvr_es::device::microphone ALIAS microphone) diff --git a/cmvr-es/devices/microphone/abstract_microphone.h b/cmvr-es/devices/microphone/abstract_microphone.h index fbdca1af..87ebcc45 100644 --- a/cmvr-es/devices/microphone/abstract_microphone.h +++ b/cmvr-es/devices/microphone/abstract_microphone.h @@ -12,9 +12,9 @@ namespace cmvr::device{ class AbstractMicrophone: public AbstractDevice { public: AbstractMicrophone() = default; - explicit AbstractMicrophone(const XmlNode &config): AbstractDevice(config) {} ~AbstractMicrophone() override = default; + DeviceKind kind() const noexcept override { return DeviceKind::Microphone; } virtual void getState(MicrophoneState &state) {} virtual void startRecording(const std::string& outputFilePath) {} virtual void stopRecording() {} @@ -28,4 +28,4 @@ namespace cmvr::device{ }; } -#endif //CMVR_ES_ABSTRACT_MICROPHONE_H \ No newline at end of file +#endif //CMVR_ES_ABSTRACT_MICROPHONE_H diff --git a/cmvr-es/devices/microphone/ffmpeg_microphone/CMakeLists.txt b/cmvr-es/devices/microphone/ffmpeg_microphone/CMakeLists.txt index 87825571..4185d31a 100644 --- a/cmvr-es/devices/microphone/ffmpeg_microphone/CMakeLists.txt +++ b/cmvr-es/devices/microphone/ffmpeg_microphone/CMakeLists.txt @@ -7,6 +7,7 @@ add_library(cmvr_es::device::ffmpeg_microphone ALIAS ffmpeg_microphone) target_link_libraries(ffmpeg_microphone PRIVATE avfilter avdevice + cmvr_es::proto ) -install(TARGETS ffmpeg_microphone LIBRARY DESTINATION lib) \ No newline at end of file +install(TARGETS ffmpeg_microphone LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/microphone/ffmpeg_microphone/include/ffmpeg_microphone.h b/cmvr-es/devices/microphone/ffmpeg_microphone/include/ffmpeg_microphone.h index 1898d138..6fc65308 100644 --- a/cmvr-es/devices/microphone/ffmpeg_microphone/include/ffmpeg_microphone.h +++ b/cmvr-es/devices/microphone/ffmpeg_microphone/include/ffmpeg_microphone.h @@ -9,7 +9,6 @@ #include #include "microphone/abstract_microphone.h" #include -#include "utils/base/include/os.h" #include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h" namespace cmvr::device { @@ -21,15 +20,14 @@ namespace cmvr::device { STOPPED, RECORDING, PAUSED - }; + }; public: - explicit ffmpegMicroPhone(const XmlNode& cfg); ffmpegMicroPhone(const config::FFMpegMicroPhoneConfig& cfg); ~ffmpegMicroPhone() override; - void init() override; - void start() override; - void stop() override; - void updateParams(const std::pair& param) override; + std::string typeName() const override { return "FFMpegMicroPhone"; } + bool init() override; + bool start() override; + bool stop() override; void getState(MicrophoneState &state) override; void startRecording(const std::string& outputFilePath) override; void stopRecording() override; @@ -77,7 +75,6 @@ namespace cmvr::device { int64_t next_pts; private: - std::string id_; std::string input_device_; std::shared_ptr audio_thread_; @@ -88,4 +85,4 @@ namespace cmvr::device { } -#endif // CMVR_ES_FFMPEG_MICROPHONE_H \ No newline at end of file +#endif // CMVR_ES_FFMPEG_MICROPHONE_H diff --git a/cmvr-es/devices/microphone/ffmpeg_microphone/src/ffmpeg_microphone.cpp b/cmvr-es/devices/microphone/ffmpeg_microphone/src/ffmpeg_microphone.cpp index d6aa1370..82dac71d 100644 --- a/cmvr-es/devices/microphone/ffmpeg_microphone/src/ffmpeg_microphone.cpp +++ b/cmvr-es/devices/microphone/ffmpeg_microphone/src/ffmpeg_microphone.cpp @@ -1,30 +1,8 @@ +#include "common/base/logging/logger.h" #include "../include/ffmpeg_microphone.h" using namespace std; using namespace cmvr::device; -ffmpegMicroPhone::ffmpegMicroPhone(const XmlNode& cfg) : AbstractMicrophone(cfg), - input_fmt_ctx(nullptr), input_codec_ctx(nullptr), input_frame(nullptr), - output_fmt_ctx(nullptr), output_fmt(nullptr), audio_st(nullptr), - audio_codec_ctx(nullptr), audio_codec(nullptr), swr_ctx(nullptr), - audio_frame(nullptr), is_recording(false), is_paused(false), - sample_rate_(44100), channels_(2) -{ - try { - id_ = cfg_.getAttrString("id"); - input_device_ = cfg_.getAttrString("alsa"); - channels_ = std::clamp(cfg_.getAttrDefault("channels", 2), 1, 8); - sample_rate_ = cfg_.getAttrDefault("sampleRate", 44100); - state_.volume = clamp(cfg_.getAttrDefault("volume", 100), 0, 100); - state_.is_initialized = false; - state_.is_running = false; - state_.is_error = false; - state_.error_message= ""; - } - catch (const exception& e) { - LOG(ERROR) << "[ffmpegMicroPhone] ([ffmpegMicroPhone]): Failed to parse XML: " << e.what(); - } -} - ffmpegMicroPhone::ffmpegMicroPhone(const config::FFMpegMicroPhoneConfig& cfg):input_fmt_ctx(nullptr), input_codec_ctx(nullptr), input_frame(nullptr), output_fmt_ctx(nullptr), output_fmt(nullptr), audio_st(nullptr), audio_codec_ctx(nullptr), audio_codec(nullptr), swr_ctx(nullptr), @@ -46,42 +24,50 @@ ffmpegMicroPhone::~ffmpegMicroPhone() { stop(); } -void ffmpegMicroPhone::init() { +bool ffmpegMicroPhone::init() { try { lock_guard lock(mtx_); state_.is_initialized = true; - LOG(INFO) << "[ffmpegMicroPhone] (init): Success, id=" << id_; + CMVR_LOG(INFO) << "[ffmpegMicroPhone] (init): Success, id=" << id_; + return true; } catch (const exception& e) { - LOG(ERROR) <<"[ffmpegMicroPhone] (init): Failed, id=" << id_ << ": " << e.what(); - throw runtime_error("[ffmpegMicroPhone] (init): Failed, id=" + id_ + ": " + string(e.what())); + CMVR_LOG(ERROR) <<"[ffmpegMicroPhone] (init): Failed, id=" << id_ << ": " << e.what(); + state_.is_error = true; + state_.error_message = e.what(); + return false; } } -void ffmpegMicroPhone::start() { +bool ffmpegMicroPhone::start() { try { lock_guard lock(mtx_); - LOG(INFO) << "[ffmpegMicroPhone] (start): Success, id=" << id_; + CMVR_LOG(INFO) << "[ffmpegMicroPhone] (start): Success, id=" << id_; + return true; } catch (const exception& e) { - LOG(ERROR) <<"[ffmpegMicroPhone] (start): Failed, id=" << id_ << ": " << e.what(); - throw runtime_error("[ffmpegMicroPhone] (start): Failed, id=" + id_ + ": " + string(e.what())); + CMVR_LOG(ERROR) <<"[ffmpegMicroPhone] (start): Failed, id=" << id_ << ": " << e.what(); + state_.is_error = true; + state_.error_message = e.what(); + return false; } } -void ffmpegMicroPhone::stop() { +bool ffmpegMicroPhone::stop() { stopRecording(); - LOG(INFO) << "[ffmpegMicroPhone] (stop): Success, id=" << id_; + CMVR_LOG(INFO) << "[ffmpegMicroPhone] (stop): Success, id=" << id_; + return true; } void ffmpegMicroPhone::pause() { try { is_paused = true; - LOG(INFO) << "[ffmpegMicroPhone] (pause): Success, id=" << id_; + CMVR_LOG(INFO) << "[ffmpegMicroPhone] (pause): Success, id=" << id_; } catch (const exception& e) { - LOG(ERROR) <<"[ffmpegMicroPhone] (pause): Failed, id=" << id_ << ": " << e.what(); - throw runtime_error("[ffmpegMicroPhone] (pause): Failed, id=" + id_ + ": " + string(e.what())); + CMVR_LOG(ERROR) <<"[ffmpegMicroPhone] (pause): Failed, id=" << id_ << ": " << e.what(); + state_.is_error = true; + state_.error_message = e.what(); } } @@ -89,11 +75,12 @@ void ffmpegMicroPhone::resume() { try { is_paused = false; pause_cv.notify_one(); - LOG(INFO) << "[ffmpegMicroPhone] (resume): Success, id=" << id_; + CMVR_LOG(INFO) << "[ffmpegMicroPhone] (resume): Success, id=" << id_; } catch (const exception& e) { - LOG(ERROR) <<"[ffmpegMicroPhone] (resume): Failed, id=" << id_ << ": " << e.what(); - throw runtime_error("[ffmpegMicroPhone] (resume): Failed, id=" + id_ + ": " + string(e.what())); + CMVR_LOG(ERROR) <<"[ffmpegMicroPhone] (resume): Failed, id=" << id_ << ": " << e.what(); + state_.is_error = true; + state_.error_message = e.what(); } } @@ -109,7 +96,8 @@ void ffmpegMicroPhone::getState(MicrophoneState& state) { void ffmpegMicroPhone::startRecording(const std::string& outputFilePath) { try { if (is_recording) { - throw runtime_error("[ffmpegMicroPhone] (startRecording): microphone is already recording"); + CMVR_LOG(ERROR) << "[ffmpegMicroPhone] (startRecording): microphone is already recording"; + return; } // 从文件路径中提取扩展名 std::string extension; @@ -118,16 +106,17 @@ void ffmpegMicroPhone::startRecording(const std::string& outputFilePath) { extension = outputFilePath.substr(dotPos + 1); } else { extension = "wav"; - LOG(WARNING) << "[ffmpegMicroPhone] (StartRecord): No file extension found, using wav as default, id=" << id_; - throw runtime_error("[ffmpegMicroPhone] (StartRecord): No file extension found, using wav as default, id=" + id_); + CMVR_LOG(WARNING) << "[ffmpegMicroPhone] (StartRecord): No file extension found, using wav as default, id=" << id_; } output_file = outputFilePath; format_name = extension; if (!initFFmpeg()) { - LOG(ERROR) <<"[ffmpegMicroPhone] (StartRecord): FFmpeg init error, id=" << id_; - throw runtime_error("[ffmpegMicroPhone] (StartRecord): FFmpeg init error, id=" + id_); + CMVR_LOG(ERROR) <<"[ffmpegMicroPhone] (StartRecord): FFmpeg init error, id=" << id_; + state_.is_error = true; + state_.error_message = "FFmpeg init error"; + return; } is_recording = true; @@ -135,15 +124,17 @@ void ffmpegMicroPhone::startRecording(const std::string& outputFilePath) { audio_thread_ = std::make_shared(&ffmpegMicroPhone::audioThread, this); } catch (const exception& e) { - LOG(ERROR) <<"[ffmpegMicroPhone] (StartRecord): Failed, id=" << id_ << ": " << e.what(); - throw runtime_error("[ffmpegMicroPhone] (StartRecord): Failed, id=" + id_ + ": " + string(e.what())); + CMVR_LOG(ERROR) <<"[ffmpegMicroPhone] (StartRecord): Failed, id=" << id_ << ": " << e.what(); + state_.is_error = true; + state_.error_message = e.what(); } } void ffmpegMicroPhone::stopRecording() { if (!is_recording) { - throw runtime_error("[ffmpegMicroPhone] (stopRecording): recording has not been started"); + CMVR_LOG(WARNING) << "[ffmpegMicroPhone] (stopRecording): recording has not been started"; + return; } is_recording = false; @@ -159,7 +150,7 @@ void ffmpegMicroPhone::stopRecording() { } closeFFmpeg(); - LOG(INFO) << "[ffmpegMicroPhone] (stopRecording): Success, id=" << id_; + CMVR_LOG(INFO) << "[ffmpegMicroPhone] (stopRecording): Success, id=" << id_; } void ffmpegMicroPhone::setVolume(const int volume) { @@ -176,7 +167,7 @@ bool ffmpegMicroPhone::initFFmpeg() { // 初始化输入设备 AVInputFormat* input_fmt = av_find_input_format("pulse"); if (!input_fmt) { - LOG(ERROR) << "Could not find pulse input format"; + CMVR_LOG(ERROR) << "Could not find pulse input format"; return false; } @@ -188,21 +179,21 @@ bool ffmpegMicroPhone::initFFmpeg() { if ((err = avformat_open_input(&input_fmt_ctx, input_device_.c_str(), input_fmt, &input_options)) < 0) { char errbuf[AV_ERROR_MAX_STRING_SIZE]; av_strerror(err, errbuf, AV_ERROR_MAX_STRING_SIZE); - LOG(ERROR) << "Could not open input device: " << errbuf; + CMVR_LOG(ERROR) << "Could not open input device: " << errbuf; av_dict_free(&input_options); return false; } av_dict_free(&input_options); if ((err = avformat_find_stream_info(input_fmt_ctx, nullptr)) < 0) { - LOG(ERROR) << "Could not find stream info"; + CMVR_LOG(ERROR) << "Could not find stream info"; return false; } // 查找音频流 int audio_stream_index = av_find_best_stream(input_fmt_ctx, AVMEDIA_TYPE_AUDIO, -1, -1, nullptr, 0); if (audio_stream_index < 0) { - LOG(ERROR) << "Could not find audio stream"; + CMVR_LOG(ERROR) << "Could not find audio stream"; return false; } @@ -210,55 +201,55 @@ bool ffmpegMicroPhone::initFFmpeg() { AVCodecParameters* codecpar = input_fmt_ctx->streams[audio_stream_index]->codecpar; const AVCodec* input_codec = avcodec_find_decoder(codecpar->codec_id); if (!input_codec) { - LOG(ERROR) << "Could not find input codec"; + CMVR_LOG(ERROR) << "Could not find input codec"; return false; } input_codec_ctx = avcodec_alloc_context3(input_codec); if (!input_codec_ctx) { - LOG(ERROR) << "Could not allocate input codec context"; + CMVR_LOG(ERROR) << "Could not allocate input codec context"; return false; } if ((err = avcodec_parameters_to_context(input_codec_ctx, codecpar)) < 0) { - LOG(ERROR) << "Could not copy input codec parameters"; + CMVR_LOG(ERROR) << "Could not copy input codec parameters"; return false; } if ((err = avcodec_open2(input_codec_ctx, input_codec, nullptr)) < 0) { - LOG(ERROR) << "Could not open input codec"; + CMVR_LOG(ERROR) << "Could not open input codec"; return false; } // 初始化输出格式 output_fmt = av_guess_format(format_name.c_str(), nullptr, nullptr); if (!output_fmt) { - LOG(ERROR) << "Unsupported format: " << format_name; + CMVR_LOG(ERROR) << "Unsupported format: " << format_name; return false; } if ((err = avformat_alloc_output_context2(&output_fmt_ctx, output_fmt, nullptr, output_file.c_str())) < 0) { - LOG(ERROR) << "Could not allocate output context"; + CMVR_LOG(ERROR) << "Could not allocate output context"; return false; } // 查找音频编码器 audio_codec = avcodec_find_encoder(output_fmt->audio_codec); if (!audio_codec) { - LOG(ERROR) << "Could not find audio encoder"; + CMVR_LOG(ERROR) << "Could not find audio encoder"; return false; } // 添加音频流 audio_st = avformat_new_stream(output_fmt_ctx, nullptr); if (!audio_st) { - LOG(ERROR) << "Could not create audio stream"; + CMVR_LOG(ERROR) << "Could not create audio stream"; return false; } audio_codec_ctx = avcodec_alloc_context3(audio_codec); if (!audio_codec_ctx) { - LOG(ERROR) << "Could not allocate audio codec context"; + CMVR_LOG(ERROR) << "Could not allocate audio codec context"; return false; } @@ -275,7 +266,7 @@ bool ffmpegMicroPhone::initFFmpeg() { if ((err = avcodec_open2(audio_codec_ctx, audio_codec, &opt)) < 0) { char errbuf[AV_ERROR_MAX_STRING_SIZE]; av_strerror(err, errbuf, AV_ERROR_MAX_STRING_SIZE); - LOG(ERROR) << "Could not open audio codec: " << errbuf; + CMVR_LOG(ERROR) << "Could not open audio codec: " << errbuf; av_dict_free(&opt); return false; } @@ -283,7 +274,7 @@ bool ffmpegMicroPhone::initFFmpeg() { // 复制编码参数到流 if (avcodec_parameters_from_context(audio_st->codecpar, audio_codec_ctx) < 0) { - LOG(ERROR) << "Could not copy codec parameters to stream"; + CMVR_LOG(ERROR) << "Could not copy codec parameters to stream"; return false; } @@ -291,7 +282,7 @@ bool ffmpegMicroPhone::initFFmpeg() { input_frame = av_frame_alloc(); audio_frame = av_frame_alloc(); if (!input_frame || !audio_frame) { - LOG(ERROR) << "Could not allocate audio frames"; + CMVR_LOG(ERROR) << "Could not allocate audio frames"; return false; } @@ -301,13 +292,13 @@ bool ffmpegMicroPhone::initFFmpeg() { audio_frame->nb_samples = audio_codec_ctx->frame_size ? audio_codec_ctx->frame_size : 1024; if (av_frame_get_buffer(audio_frame, 0) < 0) { - LOG(ERROR) << "Could not allocate audio frame buffers"; + CMVR_LOG(ERROR) << "Could not allocate audio frame buffers"; return false; } // 创建重采样上下文 swr_ctx = swr_alloc(); if (!swr_ctx) { - LOG(ERROR) << "Could not allocate resampler context"; + CMVR_LOG(ERROR) << "Could not allocate resampler context"; return false; } @@ -320,7 +311,7 @@ bool ffmpegMicroPhone::initFFmpeg() { input_codec_ctx->sample_rate, 0, nullptr); if (!swr_ctx || swr_init(swr_ctx) < 0) { - LOG(ERROR) << "Failed to initialize resampler"; + CMVR_LOG(ERROR) << "Failed to initialize resampler"; return false; } @@ -329,14 +320,14 @@ bool ffmpegMicroPhone::initFFmpeg() { if ((err = avio_open(&output_fmt_ctx->pb, output_file.c_str(), AVIO_FLAG_WRITE)) < 0) { char errbuf[AV_ERROR_MAX_STRING_SIZE]; av_strerror(err, errbuf, AV_ERROR_MAX_STRING_SIZE); - LOG(ERROR) << "Could not open output file: " << errbuf; + CMVR_LOG(ERROR) << "Could not open output file: " << errbuf; return false; } } // 写入文件头 if (avformat_write_header(output_fmt_ctx, nullptr) < 0) { - LOG(ERROR) << "Could not write output file header"; + CMVR_LOG(ERROR) << "Could not write output file header"; return false; } @@ -348,7 +339,7 @@ void ffmpegMicroPhone::audioThread() { AVPacket* input_pkt = av_packet_alloc(); AVPacket* output_pkt = av_packet_alloc(); if (!input_pkt || !output_pkt) { - LOG(ERROR) << "Could not allocate packets"; + CMVR_LOG(ERROR) << "Could not allocate packets"; return; } @@ -376,7 +367,7 @@ void ffmpegMicroPhone::audioThread() { while (avcodec_receive_frame(input_codec_ctx, input_frame) == 0) { if (input_frame->format != AV_SAMPLE_FMT_S16) { - LOG(ERROR) << "Input frame format mismatch"; + CMVR_LOG(ERROR) << "Input frame format mismatch"; return; } // 重采样 @@ -390,7 +381,7 @@ void ffmpegMicroPhone::audioThread() { (const uint8_t**)input_frame->data, input_frame->nb_samples ); if (ret < 0) { - LOG(ERROR) << "swr_convert failed"; + CMVR_LOG(ERROR) << "swr_convert failed"; av_frame_unref(input_frame); continue; } @@ -474,23 +465,3 @@ void ffmpegMicroPhone::closeFFmpeg() { avformat_free_context(output_fmt_ctx); } } - -void ffmpegMicroPhone::updateParams(const std::pair& param) -{ - std::lock_guard lock(mtx_); - try { - if (param.first == "sampleRate") { - sample_rate_ = stoi(param.second); - } - else if (param.first == "channels") { - channels_ = stoi(param.second); - } - - stop(); - init(); - start(); - } - catch (exception &e) { - throw runtime_error(e.what()); - } -} \ No newline at end of file diff --git a/cmvr-es/devices/microphone/microphone_factory.h b/cmvr-es/devices/microphone/microphone_factory.h new file mode 100644 index 00000000..ee749ee4 --- /dev/null +++ b/cmvr-es/devices/microphone/microphone_factory.h @@ -0,0 +1,60 @@ +#ifndef CMVR_ES_MICROPHONE_FACTORY_H +#define CMVR_ES_MICROPHONE_FACTORY_H + +#include +#include +#include + +#include "cmvr/config/microphone_config/microphone_config.pb.h" +#include "common/base/logging/logger.h" +#include "devices/microphone/abstract_microphone.h" +#include "devices/microphone/ffmpeg_microphone/include/ffmpeg_microphone.h" + +namespace cmvr::device { + +class MicrophoneFactory { +public: + static std::shared_ptr create(const config::MicrophoneDeviceConfig& cfg) + { + if (cfg.id().empty()) { + const std::string error = "[MicrophoneFactory]: Microphone id is empty"; + CMVR_LOG(ERROR) << error; + return nullptr; + } + + switch (cfg.backend_case()) { + case config::MicrophoneDeviceConfig::kFfmpeg: + return std::make_shared( + backendWithId_(cfg.id(), cfg.ffmpeg())); + + case config::MicrophoneDeviceConfig::BACKEND_NOT_SET: + default: + { + const std::string error = + "[MicrophoneFactory]: Microphone backend is not configured: " + cfg.id(); + CMVR_LOG(ERROR) << error; + return nullptr; + } + } + } + +private: + static config::FFMpegMicroPhoneConfig backendWithId_( + const std::string& id, + const config::FFMpegMicroPhoneConfig& source) + { + if (!source.id().empty() && source.id() != id) { + const std::string error = + "[MicrophoneFactory]: Microphone id does not match backend id: " + id; + CMVR_LOG(ERROR) << error; + return {}; + } + auto backend = source; + backend.set_id(id); + return backend; + } +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MICROPHONE_FACTORY_H diff --git a/cmvr-es/devices/motor/CMakeLists.txt b/cmvr-es/devices/motor/CMakeLists.txt index 532924d2..9c79fddf 100644 --- a/cmvr-es/devices/motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/CMakeLists.txt @@ -1,4 +1,6 @@ add_subdirectory(ti5_motor) +add_subdirectory(mujoco_motor) +add_subdirectory(motor_system) # -------------------------------------------------------- # Unit test diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index 710f88d2..69afad30 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -8,7 +8,7 @@ #pragma once #include "../abstract_device.h" -#include "../../utils/dynamics/include/joint.h" +#include "common/base/logging/logger.h" #include "motor/motor_protocol_interface.h" #include @@ -21,12 +21,12 @@ namespace cmvr::device{ } MotorProfile; - // xml 使用 struct MotorInfo { int id; std::string joint_name; - float limitQ; - float limitQd; + double limit_q_lb; + double limit_q_ub; + double limit_qd; }; typedef struct { @@ -41,14 +41,17 @@ namespace cmvr::device{ } JointStatus; public: - explicit AbstractMotor(const XmlNode &config): AbstractDevice(config) {} - AbstractMotor(const XmlNode &config,uint8_t node_id): AbstractDevice(config) ,node_id_(node_id) {} + AbstractMotor() = default; + explicit AbstractMotor(uint8_t node_id): node_id_(node_id) {} ~AbstractMotor() override = default; + DeviceKind kind() const noexcept override { return DeviceKind::Motor; } + virtual void setMode(msgs::RunMode mode) { std::scoped_lock lock(mtx_); if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return; } protocol_->setMode(node_id_, mode); } @@ -56,7 +59,8 @@ namespace cmvr::device{ virtual msgs::RunMode getMode() { std::scoped_lock lock(mtx_); if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return msgs::RUN_MODE_UNSPECIFIED; } return protocol_->getMode(node_id_); } @@ -64,7 +68,8 @@ namespace cmvr::device{ void torqueOff() { std::scoped_lock lock(mtx_); if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return; } protocol_->torqueOff(node_id_); } @@ -72,21 +77,24 @@ namespace cmvr::device{ virtual void setLimitQ(double ub, double lb) { std::scoped_lock lock(mtx_); if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return; } protocol_->setLimitQ(node_id_, ub,lb); } virtual void setLimitQd(double qd) { std::scoped_lock lock(mtx_); if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return; } protocol_->setLimitQd(node_id_, qd); } virtual void setLimitQdd(double u_qdd,double l_qdd) { std::scoped_lock lock(mtx_); if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return; } protocol_->setLimitQdd(node_id_, u_qdd,l_qdd); } @@ -95,7 +103,8 @@ namespace cmvr::device{ virtual void brake() { std::scoped_lock lock(mtx_); if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return; } protocol_->brake(node_id_); } @@ -106,7 +115,8 @@ namespace cmvr::device{ virtual void setQ(double q) { std::scoped_lock lock(mtx_); if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return; } protocol_->setQ(node_id_, q); } @@ -114,7 +124,8 @@ namespace cmvr::device{ virtual void setTarget(double q,double qd) { std::scoped_lock lock(mtx_); if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return; } protocol_->setTarget(node_id_,q, qd); } @@ -122,7 +133,8 @@ namespace cmvr::device{ virtual void setTarget(double qd) { std::scoped_lock lock(mtx_); if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return; } protocol_->setTarget(node_id_, qd); } @@ -130,14 +142,16 @@ namespace cmvr::device{ virtual bool calibrateZeroQ() { std::scoped_lock lock(mtx_); if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; } return protocol_->calibrateZeroQ(node_id_); } virtual bool reachedTargetQ() { if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; } return protocol_->reachedTargetQ(node_id_); } @@ -145,7 +159,8 @@ namespace cmvr::device{ virtual void setQd(double qd) { std::scoped_lock lock(mtx_); if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return; } return protocol_->setQd(node_id_,qd); } @@ -157,14 +172,16 @@ namespace cmvr::device{ virtual double getQ() { if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return 0.0; } return protocol_->getQ(node_id_); } virtual double getQd() { if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return 0.0; } return protocol_->getQd(node_id_); } @@ -174,6 +191,10 @@ namespace cmvr::device{ virtual void setProtocol(std::shared_ptr protocol) { std::scoped_lock lock(mtx_); protocol_ = std::move(protocol); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return; + } protocol_->initNode(node_id_); } diff --git a/cmvr-es/devices/motor/motor_manager_test.cpp b/cmvr-es/devices/motor/motor_manager_test.cpp index ea3e6769..3bf862eb 100644 --- a/cmvr-es/devices/motor/motor_manager_test.cpp +++ b/cmvr-es/devices/motor/motor_manager_test.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by lgv on 2025/8/1. // @@ -11,7 +12,6 @@ #include "canbus/can_comm/can_sender.h" #include "canbus/can_comm/message_manager.h" #include "canbus/can_client/socket/socket_can_client_raw.h" -#include "canbus/can_client/pcan//pcan_client.h" using namespace cmvr::device; using namespace cmvr::msgs; @@ -19,10 +19,17 @@ using namespace cmvr::msgs; TEST(MotorMangerTest,MyTest) { uint8_t id = 22; - XmlNode cfg; + cmvr::config::SocketCanConfig can_cfg; + can_cfg.set_channel_id(0); + cmvr::config::Ti5MotorConfig motor_cfg; + motor_cfg.set_id(id); + motor_cfg.set_joint_name("test_motor"); + motor_cfg.set_limit_q_lb(-3.14); + motor_cfg.set_limit_q_ub(3.14); + motor_cfg.set_limit_qd(3.0); // 1 === 初始化公共组件 === - auto can_client = std::make_shared(cfg); + auto can_client = std::make_shared(can_cfg); auto sender = std::make_shared>(); auto receiver = std::make_shared>(); auto message_manager = std::make_shared>(); @@ -31,20 +38,20 @@ TEST(MotorMangerTest,MyTest) { can_client->init(); auto ret = sender->Init(can_client.get(), false); if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to init can sender."; + CMVR_LOG(ERROR) << "Failed to init can sender."; } ret = receiver->Init(can_client.get(), message_manager.get(), false); if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to init can receiver."; + CMVR_LOG(ERROR) << "Failed to init can receiver."; } // 2 == 创建协议 === auto canopen_protocol = std::make_shared(sender, message_manager); // 3 === 创建电机 === - auto motor = std::make_shared(cfg,id); + auto motor = std::make_shared(motor_cfg); motor->setProtocol(canopen_protocol); // 4 === 添加电机到 MotorManager=== @@ -56,12 +63,12 @@ TEST(MotorMangerTest,MyTest) { can_client->start(); ret = sender->Start(); if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to start can sender."; + CMVR_LOG(ERROR) << "Failed to start can sender."; } ret = receiver->Start(); if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to start can receiver."; + CMVR_LOG(ERROR) << "Failed to start can receiver."; } // 6 === 控制电机 === @@ -109,5 +116,5 @@ TEST(MotorMangerTest,MyTest) { } - LOG(INFO) << "Testing MotorManger"; -} \ No newline at end of file + CMVR_LOG(INFO) << "Testing MotorManger"; +} diff --git a/cmvr-es/devices/motor/motor_system/CMakeLists.txt b/cmvr-es/devices/motor/motor_system/CMakeLists.txt new file mode 100644 index 00000000..ca0babd2 --- /dev/null +++ b/cmvr-es/devices/motor/motor_system/CMakeLists.txt @@ -0,0 +1,19 @@ +add_library(motor_system SHARED + src/motor_system.cpp +) + +target_include_directories(motor_system PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(motor_system + PUBLIC + cmvr_es::proto + PRIVATE + cmvr_es::device::canbus + cmvr_es::device::ti5motor + cmvr_es::device::mujoco_motor + cmvr_es::ik_solver + glog +) + +add_library(cmvr_es::device::motor_system ALIAS motor_system) +install(TARGETS motor_system LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/motor/motor_system/include/motor_system.h b/cmvr-es/devices/motor/motor_system/include/motor_system.h new file mode 100644 index 00000000..2e9083f2 --- /dev/null +++ b/cmvr-es/devices/motor/motor_system/include/motor_system.h @@ -0,0 +1,81 @@ +#ifndef CMVR_ES_MOTOR_SYSTEM_H +#define CMVR_ES_MOTOR_SYSTEM_H + +#include +#include +#include +#include +#include +#include + +#include "devices/abstract_device.h" +#include "devices/canbus/abstract_canbus.h" +#include "canbus/can_comm/can_receiver.h" +#include "canbus/can_comm/can_sender.h" +#include "canbus/can_comm/message_manager.h" +#include "cmvr/config/motor_config/motor_config.pb.h" +#include "cmvr/msgs/robot_detail.pb.h" +#include "motor/motor_manager.h" + +namespace cmvr::device { + +class MujocoJointBridge; + +class MotorSystem final : public AbstractDevice { +public: + MotorSystem(std::string id, const config::MotorConfig& cfg); + ~MotorSystem() override = default; + + DeviceKind kind() const noexcept override { return DeviceKind::MotorSystem; } + std::string typeName() const override { return "MotorSystem"; } + + bool init() override; + bool stop() override; + + std::shared_ptr manager() const { return motor_manager_; } + static std::shared_ptr managerFor(const std::string& id); + static std::shared_ptr mujocoBridgeFor(const std::string& id); + static void setActiveJoints(const std::string& motor_system_id, + std::unordered_map> group_joints); + static void clearActiveJoints(); + +private: + using ActiveJointSelection = std::unordered_map>; + + struct MotorGroupRuntime { + std::string name; + config::MotorBusType bus_type{config::MOTOR_BUS_UNKNOWN}; + bool enabled{false}; + std::shared_ptr client; + std::shared_ptr> sender; + std::shared_ptr> receiver; + std::shared_ptr> message_manager; + std::shared_ptr mujoco_bridge; + std::vector motor_cfgs; + }; + + bool initMotorGroup_(MotorGroupRuntime& group); + bool initMotors_(const MotorGroupRuntime& group); + bool selectActiveMotors_(const std::string& group_name, + const google::protobuf::RepeatedPtrField& source, + std::vector& selected) const; + bool applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg, + std::vector& selected) const; + static bool makeSocketCanConfig_(const config::MotorGroupConfig& group_cfg, + config::SocketCanConfig& config); + +private: + config::MotorConfig cfg_; + std::vector motor_groups_; + std::shared_ptr motor_manager_{nullptr}; + bool initialized_{false}; + + static std::mutex registry_mutex_; + static std::unordered_map> registry_; + static std::unordered_map> mujoco_bridge_registry_; + static std::unordered_map active_joints_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MOTOR_SYSTEM_H diff --git a/cmvr-es/devices/motor/motor_system/src/motor_system.cpp b/cmvr-es/devices/motor/motor_system/src/motor_system.cpp new file mode 100644 index 00000000..38555207 --- /dev/null +++ b/cmvr-es/devices/motor/motor_system/src/motor_system.cpp @@ -0,0 +1,407 @@ +#include "motor/motor_system/include/motor_system.h" + +#include +#include +#include +#include +#include +#include + +#include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h" +#include "common/base/logging/logger.h" +#include "common/config/config_files.h" + +#include "devices/motor/mujoco_motor/include/mujoco_motor.h" +#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" +#include "motor/ti5_motor/ti5_motor.h" + +namespace cmvr::device { + +std::mutex MotorSystem::registry_mutex_; +std::unordered_map> MotorSystem::registry_; +std::unordered_map> MotorSystem::mujoco_bridge_registry_; +std::unordered_map MotorSystem::active_joints_; + +MotorSystem::MotorSystem(std::string id, const config::MotorConfig& cfg) + : cfg_(cfg) +{ + id_ = std::move(id); + if (!cfg_.id().empty() && cfg_.id() != id_) { + CMVR_LOG(ERROR) << "[MotorSystem] config id '" << cfg_.id() + << "' does not match device id '" << id_ << "'"; + id_.clear(); + } +} + +bool MotorSystem::init() +{ + if (initialized_) { + return true; + } + + motor_manager_ = std::make_shared(); + if (id_.empty()) { + CMVR_LOG(ERROR) << "[MotorSystem] id is empty"; + return false; + } + motor_groups_.clear(); + motor_groups_.reserve(cfg_.motor_groups_size()); + + bool all_ok = true; + for (const auto& motor_group_cfg : cfg_.motor_groups()) { + MotorGroupRuntime group; + group.name = motor_group_cfg.id(); + if (group.name.empty()) { + CMVR_LOG(ERROR) << "[MotorSystem] motor group id is empty in system: " << id_; + continue; + } + group.bus_type = motor_group_cfg.bus_type(); + group.enabled = selectActiveMotors_(group.name, motor_group_cfg.motors(), group.motor_cfgs); + const bool selected_active_group = group.enabled; + if (group.enabled && !applyConfiguredJointLimits_(motor_group_cfg, group.motor_cfgs)) { + group.enabled = false; + } + if (group.enabled) { + switch (group.bus_type) { + case config::MOTOR_BUS_CAN: { + config::SocketCanConfig can_config; + if (!makeSocketCanConfig_(motor_group_cfg, can_config)) { + group.enabled = false; + break; + } + group.client = std::make_shared(can_config); + group.sender = std::make_shared>(); + group.receiver = std::make_shared>(); + group.message_manager = std::make_shared>(); + break; + } + case config::MOTOR_BUS_MUJOCO: + group.mujoco_bridge = std::make_shared(group.motor_cfgs.size()); + break; + case config::MOTOR_BUS_ETHERCAT: + CMVR_LOG(ERROR) << "[MotorSystem] EtherCAT motor group is configured but runtime is not implemented: " << group.name; + group.enabled = false; + break; + case config::MOTOR_BUS_UNKNOWN: + default: + CMVR_LOG(ERROR) << "[MotorSystem] Unsupported motor bus type for group: " << group.name; + group.enabled = false; + break; + } + } + if (selected_active_group && !group.enabled) { + all_ok = false; + } + motor_groups_.emplace_back(std::move(group)); + } + + std::vector> tasks; + for (auto& group : motor_groups_) { + if (!group.enabled) { + continue; + } + if (group.bus_type != config::MOTOR_BUS_MUJOCO) { + if (!initMotorGroup_(group)) { + group.enabled = false; + all_ok = false; + continue; + } + } + tasks.push_back(std::async(std::launch::async, [this, &group]() { + return initMotors_(group); + })); + } + + for (auto& task : tasks) { + all_ok = task.get() && all_ok; + } + if (!all_ok) { + CMVR_LOG(ERROR) << "[MotorSystem] motor initialization failed: " << id_; + return false; + } + + initialized_ = true; + { + std::lock_guard lock(registry_mutex_); + registry_[id_] = motor_manager_; + for (const auto& group : motor_groups_) { + if (group.mujoco_bridge) { + mujoco_bridge_registry_[id_] = group.mujoco_bridge; + break; + } + } + } + CMVR_LOG(INFO) << "[MotorSystem] Initialized motor system: " << id_; + return true; +} + +bool MotorSystem::stop() +{ + if (!motor_manager_) { + return true; + } + for (const auto& [joint_name, motor] : motor_manager_->motorsMap()) { + if (motor) { + motor->torqueOff(); + } + } + return true; +} + +std::shared_ptr MotorSystem::managerFor(const std::string& id) +{ + std::lock_guard lock(registry_mutex_); + const auto it = registry_.find(id); + if (it == registry_.end()) { + return nullptr; + } + return it->second.lock(); +} + +std::shared_ptr MotorSystem::mujocoBridgeFor(const std::string& id) +{ + std::lock_guard lock(registry_mutex_); + const auto it = mujoco_bridge_registry_.find(id); + if (it == mujoco_bridge_registry_.end()) { + return nullptr; + } + return it->second.lock(); +} + +void MotorSystem::setActiveJoints(const std::string& motor_system_id, + ActiveJointSelection group_joints) +{ + std::lock_guard lock(registry_mutex_); + active_joints_[motor_system_id] = std::move(group_joints); +} + +void MotorSystem::clearActiveJoints() +{ + std::lock_guard lock(registry_mutex_); + active_joints_.clear(); +} + +bool MotorSystem::selectActiveMotors_( + const std::string& group_name, + const google::protobuf::RepeatedPtrField& source, + std::vector& selected) const +{ + selected.clear(); + + ActiveJointSelection selection; + { + std::lock_guard lock(registry_mutex_); + const auto it = active_joints_.find(id_); + if (it != active_joints_.end()) { + selection = it->second; + } + } + + if (selection.empty()) { + CMVR_LOG(WARNING) << "[MotorSystem] No active joints selected for motor system " << id_ + << ", motor group " << group_name << " will not initialize motors."; + return false; + } + + const auto group_it = selection.find(group_name); + if (group_it == selection.end() || group_it->second.empty()) { + return false; + } + + for (const auto& motor_cfg : source) { + if (group_it->second.count(motor_cfg.joint_name()) > 0) { + selected.push_back(motor_cfg); + } + } + + if (selected.size() != group_it->second.size()) { + std::unordered_set found; + for (const auto& motor_cfg : selected) { + found.insert(motor_cfg.joint_name()); + } + for (const auto& joint_name : group_it->second) { + if (found.count(joint_name) == 0) { + CMVR_LOG(ERROR) << "[MotorSystem] active joint '" << joint_name + << "' not found in motor group '" << group_name << "'"; + return false; + } + } + } + return !selected.empty(); +} + +bool MotorSystem::applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg, + std::vector& selected) const +{ + if (selected.empty() || !group_cfg.has_joint_limits()) { + return true; + } + + const auto& limits_cfg = group_cfg.joint_limits(); + const auto source = limits_cfg.source(); + if (source == config::JOINT_LIMIT_SOURCE_UNKNOWN) { + return true; + } + + std::unordered_map limits_by_joint; + if (source == config::JOINT_LIMIT_SOURCE_CUSTOM) { + limits_by_joint.reserve(static_cast(limits_cfg.joints_size())); + for (const auto& limit : limits_cfg.joints()) { + if (!limit.joint_name().empty()) { + limits_by_joint[limit.joint_name()] = limit; + } + } + } else if (source == config::JOINT_LIMIT_SOURCE_URDF) { + const auto urdf_path = + cmvr::ConfigHelper::resolveResourceFile(group_cfg.joint_limits_urdf_path()); + if (urdf_path.empty()) { + CMVR_LOG(ERROR) << "[MotorSystem] joint_limits_urdf_path is empty for motor group: " + << group_cfg.id(); + return false; + } + + UrdfParser parser; + std::string error; + if (!parser.loadModel(urdf_path, &error)) { + CMVR_LOG(ERROR) << "[MotorSystem] Failed to load joint limit URDF: " << error; + return false; + } + const auto& model = parser.model(); + for (pinocchio::JointIndex joint_id = 1; joint_id < model.njoints; ++joint_id) { + if (model.nqs[joint_id] != 1 || model.nvs[joint_id] != 1) { + continue; + } + config::JointLimitConfig limit; + limit.set_joint_name(model.names[joint_id]); + limit.set_lower(model.lowerPositionLimit[model.idx_qs[joint_id]]); + limit.set_upper(model.upperPositionLimit[model.idx_qs[joint_id]]); + limit.set_velocity(model.velocityLimit[model.idx_vs[joint_id]]); + limits_by_joint[limit.joint_name()] = limit; + } + } else { + CMVR_LOG(ERROR) << "[MotorSystem] unsupported joint limit source"; + return false; + } + + for (auto& motor_cfg : selected) { + const auto it = limits_by_joint.find(motor_cfg.joint_name()); + if (it == limits_by_joint.end()) { + CMVR_LOG(ERROR) << "[MotorSystem] missing joint limit for " << motor_cfg.joint_name() + << " in motor group: " << group_cfg.id(); + return false; + } + const auto& limit = it->second; + if (!std::isfinite(limit.lower()) || !std::isfinite(limit.upper()) || + !std::isfinite(limit.velocity()) || limit.upper() <= limit.lower() || + limit.velocity() <= 0.0) { + CMVR_LOG(ERROR) << "[MotorSystem] invalid joint limit for " << motor_cfg.joint_name(); + return false; + } + motor_cfg.set_limit_q_lb(limit.lower()); + motor_cfg.set_limit_q_ub(limit.upper()); + motor_cfg.set_limit_qd(std::abs(limit.velocity())); + } + return true; +} + +bool MotorSystem::makeSocketCanConfig_(const config::MotorGroupConfig& group_cfg, + config::SocketCanConfig& config) +{ + if (!group_cfg.has_can()) { + CMVR_LOG(ERROR) << "[MotorSystem] CAN motor group missing can config: " << group_cfg.id(); + return false; + } + config = group_cfg.can(); + return true; +} + +bool MotorSystem::initMotorGroup_(MotorGroupRuntime& group) +{ + if (group.bus_type != config::MOTOR_BUS_CAN) { + CMVR_LOG(ERROR) << "[MotorSystem] Unsupported motor group runtime: " << group.name; + return false; + } + if (!group.client || !group.sender || !group.receiver || !group.message_manager) { + CMVR_LOG(ERROR) << "[MotorSystem] Invalid motor group runtime: " << group.name; + return false; + } + + if (!group.client->init()) { + CMVR_LOG(ERROR) << "[MotorSystem] Failed to init CAN client: " << group.name; + return false; + } + + auto ret = group.sender->Init(group.client.get(), false); + if (ret != ErrorCode::OK) { + CMVR_LOG(ERROR) << "[MotorSystem] Failed to init CAN sender: " << group.name; + return false; + } + + ret = group.receiver->Init(group.client.get(), group.message_manager.get(), false); + if (ret != ErrorCode::OK) { + CMVR_LOG(ERROR) << "[MotorSystem] Failed to init CAN receiver: " << group.name; + return false; + } + + if (!group.client->start()) { + CMVR_LOG(ERROR) << "[MotorSystem] Failed to start CAN client: " << group.name; + return false; + } + + ret = group.sender->Start(); + if (ret != ErrorCode::OK) { + CMVR_LOG(ERROR) << "[MotorSystem] Failed to start CAN sender: " << group.name; + return false; + } + + ret = group.receiver->Start(); + if (ret != ErrorCode::OK) { + CMVR_LOG(ERROR) << "[MotorSystem] Failed to start CAN receiver: " << group.name; + return false; + } + return true; +} + +bool MotorSystem::initMotors_(const MotorGroupRuntime& group) +{ + switch (group.bus_type) { + case config::MOTOR_BUS_CAN: { + auto protocol = std::make_shared(group.sender, group.message_manager); + for (const auto& cfg : group.motor_cfgs) { + auto motor = std::make_shared(cfg); + motor->setProtocol(protocol); + if (!motor->init()) { + CMVR_LOG(ERROR) << "[MotorSystem] Failed to init motor: " << cfg.joint_name(); + return false; + } + motor_manager_->addMotor(motor); + } + return true; + } + case config::MOTOR_BUS_MUJOCO: { + if (!group.mujoco_bridge) { + CMVR_LOG(ERROR) << "[MotorSystem] MuJoCo motor group missing joint bridge: " << group.name; + return false; + } + for (std::size_t i = 0; i < group.motor_cfgs.size(); ++i) { + const auto& cfg = group.motor_cfgs[i]; + auto motor = std::make_shared( + i, + cfg.joint_name(), + group.mujoco_bridge, + static_cast(cfg.id())); + motor->setLimitQ(cfg.limit_q_ub(), cfg.limit_q_lb()); + motor->setLimitQd(cfg.limit_qd()); + motor_manager_->addMotor(motor); + } + return true; + } + case config::MOTOR_BUS_ETHERCAT: + case config::MOTOR_BUS_UNKNOWN: + default: + CMVR_LOG(ERROR) << "[MotorSystem] Unsupported motor init runtime: " << group.name; + return false; + } +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/mujoco_motor/CMakeLists.txt b/cmvr-es/devices/motor/mujoco_motor/CMakeLists.txt new file mode 100644 index 00000000..d4c9271d --- /dev/null +++ b/cmvr-es/devices/motor/mujoco_motor/CMakeLists.txt @@ -0,0 +1,13 @@ +add_library(mujoco_motor SHARED + src/mujoco_motor.cpp +) + +target_include_directories(mujoco_motor PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(mujoco_motor + PUBLIC + cmvr_es::proto +) + +add_library(cmvr_es::device::mujoco_motor ALIAS mujoco_motor) +install(TARGETS mujoco_motor LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/motor/mujoco_motor/include/mujoco_joint_bridge.h b/cmvr-es/devices/motor/mujoco_motor/include/mujoco_joint_bridge.h new file mode 100644 index 00000000..6a868be8 --- /dev/null +++ b/cmvr-es/devices/motor/mujoco_motor/include/mujoco_joint_bridge.h @@ -0,0 +1,57 @@ +#ifndef CMVR_ES_MUJOCO_JOINT_BRIDGE_H +#define CMVR_ES_MUJOCO_JOINT_BRIDGE_H + +#include +#include +#include +#include +#include + +#include "cmvr/msgs/motor.pb.h" + +namespace cmvr::device { + +class MujocoJointBridge { +public: + struct CommandState { + std::vector mode; + std::vector position; + std::vector velocity; + }; + + explicit MujocoJointBridge(std::size_t dof); + + std::size_t dof() const noexcept { return dof_; } + + void setMode(std::size_t index, msgs::RunMode mode); + msgs::RunMode getMode(std::size_t index) const; + + void setPositionTarget(std::size_t index, double position, double velocity); + void setVelocityTarget(std::size_t index, double velocity); + + CommandState commands() const; + + void publishMeasured(const std::vector& position, + const std::vector& velocity); + double position(std::size_t index) const; + double velocity(std::size_t index) const; + + void markReady(bool valid); + bool waitUntilReady(std::chrono::milliseconds timeout); + +private: + void validateIndex_(std::size_t index) const; + + const std::size_t dof_{0}; + mutable std::mutex mutex_; + std::condition_variable ready_cv_; + CommandState commands_; + std::vector measured_position_; + std::vector measured_velocity_; + bool ready_{false}; + bool valid_{false}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MUJOCO_JOINT_BRIDGE_H diff --git a/cmvr-es/devices/motor/mujoco_motor/include/mujoco_motor.h b/cmvr-es/devices/motor/mujoco_motor/include/mujoco_motor.h new file mode 100644 index 00000000..bbe6baaa --- /dev/null +++ b/cmvr-es/devices/motor/mujoco_motor/include/mujoco_motor.h @@ -0,0 +1,50 @@ +#ifndef CMVR_ES_MUJOCO_MOTOR_H +#define CMVR_ES_MUJOCO_MOTOR_H + +#include +#include +#include +#include + +#include "devices/motor/abstract_motor.h" +#include "devices/motor/mujoco_motor/include/mujoco_joint_bridge.h" + +namespace cmvr::device { + +class MujocoMotor final : public AbstractMotor { +public: + MujocoMotor(std::size_t joint_index, + std::string joint_name, + std::shared_ptr bridge, + std::uint8_t node_id = 0); + + std::string typeName() const override { return "MujocoMotor"; } + bool init() override { return true; } + + void setMode(msgs::RunMode mode) override; + msgs::RunMode getMode() override; + + void setLimitQ(double ub, double lb) override; + void setLimitQd(double qd) override; + void setLimitQdd(double u_qdd, double l_qdd) override; + + void brake() override; + void setQ(double q) override; + void setTarget(double q, double qd) override; + void setTarget(double qd) override; + bool calibrateZeroQ() override; + bool reachedTargetQ() override; + void setQd(double qd) override; + double getQ() override; + double getQd() override; + +private: + std::size_t joint_index_{0}; + std::shared_ptr bridge_; + double limit_qdd_upper_{0.0}; + double limit_qdd_lower_{0.0}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MUJOCO_MOTOR_H diff --git a/cmvr-es/devices/motor/mujoco_motor/src/mujoco_motor.cpp b/cmvr-es/devices/motor/mujoco_motor/src/mujoco_motor.cpp new file mode 100644 index 00000000..a48d29db --- /dev/null +++ b/cmvr-es/devices/motor/mujoco_motor/src/mujoco_motor.cpp @@ -0,0 +1,225 @@ +#include "devices/motor/mujoco_motor/include/mujoco_motor.h" + +#include +#include +#include + +#include "common/base/logging/logger.h" + +namespace cmvr::device { + +MujocoJointBridge::MujocoJointBridge(const std::size_t dof) + : dof_(dof) +{ + if (dof_ == 0) { + CMVR_LOG(ERROR) << "MujocoJointBridge dof must be positive"; + return; + } + commands_.mode.assign(dof_, msgs::RUN_MODE_UNSPECIFIED); + commands_.position.assign(dof_, 0.0); + commands_.velocity.assign(dof_, 0.0); + measured_position_.assign(dof_, 0.0); + measured_velocity_.assign(dof_, 0.0); +} + +void MujocoJointBridge::validateIndex_(const std::size_t index) const +{ + if (index >= dof_) { + CMVR_LOG(ERROR) << "MujocoJointBridge joint index out of range"; + } +} + +void MujocoJointBridge::setMode(const std::size_t index, const msgs::RunMode mode) +{ + validateIndex_(index); + if (index >= dof_) { + return; + } + std::lock_guard lock(mutex_); + commands_.mode[index] = mode; +} + +msgs::RunMode MujocoJointBridge::getMode(const std::size_t index) const +{ + validateIndex_(index); + if (index >= dof_) { + return msgs::RUN_MODE_UNSPECIFIED; + } + std::lock_guard lock(mutex_); + return commands_.mode[index]; +} + +void MujocoJointBridge::setPositionTarget(const std::size_t index, + const double position, + const double velocity) +{ + validateIndex_(index); + if (index >= dof_) { + return; + } + std::lock_guard lock(mutex_); + commands_.position[index] = position; + commands_.velocity[index] = velocity; +} + +void MujocoJointBridge::setVelocityTarget(const std::size_t index, const double velocity) +{ + validateIndex_(index); + if (index >= dof_) { + return; + } + std::lock_guard lock(mutex_); + commands_.velocity[index] = velocity; +} + +MujocoJointBridge::CommandState MujocoJointBridge::commands() const +{ + std::lock_guard lock(mutex_); + return commands_; +} + +void MujocoJointBridge::publishMeasured(const std::vector& position, + const std::vector& velocity) +{ + if (position.size() != dof_ || velocity.size() != dof_) { + CMVR_LOG(ERROR) << "MujocoJointBridge measured vectors size mismatch"; + return; + } + std::lock_guard lock(mutex_); + measured_position_ = position; + measured_velocity_ = velocity; +} + +double MujocoJointBridge::position(const std::size_t index) const +{ + validateIndex_(index); + if (index >= dof_) { + return 0.0; + } + std::lock_guard lock(mutex_); + return measured_position_[index]; +} + +double MujocoJointBridge::velocity(const std::size_t index) const +{ + validateIndex_(index); + if (index >= dof_) { + return 0.0; + } + std::lock_guard lock(mutex_); + return measured_velocity_[index]; +} + +void MujocoJointBridge::markReady(const bool valid) +{ + { + std::lock_guard lock(mutex_); + ready_ = true; + valid_ = valid; + } + ready_cv_.notify_all(); +} + +bool MujocoJointBridge::waitUntilReady(const std::chrono::milliseconds timeout) +{ + std::unique_lock lock(mutex_); + return ready_cv_.wait_for(lock, timeout, [this] { return ready_; }) && valid_; +} + +MujocoMotor::MujocoMotor(const std::size_t joint_index, + std::string joint_name, + std::shared_ptr bridge, + const std::uint8_t node_id) + : AbstractMotor(node_id == 0 ? static_cast(joint_index + 1) : node_id), + joint_index_(joint_index), + bridge_(std::move(bridge)) +{ + if (!bridge_) { + CMVR_LOG(ERROR) << "MujocoMotor bridge is null"; + return; + } + if (joint_index_ >= bridge_->dof()) { + CMVR_LOG(ERROR) << "MujocoMotor joint index out of range"; + bridge_.reset(); + return; + } + + info_.id = id(); + info_.joint_name = std::move(joint_name); + id_ = info_.joint_name; +} + +void MujocoMotor::setMode(const msgs::RunMode mode) +{ + bridge_->setMode(joint_index_, mode); +} + +msgs::RunMode MujocoMotor::getMode() +{ + return bridge_->getMode(joint_index_); +} + +void MujocoMotor::setLimitQ(const double ub, const double lb) +{ + info_.limit_q_ub = ub; + info_.limit_q_lb = lb; +} + +void MujocoMotor::setLimitQd(const double qd) +{ + info_.limit_qd = qd; +} + +void MujocoMotor::setLimitQdd(const double u_qdd, const double l_qdd) +{ + limit_qdd_upper_ = u_qdd; + limit_qdd_lower_ = l_qdd; +} + +void MujocoMotor::brake() +{ + bridge_->setMode(joint_index_, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + bridge_->setPositionTarget(joint_index_, bridge_->position(joint_index_), 0.0); +} + +void MujocoMotor::setQ(const double q) +{ + bridge_->setPositionTarget(joint_index_, q, 0.0); +} + +void MujocoMotor::setTarget(const double q, const double qd) +{ + bridge_->setPositionTarget(joint_index_, q, qd); +} + +void MujocoMotor::setTarget(const double qd) +{ + bridge_->setVelocityTarget(joint_index_, qd); +} + +bool MujocoMotor::calibrateZeroQ() +{ + return true; +} + +bool MujocoMotor::reachedTargetQ() +{ + return true; +} + +void MujocoMotor::setQd(const double qd) +{ + bridge_->setVelocityTarget(joint_index_, qd); +} + +double MujocoMotor::getQ() +{ + return bridge_->position(joint_index_); +} + +double MujocoMotor::getQd() +{ + return bridge_->velocity(joint_index_); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/ti5_motor/CMakeLists.txt b/cmvr-es/devices/motor/ti5_motor/CMakeLists.txt index 508b88b8..8585f5c6 100644 --- a/cmvr-es/devices/motor/ti5_motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/ti5_motor/CMakeLists.txt @@ -17,8 +17,8 @@ add_library(cmvr_es::device::ti5motor ALIAS ti5motor) target_link_libraries(ti5motor PRIVATE cmvr_es::device::canbus - protobuf + cmvr_es::proto glog ) -install(TARGETS ti5motor LIBRARY DESTINATION lib) \ No newline at end of file +install(TARGETS ti5motor LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp index 88c655a2..e4e9c2c6 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp +++ b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp @@ -3,7 +3,7 @@ // #include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h" -#include "glog/logging.h" +#include "common/base/logging/logger.h" using namespace cmvr::msgs; using namespace cmvr::device::motor; diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.cpp index 6c6b5ff0..24edb9c3 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.cpp +++ b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.cpp @@ -3,7 +3,7 @@ // #include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h" -#include "glog/logging.h" +#include "common/base/logging/logger.h" using namespace cmvr::msgs; using namespace cmvr::device::motor; diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp index e90f28a9..48eaadf3 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp +++ b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by lgv on 2025/7/24. // @@ -26,13 +27,13 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response, break; case msgs::ACTUAL_POSITION_6064: motor_status->set_position(static_cast(sdo_response.data())); - LOG(INFO) << "pos = " << motor_status->position(); + CMVR_LOG(INFO) << "pos = " << motor_status->position(); break; case msgs::POSITION_OFFSET_2008: motor_status->set_position_offset(sdo_response.data()); } // - // LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_) + // CMVR_LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_) // << ": command=" << int(sdo_response.cs()) // << ", index=" << sdo_response.index() // << ", subindex=" << int(sdo_response.sub_index()) diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp index 57f83aa3..99c88c38 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp +++ b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp @@ -3,7 +3,7 @@ // #include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h" -#include "glog/logging.h" +#include "common/base/logging/logger.h" #include "canbus/canopen/register.h" using namespace cmvr::msgs; using namespace cmvr::device::motor; @@ -11,7 +11,7 @@ using namespace cmvr::device::motor; void Ti5MotorTPDO1::Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const { if (length < 7) { - LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length; + CMVR_LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length; return; } @@ -27,11 +27,11 @@ void Ti5MotorTPDO1::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot st.value = motor_status->status_word(); // if (st.op_mode_specific > 0) { - // LOG(INFO) << st.op_mode_specific ; + // CMVR_LOG(INFO) << st.op_mode_specific ; // } - // LOG(INFO) << " Motor ID " << int(this->node_id_) << " mode = " << motor_status->run_mode() ; + // CMVR_LOG(INFO) << " Motor ID " << int(this->node_id_) << " mode = " << motor_status->run_mode() ; diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.cpp index e4fd5f3c..7907fdc6 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.cpp +++ b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.cpp @@ -3,14 +3,14 @@ // #include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h" -#include "glog/logging.h" +#include "common/base/logging/logger.h" using namespace cmvr::device::motor; void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const { if (length < 8) { - LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length; + CMVR_LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length; return; } @@ -27,7 +27,7 @@ void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot auto angle_rad = (motor_status->position() * 360.0) / (gearRatio * 65536.0 * radToDeg); - // LOG(INFO) << " Motor ID " << int(this->node_id_) << " pos = " << angle_rad << " rad speed = " << speed << " rad/s"; + // CMVR_LOG(INFO) << " Motor ID " << int(this->node_id_) << " pos = " << angle_rad << " rad speed = " << speed << " rad/s"; diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp index b4a2b544..0b9a5eda 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp +++ b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by lgv on 2025/8/1. // @@ -30,7 +31,7 @@ Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptrAddMessage(nmt_command_->ID, nmt_command_, true); @@ -38,7 +39,7 @@ Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr *>( message_manager_->GetMutableProtocolDataById(SyncProtocol::ID)); if (sync_command_ == nullptr) { - LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; + CMVR_LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; } can_sender_->AddMessage(sync_command_->ID, sync_command_, false); } @@ -64,7 +65,7 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { message_manager_->GetMutableProtocolDataById(SdoRequestProtocol::ID(node_id))); if (sdo_commands_[node_id] == nullptr) { - LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!"; + CMVR_LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!"; return ErrorCode::CANBUS_ERROR; } can_sender_->AddMessage(sdo_commands_[node_id]->ID(), sdo_commands_[node_id], true); @@ -75,7 +76,7 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { message_manager_->GetMutableProtocolDataById(motor::Ti5MotorRPDO1::ID(node_id))); if (rpdo1_commands_[node_id] == nullptr) { - LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!"; + CMVR_LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!"; return ErrorCode::CANBUS_ERROR; } can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true); @@ -85,7 +86,7 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { message_manager_->GetMutableProtocolDataById(motor::Ti5MotorRPDO2::ID(node_id))); if (rpdo2_commands_[node_id] == nullptr) { - LOG(ERROR) << "Ti5 Motor RPDO2 Protocol does not exist in the MessageManager!"; + CMVR_LOG(ERROR) << "Ti5 Motor RPDO2 Protocol does not exist in the MessageManager!"; return ErrorCode::CANBUS_ERROR; } can_sender_->AddMessage(rpdo2_commands_[node_id]->ID(), rpdo2_commands_[node_id], true); @@ -437,7 +438,7 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { if (!waitUntil([&]() { return GetRobotDetail()->motors().at(node_id).position_offset() == 0; }, 1000)) { - LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed"; + CMVR_LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed"; return false; } @@ -458,7 +459,7 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos; }, 500)) { return false; - LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed"; + CMVR_LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed"; } return true; diff --git a/cmvr-es/devices/motor/ti5_motor/ti5_motor.h b/cmvr-es/devices/motor/ti5_motor/ti5_motor.h index cfc9bde9..aec1ab53 100644 --- a/cmvr-es/devices/motor/ti5_motor/ti5_motor.h +++ b/cmvr-es/devices/motor/ti5_motor/ti5_motor.h @@ -3,8 +3,10 @@ // #include "../abstract_motor.h" +#include "common/base/logging/logger.h" #include "motor/motor_protocol_interface.h" #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" +#include "cmvr/config/motor_config/motor_config.pb.h" /** * class: Ti5Motor 完成电机硬件的相关工作，软件相关的在protocol 中完成 @@ -13,21 +15,24 @@ namespace cmvr { namespace device { class Ti5Motor : public AbstractMotor { public: - Ti5Motor(const XmlNode &config,uint8_t node_id):AbstractMotor(config,node_id){} - Ti5Motor(const XmlNode &config):AbstractMotor(config) { - info_.id = config.getAttrDefault("id", -1); - info_.joint_name = config.getAttrString("jointName"); - info_.limitQ = config.getAttrDefault("limitQ", 3.14f); - info_.limitQd = config.getAttrDefault("limitQd", 0.5f); + explicit Ti5Motor(const config::Ti5MotorConfig &config) { + info_.id = config.id(); + info_.joint_name = config.joint_name(); + info_.limit_q_lb = config.limit_q_lb(); + info_.limit_q_ub = config.limit_q_ub(); + info_.limit_qd = config.limit_qd() > 0.0 ? config.limit_qd() : 0.5; node_id_ = info_.id; } + std::string typeName() const override { return "Ti5Motor"; } + /** * 电机初始化在具体的类中完成 */ - void init() override { + bool init() override { if (!protocol_) { - throw std::runtime_error("Protocol not set for motor"); + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; } if (protocol_->comm_proto == MotorProtocolInterface::CommProto::CANOPEN ) { auto canopen_protocol = std::dynamic_pointer_cast(protocol_); @@ -41,11 +46,13 @@ namespace cmvr { canopen_protocol->setMode(node_id_,msgs::RUN_MODE_CYCLIC_SYNC_POSITION); // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15); // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15); - canopen_protocol->setLimitQd(node_id_,5.0); + canopen_protocol->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb); + canopen_protocol->setLimitQd(node_id_, info_.limit_qd); canopen_protocol->setLimitQdd(node_id_,10.0,-10.0); // canopen_protocol->torqueOff(node_id_); } + return true; } }; diff --git a/cmvr-es/devices/robot/CMakeLists.txt b/cmvr-es/devices/robot/CMakeLists.txt index 9ca43ed8..db04a476 100644 --- a/cmvr-es/devices/robot/CMakeLists.txt +++ b/cmvr-es/devices/robot/CMakeLists.txt @@ -1,4 +1,3 @@ #add_subdirectory(ti5_robot) -add_subdirectory(humanoid_robot) #add_subdirectory(c701) #add_subdirectory(aubo_robot) diff --git a/cmvr-es/devices/robot/abstract_robot.h b/cmvr-es/devices/robot/abstract_robot.h index 5759c3af..fc127847 100644 --- a/cmvr-es/devices/robot/abstract_robot.h +++ b/cmvr-es/devices/robot/abstract_robot.h @@ -8,67 +8,47 @@ #include "json/json.h" #include "../abstract_device.h" -#include "../../utils/controller/include/cartesian_controller.h" -#include "planner/cartesian_space_planner/include/cartesian_twist_limiter.h" -#include "cmvr/msgs/geometry.pb.h" +#include "common/types/arm/arm_types.h" +#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" +#include "cmvr/common/geometry.pb.h" #include "cmvr/msgs/motor.pb.h" +#include "common/base/logging/logger.h" #include namespace cmvr::device{ - struct JointPoint{ - std::string joint_name; - double rad; // rad - double vel; // rad / s - - JointPoint(const std::string& name, double r, double v) - : joint_name(name), rad(r), vel(v) {} - JointPoint(const std::string& name, double r) - : joint_name(name), rad(r), vel(0) {} - JointPoint() - : joint_name(""), rad(0), vel(0) {} - } ; - - typedef struct { - std::string joint_name; - double vel; // rad / s - } JointVelocityCommand; - - typedef struct { - std::string joint_name; - double current; // - } JointCurrentCommand; - class AbstractRobot: public AbstractDevice { public: - explicit AbstractRobot(const XmlNode &config): AbstractDevice(config) {} + AbstractRobot() = default; ~AbstractRobot() override=default; - virtual int getDOF() { throw std::runtime_error("Not implemented"); } + DeviceKind kind() const noexcept override { return DeviceKind::Robot; } - virtual std::vector getJointNames() { throw std::runtime_error("Not implemented"); } + virtual int getDOF() { CMVR_LOG(ERROR) << "[AbstractRobot] getDOF is not implemented"; return 0; } - virtual std::unordered_map getJointQ() const { throw std::runtime_error("Not implemented"); } + virtual std::vector getJointNames() { CMVR_LOG(ERROR) << "[AbstractRobot] getJointNames is not implemented"; return {}; } + + virtual std::unordered_map getJointQ() const { CMVR_LOG(ERROR) << "[AbstractRobot] getJointQ is not implemented"; return {}; } /** * * @param joint_qs 函数会根据 joint_qs 的 joint name 去获取值 */ - virtual void getJointQ(std::unordered_map &joint_qs) const {throw std::runtime_error("Not implemented");} - virtual std::vector getLinkNames() { throw std::runtime_error("Not implemented"); } + virtual void getJointQ(std::unordered_map &joint_qs) const { CMVR_LOG(ERROR) << "[AbstractRobot] getJointQ(out) is not implemented"; joint_qs.clear(); } + virtual std::vector getLinkNames() { CMVR_LOG(ERROR) << "[AbstractRobot] getLinkNames is not implemented"; return {}; } virtual void getJointsState(std::vector& states) = 0; - virtual void getState(RobotState &state) { throw std::runtime_error("Not implemented"); } + virtual void getState(RobotState &state) { CMVR_LOG(ERROR) << "[AbstractRobot] getState is not implemented"; state = RobotState{}; } - virtual math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");} + virtual math::Pose3d getTransform(std::string &bask_link, std::string &target_link) { CMVR_LOG(ERROR) << "[AbstractRobot] getTransform is not implemented"; return {}; } - virtual void torqueOn() { throw std::runtime_error("Not implemented"); } - virtual void torqueOn(const std::string &joint_name) { throw std::runtime_error("Not implemented"); } + virtual void torqueOn() { CMVR_LOG(ERROR) << "[AbstractRobot] torqueOn is not implemented"; } + virtual void torqueOn(const std::string &joint_name) { CMVR_LOG(ERROR) << "[AbstractRobot] torqueOn(joint) is not implemented: " << joint_name; } - virtual void torqueOff() { throw std::runtime_error("Not implemented"); } - virtual void torqueOff(const std::string &joint_name) { throw std::runtime_error("Not implemented"); } + virtual void torqueOff() { CMVR_LOG(ERROR) << "[AbstractRobot] torqueOff is not implemented"; } + virtual void torqueOff(const std::string &joint_name) { CMVR_LOG(ERROR) << "[AbstractRobot] torqueOff(joint) is not implemented: " << joint_name; } - virtual void eStop() { throw std::runtime_error("Not implemented"); } + virtual void eStop() { CMVR_LOG(ERROR) << "[AbstractRobot] eStop is not implemented"; } /** * @brief 关节空间 point-to-point 运动（内部轨迹规划）。 @@ -77,7 +57,7 @@ namespace cmvr::device{ * @param max_vel 主导轴最大速度，单位 rad/s。 * @param max_acc 主导轴最大加速度，单位 rad/s^2。 */ - virtual void moveJ(std::vector &joints, double max_vel=0.5, double max_acc=0.1) { throw std::runtime_error("Not implemented"); } + virtual void moveJ(std::vector &joints, double max_vel=0.5, double max_acc=0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] moveJ(vector) is not implemented"; } /** * @brief 按关节命令执行关节空间运动（内部轨迹规划）。 @@ -85,9 +65,9 @@ namespace cmvr::device{ * @param vel 主导轴最大速度，单位 rad/s。 * @param acc 主导轴最大加速度，单位 rad/s^2。 */ - virtual void moveJ(std::vector &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } - virtual void speedJ(std::vector &cmd) { throw std::runtime_error("Not implemented"); } - virtual void speedJ(double vel) { throw std::runtime_error("Not implemented"); } + virtual void moveJ(std::vector &cmd, double vel=0.5, double acc=0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] moveJ(vector) is not implemented"; } + virtual void speedJ(std::vector &cmd) { CMVR_LOG(ERROR) << "[AbstractRobot] speedJ(commands) is not implemented"; } + virtual void speedJ(double vel) { CMVR_LOG(ERROR) << "[AbstractRobot] speedJ(double) is not implemented"; } /** * @brief 末端位姿目标的关节空间运动。 * @details 典型实现为先做 IK 求解关节目标，再按 `moveJ` 语义执行（关节空间规划与同步）。 @@ -97,12 +77,10 @@ namespace cmvr::device{ * @param vel 主导轴最大速度，单位 rad/s。 * @param acc 主导轴最大加速度，单位 rad/s^2。 */ - virtual void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); } - virtual void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); } + virtual void moveJ(const std::string &base_link, const std::string &ee_link, cmvr::common::Pose3d pose,double vel = 0.5, double acc = 0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] moveJ(pose) is not implemented"; } + virtual void moveDeltaJ(const std::string &base_link, const std::string &ee_link, cmvr::common::Pose3d delta_pose,double vel = 0.5, double acc = 0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] moveDeltaJ is not implemented"; } - virtual void moveJ_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } - - virtual void moveJ_IK(const std::string &base_link, const std::vector &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } + virtual void moveJ_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] moveJ_IK is not implemented"; } virtual bool moveL(const std::vector &pose, double speed = 0.25, @@ -110,28 +88,28 @@ namespace cmvr::device{ double jerk = 5.0, const std::vector &qd_max = std::vector(7, 2.5), bool asynchronous = false) { - throw std::runtime_error("Not implemented"); + CMVR_LOG(ERROR) << "[AbstractRobot] moveL is not implemented"; + return false; } virtual bool speedL(const std::vector &xd, double acceleration = 0.25, double time = 0.0, cmvr::CartesianFrame frame = cmvr::CartesianFrame::Base) { - throw std::runtime_error("Not implemented"); + CMVR_LOG(ERROR) << "[AbstractRobot] speedL is not implemented"; + return false; } virtual Eigen::Matrix getSpeedLCommandTwistBase() { return Eigen::Matrix::Zero(); } - virtual void stopSpeedL() { throw std::runtime_error("Not implemented"); } + virtual void stopSpeedL() { CMVR_LOG(ERROR) << "[AbstractRobot] stopSpeedL is not implemented"; } - virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); } + virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { CMVR_LOG(ERROR) << "[AbstractRobot] speedJ(joint) is not implemented: " << joint_name; } - virtual void followJointTrajectory(std::vector> &traj, double dt) { throw std::runtime_error("Not implemented"); } + virtual void followJointTrajectory(std::vector> &traj, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] followJointTrajectory(double) is not implemented"; } - virtual void followJointTrajectory(std::vector> &traj, double dt) { throw std::runtime_error("Not implemented"); } + virtual void followJointTrajectory(std::vector> &traj, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] followJointTrajectory(JointPoint) is not implemented"; } - virtual void followPoseTrajectory(std::vector &traj, double dt) { throw std::runtime_error("Not implemented"); } - - virtual void followPoseTrajectory(std::string &base_link, std::vector> &targets, double dt) { throw std::runtime_error("Not implemented"); } + virtual void followPoseTrajectory(std::vector &traj, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] followPoseTrajectory is not implemented"; } /** * @brief 关节空间周期伺服（无整段轨迹规划）。 @@ -139,14 +117,14 @@ namespace cmvr::device{ * @param joints 当前周期目标关节角，单位 rad。 * @param dt 当前伺服命令生效时长（控制周期），单位 s。 */ - virtual void servoJ(std::vector &joints, double dt) { throw std::runtime_error("Not implemented"); } + virtual void servoJ(std::vector &joints, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] servoJ(vector) is not implemented"; } /** * @brief 按关节命令执行周期伺服（无整段轨迹规划）。 * @param joints 当前周期关节目标命令。 * @param dt 当前伺服命令生效时长（控制周期），单位 s。 */ - virtual void servoJ(std::vector &joints, double dt) { throw std::runtime_error("Not implemented"); } + virtual void servoJ(std::vector &joints, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] servoJ(vector) is not implemented"; } /** * @brief 带速度参数的关节空间周期伺服。 @@ -154,7 +132,7 @@ namespace cmvr::device{ * @param vel 速度参数（语义由具体驱动实现决定）。 * @param dt 当前伺服命令生效时长（控制周期），单位 s。 */ - virtual void servoJ(std::vector &joints, double vel, double dt) { throw std::runtime_error("Not implemented"); } + virtual void servoJ(std::vector &joints, double vel, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] servoJ(vector, vel) is not implemented"; } /** * @brief 基于末端位姿目标的周期伺服。 @@ -165,13 +143,11 @@ namespace cmvr::device{ * @param vel 速度参数（语义由具体驱动实现决定）。 * @param acc 加速度参数（语义由具体驱动实现决定）。 */ - virtual void servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); } - virtual void servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); } + virtual void servoJ(const std::string &base_link, const std::string &ee_link, cmvr::common::Pose3d pose,double vel = 0.1, double acc = 0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] servoJ(pose) is not implemented"; } + virtual void servoDeltaJ(const std::string &base_link, const std::string &ee_link, cmvr::common::Pose3d delta_pose,double vel = 0.1, double acc = 0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] servoDeltaJ is not implemented"; } - virtual void servoL(math::Pose3d &pose, double dt) { throw std::runtime_error("Not implemented"); } - - virtual void servoL(std::string &base_link, std::vector &targets, double dt) { throw std::runtime_error("Not implemented"); } + virtual void servoL(math::Pose3d &pose, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] servoL is not implemented"; } virtual void calibrateZeroQ(const std::string &joint_name) = 0; @@ -181,9 +157,9 @@ namespace cmvr::device{ } virtual std::string getToolFrame() const { return toolFrame_; } - virtual msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0; - virtual msgs::Pose3d fk(bool is_tcp = true) = 0; - virtual std::vector ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) = 0; + virtual cmvr::common::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0; + virtual cmvr::common::Pose3d fk(bool is_tcp = true) = 0; + virtual std::vector ik(const std::string &base_link, const std::string &ee_link,cmvr::common::Pose3d pose) = 0; protected: int dof_{}; RobotState state_{}; diff --git a/cmvr-es/devices/robot/aubo_robot/CMakeLists.txt b/cmvr-es/devices/robot/aubo_robot/CMakeLists.txt index 1e16c41d..a70b9487 100644 --- a/cmvr-es/devices/robot/aubo_robot/CMakeLists.txt +++ b/cmvr-es/devices/robot/aubo_robot/CMakeLists.txt @@ -28,7 +28,8 @@ target_link_libraries(aubo_robot PRIVATE aubo_sdk robot_proxy + cmvr_es::proto ) # 别名（不变） -add_library(cmvr_es::device::aubo_robot ALIAS aubo_robot) \ No newline at end of file +add_library(cmvr_es::device::aubo_robot ALIAS aubo_robot) diff --git a/cmvr-es/devices/robot/aubo_robot/include/aubo_robot.h b/cmvr-es/devices/robot/aubo_robot/include/aubo_robot.h index 777ba1cc..9277e2de 100644 --- a/cmvr-es/devices/robot/aubo_robot/include/aubo_robot.h +++ b/cmvr-es/devices/robot/aubo_robot/include/aubo_robot.h @@ -6,6 +6,7 @@ #define CMVR_ES_AUBO_ROBOT_H #include "devices/robot/abstract_robot.h" #include "aubo_sdk/rpc.h" +#include "cmvr/config/aubo_robot_config/aubo_robot_config.pb.h" #include "cmvr/msgs/robot_detail.pb.h" using namespace arcs::common_interface; using namespace arcs::aubo_sdk; @@ -14,9 +15,11 @@ namespace cmvr::device template class AuboRobot: public AbstractRobot { public: - explicit AuboRobot(const XmlNode& cfg); + explicit AuboRobot(const config::AuboRobotConfig& cfg); ~AuboRobot(); - void init () override; + + std::string typeName() const override { return "AuboRobot"; } + bool init () override; void torqueOn() override; void torqueOff() override; @@ -28,8 +31,8 @@ namespace cmvr::device void moveL(math::Pose3d& pose, double vel, double acc) override; void calibrateZeroQ(const std::string& joint_name) override; - msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) override; - std::vector ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) override; + cmvr::common::Pose3d fk(const std::string &base_link, const std::string &ee_link) override; + std::vector ik(const std::string &base_link, const std::string &ee_link,cmvr::common::Pose3d pose) override; private: static void waitForRobotMode(const RobotInterfacePtr& robot_interface, const RobotModeType& target_mode); @@ -45,4 +48,4 @@ namespace cmvr::device -#endif //CMVR_ES_AUBO_ROBOT_H \ No newline at end of file +#endif //CMVR_ES_AUBO_ROBOT_H diff --git a/cmvr-es/devices/robot/aubo_robot/src/aubo_robot.cpp b/cmvr-es/devices/robot/aubo_robot/src/aubo_robot.cpp index 797f0183..f8c0e4b6 100644 --- a/cmvr-es/devices/robot/aubo_robot/src/aubo_robot.cpp +++ b/cmvr-es/devices/robot/aubo_robot/src/aubo_robot.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by linbo on 2025/11/7. // @@ -7,16 +8,16 @@ using namespace std; using namespace cmvr::device; template -AuboRobot::AuboRobot(const XmlNode &cfg) : AbstractRobot(cfg) +AuboRobot::AuboRobot(const config::AuboRobotConfig &cfg) { try { - id_ = cfg.getAttrString("id"); - ip_ = cfg.getAttrString("ip"); - port_ = cfg.getAttrDefault("port",30004); - username_ = cfg.getAttrString("username"); - password_ = cfg.getAttrString("password"); + id_ = cfg.id(); + ip_ = cfg.ip(); + port_ = cfg.port() > 0 ? cfg.port() : 30004; + username_ = cfg.username(); + password_ = cfg.password(); } catch (std::exception &e) { - throw runtime_error(e.what()); + CMVR_LOG(ERROR) << "[AuboRobot] Failed to parse config: " << e.what(); } } @@ -34,17 +35,24 @@ AuboRobot::~AuboRobot() } template -void AuboRobot::init () +bool AuboRobot::init () { - //初始化AuboSDK - rpc_cli_ = std::make_shared(); + try { + //初始化AuboSDK + rpc_cli_ = std::make_shared(); - // 接口调用: 设置 RPC 超时 - rpc_cli_->setRequestTimeout(1000); - // 接口调用: 连接到 RPC 服务 - rpc_cli_->connect(ip_, port_); - // 接口调用: 登录 - rpc_cli_->login(username_, password_); + // 接口调用: 设置 RPC 超时 + rpc_cli_->setRequestTimeout(1000); + // 接口调用: 连接到 RPC 服务 + rpc_cli_->connect(ip_, port_); + // 接口调用: 登录 + rpc_cli_->login(username_, password_); + return true; + } catch (const std::exception& e) { + CMVR_LOG(ERROR) << "[AuboRobot] init failed: " << e.what(); + rpc_cli_.reset(); + return false; + } } template void AuboRobot::waitForRobotMode(const RobotInterfacePtr& robot_interface, @@ -54,7 +62,7 @@ void AuboRobot::waitForRobotMode(const RobotInterfacePtr& robot_interface, auto current_mode = robot_interface->getRobotState()->getRobotModeType(); while (current_mode != target_mode) { - std::cout << "机械臂当前模式:" << current_mode << std::endl; + CMVR_LOG(INFO) << "机械臂当前模式:" << current_mode; std::this_thread::sleep_for(std::chrono::seconds(1)); current_mode = robot_interface->getRobotState()->getRobotModeType(); } @@ -78,7 +86,7 @@ void AuboRobot::torqueOn() auto robot_mode = robot_interface->getRobotState()->getRobotModeType(); if (robot_mode == RobotModeType::Running) { - std::cout << "机械臂已松刹车，处于运行模式" << std::endl; + CMVR_LOG(INFO) << "机械臂已松刹车，处于运行模式"; } else { // 接口调用: 机械臂发起上电请求 @@ -87,9 +95,9 @@ void AuboRobot::torqueOn() // 等待机械臂进入空闲模式 waitForRobotMode(robot_interface, RobotModeType::Idle); - std::cout << "机械臂上电成功，当前模式:" + CMVR_LOG(INFO) << "机械臂上电成功，当前模式:" << robot_interface->getRobotState()->getRobotModeType() - << std::endl; + ; // 接口调用: 机械臂发起松刹车请求 rpc_cli_->getRobotInterface(robot_name)->getRobotManage()->startup(); @@ -97,9 +105,9 @@ void AuboRobot::torqueOn() // 等待机械臂进入运行模式 waitForRobotMode(robot_interface, RobotModeType::Running); - std::cout << "机械臂松刹车成功，当前模式:" + CMVR_LOG(INFO) << "机械臂松刹车成功，当前模式:" << robot_interface->getRobotState()->getRobotModeType() - << std::endl; + ; } } template @@ -120,7 +128,8 @@ void AuboRobot::getJointsState(std::vector& states) states.push_back(state); } } catch (exception &e) { - throw runtime_error(e.what()); + CMVR_LOG(ERROR) << "[AuboRobot] getJointsState failed: " << e.what(); + states.clear(); } } @@ -139,9 +148,9 @@ void AuboRobot::torqueOff() // 等待机械臂进入断电模式 waitForRobotMode(robot_interface, RobotModeType::PowerOff); - std::cout << "机械臂断电成功，当前模式:" + CMVR_LOG(INFO) << "机械臂断电成功，当前模式:" << robot_interface->getRobotState()->getRobotModeType() - << std::endl; + ; } template void AuboRobot::moveJ(std::vector& cmd, double vel, double acc) @@ -151,8 +160,8 @@ void AuboRobot::moveJ(std::vector& cmd, double vel, double acc) //检查参数 if (cmd.size() != dof_) { - LOG(ERROR) << "[AuboRobot](moveJ)Wrong number of cmd"; - throw std::invalid_argument("[AuboRobot](moveJ)Wrong number of cmd"); + CMVR_LOG(ERROR) << "[AuboRobot](moveJ)Wrong number of cmd"; + return; } //调用aubo sdk接口实现moveJ @@ -175,7 +184,7 @@ void AuboRobot::moveJ(std::vector& cmd, double vel, double acc) } catch (std::exception& e) { - throw std::invalid_argument(e.what()); + CMVR_LOG(ERROR) << "[AuboRobot] moveJ failed: " << e.what(); } } @@ -186,16 +195,16 @@ void AuboRobot::calibrateZeroQ(const std::string& joint_name) } template -cmvr::msgs::Pose3d AuboRobot::fk(const std::string &base_link, const std::string &ee_link) +cmvr::common::Pose3d AuboRobot::fk(const std::string &base_link, const std::string &ee_link) { - cmvr::msgs::Pose3d pose; + cmvr::common::Pose3d pose; return pose; } template -std::vector AuboRobot::ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) +std::vector AuboRobot::ik(const std::string &base_link, const std::string &ee_link,cmvr::common::Pose3d pose) { std::vector ik; @@ -253,9 +262,9 @@ void AuboRobot::moveL(math::Pose3d& pose, double vel, double acc) // 阻塞 auto ret = waitArrival(robot_interface); if (ret == 0) { - LOG(INFO)<<"直线运动到位置成功！"; + CMVR_LOG(INFO)<<"直线运动到位置成功！"; } else { - LOG(INFO)<<"直线运动到位置失败！"; + CMVR_LOG(INFO)<<"直线运动到位置失败！"; } } diff --git a/cmvr-es/devices/robot/c701/motor/motor_controller.cpp b/cmvr-es/devices/robot/c701/motor/motor_controller.cpp index 5586d6fc..fa113c64 100644 --- a/cmvr-es/devices/robot/c701/motor/motor_controller.cpp +++ b/cmvr-es/devices/robot/c701/motor/motor_controller.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by lgv on 2025/7/17. // @@ -17,8 +18,14 @@ using namespace cmvr::msgs; using namespace cmvr::msgs; ErrorCode MotorController::Init(cmvr::device::AbstractCanbus *can_client, bool enable_log) { - can_client->init(); - can_client->start(); + if (!can_client->init()) { + CMVR_LOG(ERROR) << "[MotorController] CAN client init failed"; + return ErrorCode::CAN_CLIENT_ERROR_BASE; + } + if (!can_client->start()) { + CMVR_LOG(ERROR) << "[MotorController] CAN client start failed"; + return ErrorCode::CAN_CLIENT_ERROR_BASE; + } auto ret = ErrorCode::OK; @@ -50,7 +57,7 @@ ErrorCode MotorController::Init(cmvr::device::AbstractCanbus *can_client, bool e can_sender_ = std::make_shared >(); ret = can_sender_->Init(can_client, enable_log); if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to init can sender."; + CMVR_LOG(ERROR) << "Failed to init can sender."; return ret; } @@ -59,7 +66,7 @@ ErrorCode MotorController::Init(cmvr::device::AbstractCanbus *can_client, bool e can_receiver_ = std::make_shared >(); ret = can_receiver_->Init(can_client, message_manager_.get(), enable_log); if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to init can receiver."; + CMVR_LOG(ERROR) << "Failed to init can receiver."; return ret; } @@ -72,7 +79,7 @@ ErrorCode MotorController::Init(cmvr::device::AbstractCanbus *can_client, bool e if (nmt_command_ == nullptr) { - LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; + CMVR_LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; return ErrorCode::CANBUS_ERROR; } can_sender_->AddMessage(nmt_command_->ID, nmt_command_, true); @@ -83,7 +90,7 @@ ErrorCode MotorController::Init(cmvr::device::AbstractCanbus *can_client, bool e if (sync_command_ == nullptr) { - LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; + CMVR_LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; return ErrorCode::CANBUS_ERROR; } // can_sender_->AddMessage(sync_command_->ID, sync_command_, false); @@ -95,7 +102,7 @@ ErrorCode MotorController::Init(cmvr::device::AbstractCanbus *can_client, bool e message_manager_->GetMutableProtocolDataById(SdoRequestProtocol::ID(node_id))); if (sdo_commands_[node_id] == nullptr) { - LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!"; + CMVR_LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!"; return ErrorCode::CANBUS_ERROR; } can_sender_->AddMessage(sdo_commands_[node_id]->ID(), sdo_commands_[node_id], true); @@ -107,7 +114,7 @@ ErrorCode MotorController::Init(cmvr::device::AbstractCanbus *can_client, bool e message_manager_->GetMutableProtocolDataById(Ti5MotorRPDO1::ID(node_id))); if (rpdo1_commands_[node_id] == nullptr) { - LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!"; + CMVR_LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!"; return ErrorCode::CANBUS_ERROR; } can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true); @@ -116,7 +123,7 @@ ErrorCode MotorController::Init(cmvr::device::AbstractCanbus *can_client, bool e // need sleep to ensure all messages received - LOG(INFO) << "Motor Controller is initialized."; + CMVR_LOG(INFO) << "Motor Controller is initialized."; return ErrorCode::OK; @@ -126,13 +133,13 @@ ErrorCode MotorController::Start() { auto ret = ErrorCode::OK; ret = can_sender_->Start(); if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to start can sender."; + CMVR_LOG(ERROR) << "Failed to start can sender."; return ret; } ret = can_receiver_->Start(); if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to start can receiver."; + CMVR_LOG(ERROR) << "Failed to start can receiver."; return ret; } return ret; @@ -397,4 +404,3 @@ void MotorController::SeedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, } - diff --git a/cmvr-es/devices/robot/c701/motor/motor_controller_test.cpp b/cmvr-es/devices/robot/c701/motor/motor_controller_test.cpp index 3e695e17..f060c132 100644 --- a/cmvr-es/devices/robot/c701/motor/motor_controller_test.cpp +++ b/cmvr-es/devices/robot/c701/motor/motor_controller_test.cpp @@ -35,8 +35,9 @@ TEST(MotorControllerTest, MotorControllerCmdTest) { - XmlNode xml_node; - SocketCanClientRaw can_client(xml_node); + cmvr::config::SocketCanConfig can_config; + can_config.set_channel_id(0); + SocketCanClientRaw can_client(can_config); can_client.start(); @@ -83,4 +84,4 @@ TEST(MotorControllerTest, MotorControllerCmdTest) { EXPECT_EQ(result, ErrorCode::CANBUS_ERROR); -} \ No newline at end of file +} diff --git a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp index 89b3f5c1..e5025791 100644 --- a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp +++ b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp @@ -3,7 +3,7 @@ // #include "robot/c701/motor/protocol/ti5_motor_rpdo1.h" -#include "glog/logging.h" +#include "common/base/logging/logger.h" using namespace cmvr::msgs; using namespace cmvr::device; diff --git a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp index 84d97bd2..fc9e020a 100644 --- a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp +++ b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by lgv on 2025/7/24. // @@ -27,11 +28,11 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response, break; case msgs::ACTUAL_POSITION_6064: motor_status->set_position(static_cast(sdo_response.data())); - LOG(INFO) << "pos = " << motor_status->position(); + CMVR_LOG(INFO) << "pos = " << motor_status->position(); break; } - // LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_) + // CMVR_LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_) // << ": command=" << int(sdo_response.cs()) // << ", index=" << sdo_response.index() // << ", subindex=" << int(sdo_response.sub_index()) diff --git a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp index f053e4e4..0d2bcef0 100644 --- a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp +++ b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp @@ -3,7 +3,7 @@ // #include "ti5_motor_tpdo1.h" -#include "glog/logging.h" +#include "common/base/logging/logger.h" #include "canbus/canopen/register.h" using namespace cmvr::msgs; using namespace cmvr::device; @@ -12,7 +12,7 @@ using namespace cmvr::robot::motor; void Ti5MotorTPDO1::Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const { if (length < 7) { - LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length; + CMVR_LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length; return; } @@ -28,7 +28,7 @@ void Ti5MotorTPDO1::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot st.value = motor_status->status_word(); if (st.op_mode_specific > 0) { - LOG(INFO) << st.op_mode_specific ; + CMVR_LOG(INFO) << st.op_mode_specific ; } diff --git a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp index 50798341..3716f96c 100644 --- a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp +++ b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp @@ -3,7 +3,7 @@ // #include "ti5_motor_tpdo2.h" -#include "glog/logging.h" +#include "common/base/logging/logger.h" using namespace cmvr::msgs; using namespace cmvr::device; @@ -12,7 +12,7 @@ using namespace cmvr::robot::motor; void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const { if (length < 8) { - LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length; + CMVR_LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length; return; } @@ -22,5 +22,5 @@ void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot motor_status->set_position(bytes[3] << 24 | bytes[2] << 16 | bytes[1] << 8 | bytes[0]); motor_status->set_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]); - LOG(INFO) << "Motor ID " << this->node_id_ << "pos = " << motor_status->position() << " speed = " << motor_status->speed(); + CMVR_LOG(INFO) << "Motor ID " << this->node_id_ << "pos = " << motor_status->position() << " speed = " << motor_status->speed(); } diff --git a/cmvr-es/devices/robot/humanoid_robot/CMakeLists.txt b/cmvr-es/devices/robot/humanoid_robot/CMakeLists.txt deleted file mode 100644 index 76bae1b3..00000000 --- a/cmvr-es/devices/robot/humanoid_robot/CMakeLists.txt +++ /dev/null @@ -1,47 +0,0 @@ -add_library(humanoid_robot SHARED src/humanoid_robot.cpp) - - -target_include_directories(humanoid_robot PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -add_library(cmvr_es::device::humanoid_robot ALIAS humanoid_robot) - -target_link_libraries(humanoid_robot PRIVATE - cmvr_es::utils - cmvr_es::data_center - protobuf::libprotobuf - cmvr_es::ik_solver - cmvr_es::planner - toppra - glog - osqp -) - -install(TARGETS humanoid_robot LIBRARY DESTINATION lib) - - -# -------------------------------------------------------- -# Unit test -# -------------------------------------------------------- -add_executable(humanoid_robot_test - src/humanoid_robot_test.cpp -) - -target_link_libraries(humanoid_robot_test - PRIVATE - cmvr_es::device_manager - cmvr_es::controller - cmvr_es::common - cmvr_es::data_center - osqp - cmvr_es::device::humanoid_robot - gtest - gtest_main - pthread - glog - matplot - cmvr_es::proto - ${OpenCV_LIBS} - ccd - fcl - -) diff --git a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h deleted file mode 100644 index 7770997b..00000000 --- a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h +++ /dev/null @@ -1,241 +0,0 @@ -// -// Created by xtkuang on 2025/7/24. -// - -#ifndef CMVR_ES_HUMANOID_ROBOT_H -#define CMVR_ES_HUMANOID_ROBOT_H - -#include "../../abstract_robot.h" -#include "../../../../utils/controller/include/cartesian_controller.h" -#include "../../../../utils/base/include/os.h" -#include "../../../../utils/base/include/timer.h" -#include "../../../../utils/base/include/ring_buffer.h" - -#include "../../../canbus/abstract_canbus.h" -#include "canbus/can_comm/can_sender.h" -#include "canbus/can_comm/can_receiver.h" -#include "canbus/can_comm/message_manager.h" -#include "cmvr/msgs/robot_detail.pb.h" -#include "motor/motor_manager.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "planner/joint_space_planner/include/joint_space_planner_creator.h" -#include "planner/joint_space_planner/include/joint_space_planner.h" -#include "ik_solver/include/pinocchio_dls_ik_solver.h" - -namespace cmvr::device{ - - template - class HumanoidRobot: public AbstractRobot { - typedef enum { - ROBOT_DISABLED, - ROBOT_READY, - ROBOT_RUNNING, - ROBOT_ESTOP, - ROBOT_TOROFF, - ROBOT_ERROR - } RobotStateMachine; - -// typedef enum { -// PPM_MODE, PVM_MODE, PTM_MODE, CSP_MODE, CSV_MODE, CSC_MODE -// } RobotRunningMode; - - public: - explicit HumanoidRobot(const XmlNode& cfg); - ~HumanoidRobot(); - - void init() override; - - /* get robot state and info*/ - int getDOF() override; - std::vector getJointNames() override; - std::vector getLinkNames() override; - - std::unordered_map getJointQ() const override; - void getJointQ(std::unordered_map &joint_qs) const override; - - void getJointsState(std::vector& states) override; - void getState(RobotState &state) override; - math::Pose3d getTransform(std::string &bask_link, std::string &target_link) override; - - /* torque on and off */ - void torqueOn() override; - void torqueOn(const std::string &joint_name) override; - void torqueOff() override; - void torqueOff(const std::string &joint_name) override; - void calibrateZeroQ(const std::string &joint_name) override; - - /* robot basic command*/ - void eStop() override; - void moveJ(std::vector &cmd, double vel = 0.5, double acc = 0.1) override; - void speedJ(std::vector &cmd) override; - void speedJ(double vel) override; - void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel, double acc) override; - void moveJ_IK(const std::string &base_link, const std::vector &targets, double vel, double acc) override ; - void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override; - bool moveL(const std::vector &pose, - double speed = 0.25, - double acceleration = 1.2, - double jerk = 5.0, - const std::vector &qd_max = std::vector(7, 2.5), - bool asynchronous = false) override; - - - void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override; - - bool speedL(const std::vector &xd, - double acceleration = 0.25, - double time = 0.0, - cmvr::CartesianFrame frame = cmvr::CartesianFrame::Base) override; - void stopSpeedL() override; - Eigen::Matrix getSpeedLCommandTwistBase() override; - - - - - void servoJ(std::vector &joints, double dt) override; - void servoJ(std::vector &joints, double vel, double dt) override; - void servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, double acc) override; - void servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override; - void servoL(std::string &base_link, std::vector &targets, double dt) override; - - protected: - bool check_joint_traj_(std::vector> &traj, double dt); - void update_state_(); - int exec_PPM_cmd_(std::vector &ids, std::vector &pos, std::vector &vel, std::vector &acc); - int exec_PVM_cmd_(std::vector &ids, std::vector &vel, std::vector &acc); - int exec_PTM_cmd_(std::vector &ids, std::vector &acc); - int exec_CSP_cmd_(std::vector &ids, std::vector &pos, int dt); - int exec_CSV_cmd_(std::vector &ids, std::vector &vel, int dt); - int exec_CSC_cmd_(std::vector &ids, std::vector &cur, int dt); - void sendJointVelocityCommand_(const std::vector& cmd); - bool updateSpeedLAccelerationConfig_(double acceleration); - void ensureSpeedLWorkerStarted_(); - void stopSpeedLWorker_(); - void speedLWorkerLoop_(); - bool executeMoveLScurve_(const Eigen::Matrix4d& target_pose_input, - double v_tcp_max, - double a_tcp_max, - double j_tcp_max, - const std::vector& qd_max, - double dt_real, - bool is_tcp, - cmvr::CartesianFrame input_frame); - - - - - - // 添加这三个函数的声明 - bool computeJacobian(const std::shared_ptr>& state, - const std::string& base_link, - const std::string& ee_link, - Eigen::Matrix& J); - - Eigen::Vector3d getEndEffectorPosition(const std::shared_ptr>& state, - const std::string& ee_link); - - Eigen::Matrix3d getEndEffectorOrientation(const std::shared_ptr>& state, - const std::string& ee_link); - - - - public: - // 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Y→Y→Z - static Eigen::Matrix3d eulerZYXToRotationMatrix(double rx, double ry, double rz); - static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R); - msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) override; - msgs::Pose3d fk(bool is_tcp) override; - std::vector ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) override; - - - private: - std::string id_; - int upd_freq_; - std::shared_ptr upd_timer_; - std::atomic rsm_{ROBOT_DISABLED}; -// std::atomic rmode_{PPM_MODE}; - - std::shared_ptr> m_state_; - std::shared_ptr> m_robot_; - std::shared_ptr> m_cctrl_; - - std::vector joint_names_; - std::vector link_names_; - - std::atomic flash_cmd_{false}; - std::mutex state_mtx_{}; - std::mutex exec_mtx_{}; - - std::shared_ptr> CSP_buffer_; - std::shared_ptr> CSV_buffer_; - std::shared_ptr> CSC_buffer_; - - - struct ControlPoint { - Eigen::Vector joint_positions; - Eigen::Vector joint_velocities; - double timestamp; - ControlPoint() : timestamp(0.0) {} - ControlPoint(const Eigen::Vector& pos, const Eigen::Vector& vel, double time): joint_positions(pos), joint_velocities(vel), timestamp(time) {} - }; - // 线程间通信的队列和同步变量 - std::queue control_queue_; - std::mutex queue_mutex_; - std::condition_variable queue_cv_; - std::atomic trajectory_completed_{false}; - std::atomic control_thread_ready_{false}; - std::atomic stop_requested_{false}; - - // 线程对象 - std::unique_ptr trajectory_thread_; - std::unique_ptr control_thread_; - - - - - private: - - std::shared_ptr motor_manager_{nullptr}; - - std::shared_ptr joint_space_planner_{nullptr}; - std::shared_ptr ik_solver_{nullptr}; - PinocchioDlsIKSolver::SpeedLConfig speedl_config_{}; - std::unique_ptr speedl_thread_; - std::mutex speedl_mutex_; - std::condition_variable speedl_cv_; - std::atomic speedl_stop_requested_{false}; - bool speedl_command_active_{false}; - Eigen::Matrix speedl_target_twist_{Eigen::Matrix::Zero()}; - Eigen::Matrix speedl_last_command_twist_base_{Eigen::Matrix::Zero()}; - double speedl_target_acceleration_{0.25}; - cmvr::CartesianFrame speedl_target_frame_{cmvr::CartesianFrame::Base}; - double speedl_applied_acceleration_{0.25}; - std::uint64_t speedl_command_version_{0}; - - // 每个电机组的锁和执行状态 - std::mutex left_arm_mutex_, right_arm_mutex_, head_mutex_, waist_mutex_; - std::atomic is_left_arm_busy_{false}, is_right_arm_busy_{false}; - std::atomic is_head_busy_{false}, is_waist_busy_{false}; - - - }; - - -} - - -#endif //CMVR_ES_HUMANOID_ROBOT_H diff --git a/cmvr-es/devices/robot/humanoid_robot/joint_positions_1.csv b/cmvr-es/devices/robot/humanoid_robot/joint_positions_1.csv deleted file mode 100644 index 5f21dbbb..00000000 --- a/cmvr-es/devices/robot/humanoid_robot/joint_positions_1.csv +++ /dev/null @@ -1,201 +0,0 @@ -idx,q0, q1, q2, q3, q4, q5, q6,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R,pLx, pLy, pLz, pRx, pRy, pRz -0,-0.060915,-0.408236,-1.315908,-1.570092,1.967174,0.099621,0.174091,0.076603,1.162435,1.720999,1.810691,-2.709591,0.039061,0.356226,0.250063,0.349893,-0.099981,0.250748,-0.25,-0.1 -1,-0.060358,-0.401354,-1.32106,-1.576596,1.961162,0.102057,0.168042,0.079723,1.162522,1.719628,1.807672,-2.710127,0.039137,0.35604,0.250106,0.349963,-0.096917,0.251478,-0.249999,-0.099996 -2,-0.05968,-0.39443,-1.326281,-1.583948,1.955119,0.105347,0.16177,0.082918,1.162616,1.718223,1.804555,-2.71068,0.039224,0.355827,0.250116,0.349818,-0.093782,0.252227,-0.249999,-0.099996 -3,-0.059082,-0.387617,-1.331445,-1.5915,1.949159,0.108962,0.155437,0.086114,1.162716,1.71682,1.801428,-2.711233,0.039315,0.355606,0.250119,0.349573,-0.09065,0.252977,-0.249999,-0.099996 -4,-0.058629,-0.380953,-1.336515,-1.599206,1.943318,0.112876,0.149029,0.089308,1.16282,1.715421,1.798294,-2.711785,0.039412,0.355376,0.250119,0.349229,-0.087527,0.253727,-0.249999,-0.099996 -5,-0.058345,-0.374454,-1.341473,-1.607054,1.937613,0.117085,0.142542,0.0925,1.162929,1.714026,1.795153,-2.712338,0.039512,0.355139,0.250119,0.348787,-0.084416,0.254477,-0.249999,-0.099996 -6,-0.05825,-0.368135,-1.346304,-1.615036,1.932059,0.121585,0.135974,0.095689,1.163043,1.712635,1.792005,-2.712891,0.039617,0.354894,0.250118,0.348247,-0.08132,0.255227,-0.249999,-0.099996 -7,-0.058365,-0.362008,-1.350992,-1.623146,1.926671,0.126373,0.129323,0.098876,1.163161,1.711247,1.78885,-2.713444,0.039725,0.354641,0.250116,0.347611,-0.078242,0.255977,-0.249999,-0.099996 -8,-0.058709,-0.356088,-1.355519,-1.631377,1.921466,0.131446,0.122585,0.102061,1.163284,1.709863,1.785688,-2.713997,0.039839,0.354381,0.250113,0.346878,-0.075186,0.256727,-0.249999,-0.099996 -9,-0.059306,-0.350389,-1.359868,-1.639721,1.91646,0.136799,0.115756,0.105243,1.163411,1.708483,1.782519,-2.71455,0.039956,0.354113,0.250109,0.34605,-0.072155,0.257477,-0.249999,-0.099996 -10,-0.060176,-0.344926,-1.364019,-1.648171,1.911668,0.142428,0.108834,0.108424,1.163543,1.707107,1.779343,-2.715104,0.040077,0.353837,0.250104,0.345128,-0.06915,0.258227,-0.249999,-0.099996 -11,-0.061344,-0.339714,-1.367952,-1.656721,1.907109,0.148328,0.101812,0.111602,1.16368,1.705734,1.776159,-2.715657,0.040203,0.353553,0.250097,0.344111,-0.066176,0.258977,-0.249998,-0.099996 -12,-0.062969,-0.334882,-1.371537,-1.665188,1.9029,0.154381,0.094696,0.114728,1.163818,1.704388,1.773028,-2.7162,0.040328,0.353274,0.250106,0.343021,-0.063264,0.259714,-0.249998,-0.099994 -13,-0.06483,-0.330233,-1.374954,-1.673896,1.898871,0.160799,0.087449,0.1179,1.163963,1.703023,1.769834,-2.716753,0.040461,0.352976,0.2501,0.341822,-0.060361,0.260464,-0.249998,-0.099994 -14,-0.067019,-0.325859,-1.378112,-1.682702,1.895108,0.167483,0.080102,0.121072,1.164112,1.701662,1.766629,-2.717307,0.040599,0.35267,0.250087,0.340532,-0.057495,0.261214,-0.249998,-0.099994 -15,-0.069586,-0.321792,-1.380972,-1.691582,1.891642,0.174418,0.072639,0.124242,1.164266,1.700303,1.763418,-2.71786,0.040741,0.352355,0.250072,0.339154,-0.054671,0.261964,-0.249998,-0.099993 -16,-0.072563,-0.31805,-1.383506,-1.700525,1.888495,0.181599,0.065052,0.12741,1.164424,1.698949,1.760199,-2.718414,0.040887,0.352033,0.250055,0.337689,-0.051892,0.262714,-0.249998,-0.099993 -17,-0.075974,-0.314651,-1.38569,-1.709523,1.885686,0.189019,0.057333,0.130576,1.164587,1.697598,1.756973,-2.718968,0.041036,0.351703,0.250035,0.336137,-0.04916,0.263463,-0.249998,-0.099993 -18,-0.079841,-0.31161,-1.3875,-1.718567,1.883232,0.196671,0.049476,0.13374,1.164754,1.696251,1.75374,-2.719521,0.04119,0.351366,0.250014,0.334502,-0.046479,0.264213,-0.249998,-0.099993 -19,-0.084184,-0.308942,-1.388914,-1.727648,1.881153,0.204548,0.041475,0.136902,1.164925,1.694908,1.750499,-2.720075,0.041348,0.351021,0.24999,0.332783,-0.04385,0.264963,-0.249997,-0.099993 -20,-0.089019,-0.306661,-1.38991,-1.736757,1.879464,0.212643,0.033326,0.140062,1.1651,1.693569,1.747252,-2.72063,0.04151,0.350668,0.249965,0.330984,-0.041278,0.265713,-0.249997,-0.099993 -21,-0.094363,-0.304781,-1.390468,-1.745886,1.878182,0.22095,0.025024,0.14322,1.16528,1.692233,1.743997,-2.721184,0.041675,0.350308,0.249939,0.329106,-0.038763,0.266463,-0.249997,-0.099993 -22,-0.100423,-0.303377,-1.390478,-1.75511,1.877391,0.229577,0.016412,0.146428,1.165467,1.690878,1.740672,-2.721748,0.041851,0.349924,0.249921,0.327118,-0.036287,0.267226,-0.249998,-0.099995 -23,-0.106906,-0.302383,-1.390049,-1.764226,1.877016,0.238282,0.00775,0.149584,1.165655,1.689548,1.7374,-2.722303,0.042025,0.349548,0.249901,0.325084,-0.033897,0.267976,-0.249998,-0.099995 -24,-0.1139,-0.301816,-1.389145,-1.773337,1.877083,0.247173,-0.001063,0.152737,1.165848,1.688223,1.734123,-2.722858,0.042202,0.349164,0.249878,0.322977,-0.031573,0.268726,-0.249998,-0.099995 -25,-0.121686,-0.301807,-1.387596,-1.782444,1.877728,0.256313,-0.010194,0.155921,1.166046,1.686888,1.730797,-2.723419,0.042388,0.348762,0.249873,0.320774,-0.029305,0.269485,-0.249998,-0.099997 -26,-0.129825,-0.302184,-1.385632,-1.791483,1.878778,0.265551,-0.019371,0.159071,1.166246,1.68557,1.727504,-2.723974,0.042573,0.348362,0.249859,0.318524,-0.027119,0.270235,-0.249998,-0.099997 -27,-0.13871,-0.303127,-1.38302,-1.800472,1.880419,0.274993,-0.028835,0.162239,1.166452,1.684247,1.724178,-2.724533,0.042765,0.347948,0.249863,0.316188,-0.024998,0.27099,-0.249999,-0.099998 -28,-0.147884,-0.304436,-1.380018,-1.809418,1.882456,0.284547,-0.038334,0.165385,1.166661,1.682937,1.72087,-2.725089,0.042957,0.347533,0.249854,0.313804,-0.022959,0.27174,-0.249999,-0.099998 -29,-0.157749,-0.306312,-1.376376,-1.818277,1.885089,0.29427,-0.048092,0.168543,1.166874,1.681624,1.717538,-2.725647,0.043156,0.347106,0.249864,0.311343,-0.020992,0.272494,-0.249999,-0.099999 -30,-0.167836,-0.308537,-1.372367,-1.8271,1.88811,0.304106,-0.057867,0.171685,1.167091,1.680321,1.714215,-2.726202,0.043356,0.346676,0.249859,0.308834,-0.019107,0.273244,-0.249999,-0.099999 -31,-0.178284,-0.311188,-1.3679,-1.835862,1.891596,0.314072,-0.067738,0.174826,1.167311,1.679021,1.710885,-2.726758,0.043559,0.346238,0.249851,0.306268,-0.017303,0.273993,-0.249999,-0.099999 -32,-0.189344,-0.314406,-1.362807,-1.844489,1.895686,0.324168,-0.077835,0.177975,1.167536,1.677722,1.707536,-2.727316,0.043768,0.345789,0.249868,0.303633,-0.015576,0.274746,-0.249999,-0.099999 -33,-0.200505,-0.317941,-1.357395,-1.85307,1.900141,0.334361,-0.087902,0.181112,1.167764,1.676429,1.704191,-2.727873,0.043979,0.345336,0.249867,0.300957,-0.013936,0.275496,-0.249999,-0.099999 -34,-0.211906,-0.321871,-1.351569,-1.861564,1.90504,0.344657,-0.098013,0.184249,1.167996,1.675141,1.700838,-2.728429,0.044193,0.344875,0.249865,0.298231,-0.012381,0.276246,-0.249999,-0.099999 -35,-0.223529,-0.326198,-1.345331,-1.869954,1.910385,0.355043,-0.10816,0.187384,1.168232,1.673855,1.697478,-2.728986,0.044412,0.344406,0.249864,0.295457,-0.010913,0.276996,-0.249999,-0.099999 -36,-0.235332,-0.330907,-1.338698,-1.878231,1.916165,0.365511,-0.118324,0.190517,1.168472,1.672574,1.69411,-2.729543,0.044633,0.34393,0.249864,0.292638,-0.009532,0.277746,-0.249999,-0.099999 -37,-0.247268,-0.335982,-1.33169,-1.886387,1.922366,0.376051,-0.12848,0.193649,1.168715,1.671295,1.690734,-2.7301,0.044858,0.343447,0.249865,0.289776,-0.008241,0.278496,-0.249999,-0.099999 -38,-0.259293,-0.341406,-1.324327,-1.894413,1.928971,0.386654,-0.138607,0.196779,1.168962,1.670021,1.687351,-2.730658,0.045087,0.342955,0.249867,0.286875,-0.00704,0.279246,-0.249999,-0.099999 -39,-0.271362,-0.347161,-1.316629,-1.902302,1.935964,0.39731,-0.148681,0.199908,1.169213,1.66875,1.68396,-2.731215,0.045319,0.342456,0.24987,0.283938,-0.00593,0.279996,-0.249999,-0.099999 -40,-0.283431,-0.353228,-1.308617,-1.910045,1.943329,0.408009,-0.158679,0.203036,1.169468,1.667482,1.680561,-2.731774,0.045555,0.341949,0.249873,0.280966,-0.004914,0.280746,-0.249999,-0.099999 -41,-0.29546,-0.35959,-1.300309,-1.917635,1.951048,0.418742,-0.168577,0.206163,1.169727,1.666218,1.677154,-2.732332,0.045794,0.341435,0.249877,0.277964,-0.003991,0.281496,-0.249999,-0.099999 -42,-0.307408,-0.366228,-1.291725,-1.925064,1.959102,0.429498,-0.178352,0.209288,1.169989,1.664958,1.67374,-2.73289,0.046036,0.340913,0.249882,0.274934,-0.003162,0.282246,-0.249999,-0.099999 -43,-0.319238,-0.373123,-1.282883,-1.932323,1.967473,0.440268,-0.187983,0.212412,1.170255,1.663701,1.670317,-2.733449,0.046282,0.340383,0.249887,0.271879,-0.002429,0.282996,-0.249999,-0.099999 -44,-0.330669,-0.380116,-1.273961,-1.939465,1.976005,0.451018,-0.197309,0.215525,1.170523,1.662451,1.666899,-2.734007,0.04653,0.33985,0.249868,0.268821,-0.001798,0.283743,-0.249999,-0.099998 -45,-0.342146,-0.387462,-1.264666,-1.946368,1.984945,0.461783,-0.206575,0.218647,1.170796,1.661201,1.663461,-2.734566,0.046782,0.339305,0.249872,0.265727,-0.001259,0.284493,-0.249999,-0.099998 -46,-0.353431,-0.395026,-1.255151,-1.95307,1.994159,0.472532,-0.215647,0.221767,1.171073,1.659954,1.660015,-2.735126,0.047038,0.338752,0.249878,0.262616,-0.000816,0.285243,-0.249999,-0.099998 -47,-0.364474,-0.40278,-1.245444,-1.959571,2.003617,0.483254,-0.224493,0.224886,1.171353,1.658711,1.656561,-2.735686,0.047298,0.338191,0.249884,0.259492,-0.000472,0.285993,-0.249999,-0.099998 -48,-0.375249,-0.410707,-1.235564,-1.965864,2.0133,0.493938,-0.233093,0.228004,1.171636,1.657471,1.653099,-2.736246,0.04756,0.337623,0.24989,0.256359,-0.000225,0.286743,-0.249999,-0.099998 -49,-0.385729,-0.41879,-1.225527,-1.971942,2.023189,0.504573,-0.24143,0.231121,1.171924,1.656235,1.649628,-2.736807,0.047826,0.337047,0.249897,0.25322,-0.000076,0.287493,-0.249999,-0.099998 -50,-0.395894,-0.427014,-1.215347,-1.977796,2.033265,0.515151,-0.249487,0.234237,1.172214,1.655002,1.64615,-2.737368,0.048095,0.336463,0.249903,0.250077,-0.000027,0.288243,-0.249999,-0.099998 -51,-0.405725,-0.435365,-1.205041,-1.98342,2.04351,0.525659,-0.257248,0.237352,1.172508,1.653773,1.642663,-2.737929,0.048368,0.335872,0.249909,0.246935,-0.000075,0.288993,-0.249999,-0.099998 -52,-0.414974,-0.443688,-1.19478,-1.988854,2.053763,0.53604,-0.264557,0.240452,1.172805,1.652552,1.639189,-2.738489,0.04864,0.33528,0.249893,0.24382,-0.00023,0.289739,-0.249998,-0.099997 -53,-0.424075,-0.452245,-1.184272,-1.994001,2.064283,0.546375,-0.271674,0.243565,1.173106,1.65133,1.635686,-2.739051,0.048919,0.334673,0.249897,0.240688,-0.000476,0.290489,-0.249998,-0.099997 -54,-0.432811,-0.460899,-1.17367,-1.99889,2.074925,0.55661,-0.278462,0.246677,1.17341,1.65011,1.632175,-2.739613,0.049201,0.334059,0.249904,0.237563,-0.00082,0.291239,-0.249998,-0.099997 -55,-0.441151,-0.469628,-1.163001,-2.003522,2.085661,0.566733,-0.284898,0.249788,1.173718,1.648894,1.628656,-2.740176,0.049486,0.333437,0.24991,0.234451,-0.001261,0.291989,-0.249998,-0.099997 -56,-0.449084,-0.478419,-1.15228,-2.007891,2.096472,0.576733,-0.290969,0.252898,1.174029,1.647682,1.625128,-2.740739,0.049774,0.332807,0.249916,0.231354,-0.0018,0.292739,-0.249998,-0.099997 -57,-0.456598,-0.487259,-1.141525,-2.011991,2.107338,0.586598,-0.296662,0.256008,1.174343,1.646473,1.621592,-2.741303,0.050066,0.332169,0.249922,0.228275,-0.002436,0.293489,-0.249998,-0.099997 -58,-0.463685,-0.496139,-1.13075,-2.015816,2.118241,0.596319,-0.301967,0.259117,1.174661,1.645267,1.618048,-2.741867,0.05036,0.331524,0.249927,0.225218,-0.003168,0.294239,-0.249998,-0.099997 -59,-0.470337,-0.505045,-1.11997,-2.019361,2.129159,0.605883,-0.306873,0.262225,1.174982,1.644064,1.614495,-2.742431,0.050658,0.33087,0.249933,0.222185,-0.003995,0.294989,-0.249998,-0.099997 -60,-0.476351,-0.51383,-1.109356,-2.022654,2.139932,0.615209,-0.311237,0.26531,1.175304,1.642874,1.610965,-2.742992,0.050954,0.33022,0.24992,0.219211,-0.004924,0.295733,-0.249997,-0.099996 -61,-0.482111,-0.522757,-1.098619,-2.025628,2.150821,0.624426,-0.315312,0.268416,1.175632,1.641678,1.607397,-2.743557,0.051257,0.329552,0.249924,0.216237,-0.00594,0.296483,-0.249997,-0.099996 -62,-0.487437,-0.531691,-1.087914,-2.028304,2.161679,0.633457,-0.318969,0.271523,1.175962,1.640485,1.603818,-2.744122,0.051564,0.328875,0.24993,0.213295,-0.007049,0.297233,-0.249997,-0.099996 -63,-0.492309,-0.540613,-1.077268,-2.030682,2.172476,0.642288,-0.322192,0.274628,1.176296,1.639296,1.600231,-2.744688,0.051874,0.328191,0.249935,0.210389,-0.008249,0.297983,-0.249997,-0.099996 -64,-0.496725,-0.549514,-1.066698,-2.032759,2.183192,0.650909,-0.324972,0.277733,1.176633,1.63811,1.596636,-2.745255,0.052187,0.327499,0.24994,0.207522,-0.00954,0.298733,-0.249997,-0.099996 -65,-0.500683,-0.558384,-1.05622,-2.034533,2.193808,0.659308,-0.327301,0.280838,1.176974,1.636927,1.593032,-2.745822,0.052503,0.326799,0.249945,0.204697,-0.01092,0.299482,-0.249997,-0.099996 -66,-0.504182,-0.567215,-1.04585,-2.036,2.204304,0.667475,-0.329173,0.283942,1.177317,1.635748,1.589419,-2.746389,0.052821,0.326091,0.249949,0.201917,-0.012387,0.300232,-0.249997,-0.099995 -67,-0.507223,-0.575999,-1.035604,-2.037159,2.214662,0.675399,-0.330581,0.287045,1.177664,1.634571,1.585797,-2.746957,0.053143,0.325376,0.249954,0.199184,-0.013942,0.300982,-0.249997,-0.099995 -68,-0.509651,-0.584588,-1.025653,-2.038034,2.224713,0.682981,-0.331412,0.290114,1.17801,1.633411,1.582217,-2.747519,0.05346,0.32467,0.249944,0.196538,-0.015581,0.301723,-0.249996,-0.099993 -69,-0.511777,-0.59325,-1.015708,-2.038575,2.234733,0.690387,-0.331877,0.293215,1.178363,1.632242,1.57858,-2.748088,0.053787,0.323939,0.249947,0.193909,-0.017304,0.302473,-0.249996,-0.099993 -70,-0.513466,-0.60185,-1.005924,-2.0388,2.244567,0.697524,-0.331875,0.296317,1.178718,1.631075,1.574932,-2.748657,0.054117,0.3232,0.249952,0.191333,-0.019107,0.303223,-0.249996,-0.099993 -71,-0.514706,-0.610372,-0.996328,-2.038712,2.25419,0.704378,-0.331393,0.29942,1.179077,1.629912,1.571275,-2.749227,0.054451,0.322453,0.249956,0.188815,-0.02099,0.303972,-0.249996,-0.099993 -72,-0.515502,-0.618808,-0.986934,-2.038312,2.263582,0.71094,-0.330427,0.302522,1.179439,1.628751,1.567609,-2.749797,0.054787,0.321699,0.24996,0.186358,-0.022951,0.304722,-0.249996,-0.099993 -73,-0.515859,-0.627152,-0.977758,-2.037601,2.272728,0.717199,-0.328976,0.305623,1.179804,1.627594,1.563933,-2.750368,0.055126,0.320936,0.249963,0.183963,-0.024988,0.305472,-0.249996,-0.099993 -74,-0.515782,-0.635396,-0.968814,-2.036579,2.28161,0.723146,-0.327038,0.308725,1.180172,1.62644,1.560249,-2.750939,0.055468,0.320165,0.249966,0.181633,-0.027099,0.306222,-0.249996,-0.099993 -75,-0.515278,-0.643534,-0.960116,-2.035247,2.290214,0.728773,-0.324614,0.311826,1.180543,1.625289,1.556556,-2.751511,0.055813,0.319387,0.249969,0.179371,-0.029283,0.306972,-0.249996,-0.099993 -76,-0.514266,-0.651419,-0.951823,-2.03365,2.298384,0.733987,-0.321657,0.314873,1.180911,1.624161,1.552933,-2.752073,0.056148,0.318627,0.249962,0.177216,-0.031521,0.307708,-0.249994,-0.09999 -77,-0.512934,-0.659324,-0.943654,-2.031712,2.306388,0.738952,-0.318267,0.317971,1.181288,1.623018,1.549226,-2.752646,0.056497,0.317835,0.249963,0.175096,-0.03384,0.308457,-0.249994,-0.099989 -78,-0.511211,-0.667114,-0.935758,-2.029469,2.314083,0.743578,-0.314403,0.321072,1.181667,1.621876,1.545506,-2.753219,0.05685,0.317033,0.249966,0.173047,-0.036225,0.309206,-0.249994,-0.099989 -79,-0.509097,-0.674775,-0.928154,-2.026926,2.321449,0.747854,-0.310063,0.324173,1.18205,1.620738,1.541776,-2.753793,0.057206,0.316223,0.249968,0.171074,-0.038672,0.309956,-0.249994,-0.099989 -80,-0.506602,-0.682299,-0.920854,-2.024088,2.328473,0.751773,-0.305249,0.327275,1.182435,1.619602,1.538036,-2.754368,0.057564,0.315404,0.24997,0.169179,-0.04118,0.310706,-0.249994,-0.099989 -81,-0.503736,-0.689683,-0.913866,-2.020959,2.335146,0.755329,-0.299969,0.330376,1.182824,1.61847,1.534287,-2.754943,0.057925,0.314578,0.249972,0.167365,-0.043745,0.311456,-0.249993,-0.099989 -82,-0.500511,-0.69692,-0.907198,-2.017543,2.34146,0.758516,-0.294227,0.333478,1.183215,1.61734,1.530529,-2.755518,0.058289,0.313744,0.249973,0.165631,-0.046367,0.312206,-0.249993,-0.099989 -83,-0.496941,-0.704006,-0.900857,-2.013844,2.347408,0.76133,-0.288034,0.33658,1.183609,1.616214,1.526761,-2.756095,0.058656,0.312901,0.249974,0.163982,-0.049041,0.312956,-0.249993,-0.099989 -84,-0.493039,-0.710935,-0.894849,-2.009867,2.352985,0.763767,-0.281398,0.339682,1.184006,1.615091,1.522983,-2.756672,0.059025,0.312051,0.249975,0.162417,-0.051765,0.313705,-0.249993,-0.099989 -85,-0.488818,-0.717704,-0.889179,-2.005619,2.358186,0.765823,-0.274329,0.342785,1.184406,1.61397,1.519195,-2.757249,0.059397,0.311192,0.249976,0.160939,-0.054538,0.314455,-0.249993,-0.099989 -86,-0.484295,-0.724307,-0.88385,-2.001104,2.363009,0.767496,-0.266839,0.345888,1.184808,1.612853,1.515398,-2.757828,0.059772,0.310325,0.249977,0.15955,-0.057355,0.315205,-0.249993,-0.099989 -87,-0.479483,-0.730741,-0.878866,-1.996329,2.36745,0.768784,-0.258941,0.348991,1.185213,1.611738,1.511591,-2.758406,0.060149,0.30945,0.249977,0.158249,-0.060214,0.315955,-0.249993,-0.099989 -88,-0.474399,-0.737002,-0.874227,-1.9913,2.37151,0.769686,-0.250648,0.352095,1.185621,1.610627,1.507774,-2.758986,0.060529,0.308567,0.249978,0.15704,-0.063112,0.316705,-0.249993,-0.099989 -89,-0.469059,-0.743085,-0.869935,-1.986023,2.37519,0.7702,-0.241976,0.355199,1.186032,1.609518,1.503947,-2.759566,0.060912,0.307676,0.249978,0.155922,-0.066047,0.317454,-0.249993,-0.099988 -90,-0.46348,-0.748987,-0.865988,-1.980504,2.378491,0.770328,-0.23294,0.358304,1.186446,1.608413,1.50011,-2.760147,0.061297,0.306777,0.249978,0.154898,-0.069016,0.318204,-0.249993,-0.099988 -91,-0.457679,-0.754705,-0.862385,-1.974752,2.381417,0.77007,-0.223557,0.36141,1.186862,1.60731,1.496263,-2.760728,0.061685,0.305869,0.249978,0.153968,-0.072015,0.318954,-0.249993,-0.099988 -92,-0.451673,-0.760236,-0.859123,-1.968772,2.383972,0.769427,-0.213844,0.364516,1.187281,1.60621,1.492406,-2.76131,0.062076,0.304953,0.249978,0.153133,-0.075042,0.319704,-0.249993,-0.099988 -93,-0.44548,-0.765575,-0.856199,-1.962573,2.386161,0.768404,-0.20382,0.367623,1.187702,1.605113,1.488539,-2.761893,0.062469,0.304029,0.249977,0.152394,-0.078094,0.320454,-0.249993,-0.099988 -94,-0.439118,-0.770721,-0.853608,-1.956162,2.38799,0.767001,-0.193504,0.37073,1.188127,1.604019,1.484661,-2.762477,0.062864,0.303096,0.249977,0.151751,-0.081167,0.321203,-0.249992,-0.099988 -95,-0.432604,-0.775671,-0.851345,-1.949546,2.389468,0.765224,-0.182916,0.373839,1.188554,1.602928,1.480773,-2.763061,0.063262,0.302155,0.249977,0.151206,-0.084259,0.321953,-0.249992,-0.099988 -96,-0.425956,-0.780421,-0.849403,-1.942734,2.390601,0.763076,-0.172076,0.376948,1.188983,1.601839,1.476874,-2.763646,0.063663,0.301206,0.249977,0.150759,-0.087367,0.322703,-0.249992,-0.099988 -97,-0.419191,-0.784971,-0.847776,-1.935733,2.391399,0.760563,-0.161004,0.380058,1.189415,1.600753,1.472966,-2.764232,0.064066,0.300249,0.249977,0.150409,-0.090487,0.323453,-0.249992,-0.099988 -98,-0.412328,-0.789317,-0.846456,-1.92855,2.391871,0.757691,-0.149722,0.383169,1.18985,1.599671,1.469046,-2.764818,0.064472,0.299283,0.249976,0.150159,-0.093617,0.324202,-0.249992,-0.099988 -99,-0.405384,-0.793458,-0.845435,-1.921196,2.392027,0.754465,-0.138251,0.386281,1.190288,1.598591,1.465116,-2.765405,0.06488,0.298308,0.249976,0.150007,-0.096753,0.324952,-0.249992,-0.099988 -100,-0.398282,-0.797423,-0.844734,-1.913546,2.391833,0.750789,-0.126435,0.389455,1.190735,1.597493,1.461081,-2.766004,0.065307,0.29729,0.249983,0.149967,-0.099933,0.325718,-0.249994,-0.099991 -101,-0.391221,-0.801146,-0.84429,-1.905863,2.391383,0.746868,-0.114642,0.392573,1.191178,1.596418,1.457125,-2.766594,0.065721,0.296297,0.249984,0.150015,-0.103074,0.326468,-0.249994,-0.099991 -102,-0.384132,-0.80466,-0.844115,-1.898037,2.390654,0.742618,-0.102728,0.395688,1.191623,1.595346,1.453161,-2.767183,0.066137,0.295297,0.249983,0.150161,-0.106211,0.327218,-0.249994,-0.099991 -103,-0.377028,-0.807962,-0.844202,-1.890071,2.389654,0.738045,-0.090712,0.398805,1.192071,1.594276,1.449187,-2.767774,0.066555,0.294288,0.249983,0.150406,-0.109342,0.327968,-0.249994,-0.099991 -104,-0.369922,-0.811052,-0.844539,-1.881975,2.388394,0.733157,-0.078613,0.401924,1.192521,1.59321,1.445202,-2.768365,0.066976,0.29327,0.249983,0.150749,-0.112464,0.328717,-0.249994,-0.099991 -105,-0.362831,-0.813929,-0.845119,-1.873756,2.386887,0.727962,-0.066451,0.405043,1.192974,1.592146,1.441206,-2.768957,0.0674,0.292245,0.249983,0.15119,-0.115574,0.329467,-0.249994,-0.099991 -106,-0.355767,-0.816592,-0.845929,-1.865423,2.385144,0.722469,-0.054248,0.408164,1.193429,1.591085,1.437199,-2.76955,0.067825,0.29121,0.249983,0.151729,-0.118669,0.330217,-0.249994,-0.099991 -107,-0.348744,-0.819041,-0.846959,-1.856985,2.383177,0.716687,-0.042023,0.411287,1.193887,1.590026,1.43318,-2.770143,0.068253,0.290167,0.249983,0.152365,-0.121745,0.330967,-0.249994,-0.099991 -108,-0.341775,-0.821275,-0.8482,-1.84845,2.380998,0.710626,-0.029794,0.41441,1.194348,1.58897,1.42915,-2.770738,0.068684,0.289115,0.249983,0.153098,-0.124799,0.331717,-0.249994,-0.099991 -109,-0.33487,-0.823294,-0.84964,-1.839826,2.378619,0.704296,-0.01758,0.417536,1.19481,1.587917,1.425109,-2.771333,0.069117,0.288054,0.249982,0.153926,-0.12783,0.332466,-0.249994,-0.099991 -110,-0.328042,-0.825099,-0.85127,-1.831123,2.37605,0.697705,-0.0054,0.420663,1.195276,1.586866,1.421056,-2.77193,0.069552,0.286985,0.249982,0.154849,-0.130832,0.333216,-0.249994,-0.099991 -111,-0.321299,-0.826689,-0.853079,-1.822348,2.373303,0.690863,0.006729,0.423792,1.195744,1.585818,1.416991,-2.772527,0.06999,0.285907,0.249982,0.155866,-0.133805,0.333966,-0.249994,-0.09999 -112,-0.314653,-0.828064,-0.855058,-1.813509,2.370388,0.683781,0.018791,0.426922,1.196214,1.584773,1.412915,-2.773125,0.07043,0.28482,0.249982,0.156976,-0.136744,0.334716,-0.249994,-0.09999 -113,-0.30811,-0.829226,-0.857197,-1.804617,2.367317,0.676469,0.03077,0.430055,1.196686,1.58373,1.408827,-2.773724,0.070873,0.283724,0.249982,0.158177,-0.139647,0.335466,-0.249993,-0.09999 -114,-0.30168,-0.830174,-0.859486,-1.795678,2.364099,0.668935,0.04265,0.433189,1.197162,1.582689,1.404728,-2.774324,0.071318,0.282619,0.249982,0.15947,-0.14251,0.336215,-0.249993,-0.09999 -115,-0.295369,-0.83091,-0.861916,-1.786701,2.360745,0.66119,0.054418,0.436325,1.197639,1.581651,1.400616,-2.774925,0.071765,0.281505,0.249982,0.160851,-0.145332,0.336965,-0.249993,-0.09999 -116,-0.289184,-0.831434,-0.864478,-1.777695,2.357263,0.653244,0.066059,0.439463,1.198119,1.580615,1.396492,-2.775526,0.072215,0.280383,0.249982,0.162321,-0.148109,0.337715,-0.249993,-0.09999 -117,-0.28313,-0.831747,-0.867164,-1.768668,2.353664,0.645107,0.07756,0.442603,1.198601,1.579582,1.392356,-2.776129,0.072667,0.279251,0.249982,0.163877,-0.150839,0.338465,-0.249993,-0.09999 -118,-0.277213,-0.83185,-0.869966,-1.759627,2.349955,0.636787,0.08891,0.445745,1.199086,1.578551,1.388208,-2.776733,0.073121,0.27811,0.249982,0.165519,-0.153518,0.339214,-0.249993,-0.09999 -119,-0.271436,-0.831746,-0.872875,-1.750583,2.346145,0.628297,0.100096,0.44889,1.199573,1.577523,1.384047,-2.777338,0.073578,0.276961,0.249982,0.167243,-0.156144,0.339964,-0.249993,-0.09999 -120,-0.265803,-0.831434,-0.875883,-1.741541,2.342241,0.619643,0.111108,0.452036,1.200063,1.576497,1.379874,-2.777943,0.074037,0.275802,0.249982,0.169049,-0.158716,0.340714,-0.249993,-0.09999 -121,-0.260318,-0.830917,-0.878985,-1.732512,2.338253,0.610838,0.121936,0.455185,1.200555,1.575474,1.375689,-2.77855,0.074498,0.274634,0.249982,0.170935,-0.161229,0.341464,-0.249993,-0.09999 -122,-0.254982,-0.830196,-0.882173,-1.723502,2.334186,0.601889,0.13257,0.458337,1.201049,1.574453,1.371491,-2.779158,0.074962,0.273456,0.249982,0.172899,-0.163681,0.342214,-0.249993,-0.09999 -123,-0.249797,-0.829273,-0.885439,-1.71452,2.330047,0.592807,0.143001,0.461491,1.201545,1.573434,1.36728,-2.779767,0.075428,0.27227,0.249982,0.174939,-0.166071,0.342963,-0.249993,-0.099989 -124,-0.244764,-0.82815,-0.88878,-1.705574,2.325842,0.5836,0.153221,0.464647,1.202044,1.572417,1.363056,-2.780376,0.075896,0.271074,0.249982,0.177054,-0.168396,0.343713,-0.249993,-0.099989 -125,-0.239884,-0.826828,-0.892187,-1.696671,2.321578,0.574278,0.163223,0.467806,1.202546,1.571403,1.358819,-2.780987,0.076366,0.269868,0.249982,0.17924,-0.170653,0.344463,-0.249993,-0.099989 -126,-0.235158,-0.82531,-0.895657,-1.687821,2.317259,0.564851,0.173,0.470967,1.203049,1.570391,1.354569,-2.781599,0.076839,0.268654,0.249982,0.181496,-0.17284,0.345212,-0.249993,-0.099989 -127,-0.230584,-0.823598,-0.899184,-1.67903,2.312892,0.555326,0.182545,0.474132,1.203555,1.569381,1.350306,-2.782212,0.077314,0.26743,0.249982,0.183819,-0.174956,0.345962,-0.249992,-0.099989 -128,-0.226163,-0.821694,-0.902764,-1.670306,2.30848,0.545714,0.191852,0.477299,1.204063,1.568374,1.34603,-2.782827,0.077792,0.266196,0.249982,0.186208,-0.176997,0.346712,-0.249992,-0.099989 -129,-0.221892,-0.819599,-0.906391,-1.661658,2.304027,0.536023,0.200916,0.480469,1.204574,1.567369,1.34174,-2.783442,0.078271,0.264953,0.249982,0.18866,-0.178963,0.347462,-0.249992,-0.099989 -130,-0.217771,-0.817317,-0.910063,-1.653092,2.299539,0.526263,0.209732,0.483641,1.205087,1.566366,1.337437,-2.784058,0.078753,0.2637,0.249983,0.191172,-0.18085,0.348211,-0.249992,-0.099989 -131,-0.213798,-0.814849,-0.913775,-1.644617,2.295018,0.516441,0.218297,0.486817,1.205602,1.565365,1.33312,-2.784676,0.079237,0.262438,0.249983,0.193742,-0.182658,0.348961,-0.249992,-0.099989 -132,-0.20997,-0.812198,-0.917524,-1.636239,2.290467,0.506566,0.226604,0.489996,1.206119,1.564366,1.328789,-2.785295,0.079724,0.261166,0.249983,0.196367,-0.184384,0.349711,-0.249992,-0.099989 -133,-0.206308,-0.809373,-0.921271,-1.628116,2.28594,0.49679,0.234542,0.493109,1.20663,1.56339,1.32456,-2.785902,0.080193,0.259931,0.249977,0.199014,-0.186,0.350443,-0.249989,-0.099984 -134,-0.20276,-0.806363,-0.925086,-1.619963,2.281339,0.486841,0.242327,0.496288,1.207151,1.562397,1.320212,-2.786522,0.080682,0.258644,0.249977,0.201742,-0.187558,0.351191,-0.249989,-0.099984 -135,-0.199349,-0.803179,-0.92893,-1.611923,2.276715,0.476859,0.24985,0.499476,1.207676,1.561405,1.315841,-2.787144,0.081174,0.257344,0.249977,0.204518,-0.18903,0.351941,-0.249989,-0.099983 -136,-0.196073,-0.799821,-0.932801,-1.60401,2.272072,0.466858,0.257105,0.502667,1.208202,1.560415,1.311455,-2.787767,0.08167,0.256033,0.249978,0.207339,-0.190415,0.35269,-0.249988,-0.099983 -137,-0.189338,-0.799968,-0.943864,-1.596137,2.273566,0.453765,0.26,0.506047,1.208753,1.559373,1.306741,-2.788426,0.082223,0.25458,0.25003,0.210233,-0.191671,0.353486,-0.249997,-0.099996 -138,-0.179941,-0.802736,-0.960093,-1.588503,2.279525,0.438622,0.26,0.509267,1.209287,1.558381,1.302288,-2.789056,0.08273,0.253233,0.250032,0.213137,-0.192872,0.35424,-0.249998,-0.099997 -139,-0.171151,-0.805136,-0.975801,-1.58111,2.284954,0.42378,0.26,0.512455,1.209818,1.557401,1.297883,-2.78968,0.083228,0.251902,0.250039,0.216063,-0.193958,0.354986,-0.249997,-0.099996 -140,-0.162675,-0.807369,-0.991373,-1.573787,2.290138,0.408954,0.26,0.515662,1.210354,1.556419,1.293436,-2.790309,0.083732,0.25055,0.250038,0.219036,-0.194976,0.355736,-0.249997,-0.099996 -141,-0.154652,-0.809315,-1.006594,-1.56662,2.29492,0.394309,0.26,0.518873,1.210892,1.555438,1.288973,-2.790939,0.084239,0.249187,0.250037,0.22204,-0.195904,0.356486,-0.249997,-0.099996 -142,-0.147083,-0.810949,-1.021432,-1.559621,2.299278,0.379875,0.26,0.522089,1.211432,1.554459,1.284495,-2.791571,0.084749,0.247814,0.250035,0.225073,-0.196737,0.357236,-0.249997,-0.099996 -143,-0.13996,-0.812254,-1.035867,-1.552796,2.303199,0.365674,0.26,0.525308,1.211975,1.553483,1.280002,-2.792204,0.08526,0.24643,0.250034,0.228129,-0.197474,0.357986,-0.249997,-0.099996 -144,-0.133274,-0.813212,-1.049883,-1.546152,2.306672,0.351724,0.26,0.528532,1.21252,1.552508,1.275494,-2.792838,0.085774,0.245036,0.250032,0.231208,-0.198116,0.358735,-0.249996,-0.099995 -145,-0.127013,-0.813809,-1.063462,-1.539696,2.309687,0.338045,0.26,0.531759,1.213067,1.551535,1.270969,-2.793473,0.08629,0.243631,0.25003,0.234305,-0.198661,0.359485,-0.249996,-0.099995 -146,-0.121294,-0.813902,-1.07638,-1.533529,2.31207,0.324821,0.26,0.534966,1.213613,1.55057,1.266474,-2.794106,0.086801,0.242236,0.250036,0.237402,-0.199082,0.360229,-0.249995,-0.099993 -147,-0.115848,-0.813728,-1.089036,-1.527471,2.31413,0.311744,0.26,0.538202,1.214164,1.549601,1.261919,-2.794744,0.087321,0.240811,0.250035,0.240526,-0.199431,0.360979,-0.249995,-0.099993 -148,-0.110778,-0.813166,-1.101235,-1.521609,2.31572,0.298973,0.26,0.541442,1.214718,1.548634,1.257348,-2.795384,0.087844,0.239375,0.250033,0.243661,-0.199685,0.361728,-0.249995,-0.099993 -149,-0.106081,-0.812196,-1.112948,-1.515958,2.316817,0.286536,0.26,0.544687,1.215274,1.547668,1.25276,-2.796025,0.088368,0.237928,0.250031,0.246802,-0.19984,0.362478,-0.249995,-0.099993 -150,-0.101742,-0.810805,-1.124164,-1.510524,2.317415,0.274449,0.26,0.547937,1.215832,1.546704,1.248156,-2.796668,0.088896,0.23647,0.250029,0.249946,-0.199898,0.363228,-0.249995,-0.099993 -151,-0.097855,-0.808869,-1.134674,-1.505405,2.317359,0.262888,0.26,0.551154,1.216388,1.545752,1.243602,-2.797305,0.089413,0.23503,0.250033,0.253072,-0.199833,0.363968,-0.249993,-0.09999 -152,-0.094191,-0.806611,-1.144867,-1.500425,2.316936,0.251539,0.26,0.554412,1.21695,1.544791,1.238968,-2.797951,0.089944,0.233552,0.250032,0.256213,-0.199693,0.364718,-0.249993,-0.09999 -153,-0.090829,-0.80393,-1.154564,-1.495666,2.316019,0.240555,0.26,0.557676,1.217515,1.543832,1.234314,-2.798598,0.090478,0.232061,0.250029,0.259349,-0.199457,0.365467,-0.249993,-0.09999 -154,-0.087869,-0.800706,-1.163538,-1.49124,2.314457,0.230136,0.26,0.560889,1.218075,1.54289,1.229741,-2.799236,0.090997,0.230602,0.250034,0.262452,-0.199103,0.366204,-0.24999,-0.099986 -155,-0.085092,-0.797135,-1.172188,-1.486964,2.312509,0.219943,0.26,0.564159,1.218644,1.541935,1.22506,-2.799886,0.091534,0.229092,0.250032,0.265565,-0.19867,0.366952,-0.24999,-0.099986 -156,-0.082572,-0.793139,-1.180341,-1.482922,2.310069,0.210131,0.26,0.567438,1.219215,1.540981,1.220356,-2.800538,0.092074,0.227569,0.250029,0.268665,-0.198144,0.367702,-0.24999,-0.099985 -157,-0.080307,-0.788702,-1.187974,-1.479129,2.307123,0.200725,0.26,0.570723,1.219789,1.540028,1.215634,-2.801191,0.092617,0.226033,0.250026,0.271747,-0.197521,0.368451,-0.249989,-0.099985 -158,-0.078367,-0.783783,-1.194913,-1.475669,2.303601,0.191891,0.26,0.57393,1.220354,1.539099,1.211038,-2.80183,0.093137,0.224548,0.25003,0.274775,-0.196793,0.369181,-0.249986,-0.09998 -159,-0.076574,-0.778469,-1.201508,-1.47239,2.299646,0.183307,0.26,0.577217,1.220932,1.538152,1.206296,-2.802486,0.093681,0.222997,0.250028,0.27781,-0.195978,0.369929,-0.249985,-0.099979 -160,-0.074985,-0.77274,-1.207618,-1.469363,2.295216,0.175117,0.26,0.580517,1.221512,1.537204,1.201522,-2.803144,0.09423,0.221429,0.250025,0.280819,-0.195072,0.370678,-0.249985,-0.099979 -161,-0.073599,-0.766586,-1.213226,-1.4666,2.290301,0.167338,0.26,0.583823,1.222095,1.536257,1.196729,-2.803804,0.094782,0.219848,0.250021,0.283798,-0.194071,0.371427,-0.249985,-0.099979 -162,-0.072387,-0.760035,-1.218367,-1.464098,2.284931,0.15995,0.26,0.587136,1.22268,1.535311,1.191918,-2.804465,0.095336,0.218255,0.250016,0.286745,-0.192982,0.372176,-0.249985,-0.099978 -163,-0.071224,-0.75335,-1.223466,-1.461899,2.279459,0.152798,0.259729,0.590454,1.223267,1.534367,1.187087,-2.805129,0.095892,0.21665,0.250015,0.289652,-0.19179,0.372926,-0.249985,-0.099978 -164,-0.070077,-0.746586,-1.228599,-1.459991,2.273949,0.145843,0.259163,0.593779,1.223857,1.533425,1.182238,-2.805794,0.09645,0.215033,0.250018,0.292517,-0.190501,0.373675,-0.249984,-0.099978 -165,-0.068931,-0.73975,-1.233768,-1.45836,2.2684,0.139081,0.258333,0.59711,1.224448,1.532483,1.177369,-2.806461,0.097011,0.213403,0.25002,0.29534,-0.189123,0.374424,-0.249984,-0.099978 -166,-0.067781,-0.732847,-1.238973,-1.457005,2.262812,0.132517,0.257241,0.600447,1.225042,1.531543,1.17248,-2.80713,0.097574,0.211762,0.250023,0.298118,-0.187656,0.375174,-0.249984,-0.099977 -167,-0.066626,-0.72588,-1.244215,-1.455929,2.257183,0.126158,0.255893,0.603792,1.225639,1.530604,1.167572,-2.807801,0.098139,0.210108,0.250026,0.300849,-0.186103,0.375923,-0.249984,-0.099977 -168,-0.065462,-0.718854,-1.249494,-1.455133,2.251513,0.12001,0.254291,0.607142,1.226238,1.529666,1.162644,-2.808473,0.098707,0.208441,0.250029,0.303529,-0.184465,0.376672,-0.249984,-0.099977 -169,-0.064288,-0.711771,-1.254812,-1.454617,2.245801,0.11408,0.25244,0.6105,1.226839,1.528729,1.157695,-2.809148,0.099277,0.206762,0.250032,0.306157,-0.182744,0.377421,-0.249983,-0.099977 -170,-0.063101,-0.704637,-1.260168,-1.454382,2.240046,0.108372,0.250343,0.613865,1.227442,1.527794,1.152726,-2.809824,0.099849,0.20507,0.250035,0.308729,-0.180941,0.378171,-0.249983,-0.099976 -171,-0.061899,-0.697456,-1.265563,-1.454429,2.234248,0.102894,0.248005,0.617237,1.228048,1.526859,1.147736,-2.810503,0.100424,0.203365,0.250038,0.311244,-0.179058,0.37892,-0.249983,-0.099976 -172,-0.060681,-0.690231,-1.270997,-1.454757,2.228408,0.097651,0.24543,0.620616,1.228656,1.525926,1.142725,-2.811183,0.101001,0.201647,0.250041,0.313698,-0.177097,0.379669,-0.249983,-0.099976 -173,-0.059446,-0.682966,-1.27647,-1.455366,2.222524,0.092649,0.242622,0.624002,1.229266,1.524993,1.137693,-2.811866,0.101581,0.199916,0.250044,0.316089,-0.17506,0.380418,-0.249983,-0.099976 -174,-0.058191,-0.675667,-1.281982,-1.456256,2.216598,0.087892,0.239587,0.627396,1.229879,1.524062,1.13264,-2.81255,0.102163,0.198172,0.250047,0.318415,-0.172949,0.381167,-0.249982,-0.099975 -175,-0.056917,-0.668337,-1.287533,-1.457425,2.21063,0.083387,0.236329,0.630798,1.230494,1.523131,1.127564,-2.813237,0.102747,0.196414,0.250051,0.320673,-0.170766,0.381916,-0.249982,-0.099975 -176,-0.055623,-0.660981,-1.293123,-1.458872,2.204622,0.079138,0.232854,0.634208,1.231112,1.522202,1.122467,-2.813925,0.103334,0.194643,0.250054,0.322862,-0.168513,0.382666,-0.249982,-0.099975 -177,-0.054309,-0.653604,-1.29875,-1.460596,2.198575,0.07515,0.229166,0.637626,1.231732,1.521273,1.117347,-2.814616,0.103923,0.192858,0.250057,0.324979,-0.166192,0.383415,-0.249982,-0.099975 -178,-0.052974,-0.646209,-1.304414,-1.462594,2.192489,0.071428,0.225271,0.641052,1.232354,1.520345,1.112204,-2.815309,0.104515,0.191059,0.250061,0.327022,-0.163807,0.384164,-0.249982,-0.099974 -179,-0.05162,-0.638803,-1.310115,-1.464866,2.186368,0.067976,0.221174,0.644486,1.232979,1.519418,1.107039,-2.816004,0.105109,0.189246,0.250064,0.328988,-0.161358,0.384913,-0.249981,-0.099974 -180,-0.050246,-0.631389,-1.31585,-1.467408,2.180214,0.064799,0.216881,0.647929,1.233607,1.518492,1.10185,-2.816702,0.105706,0.18742,0.250068,0.330877,-0.158848,0.385662,-0.249981,-0.099974 -181,-0.048735,-0.623867,-1.321716,-1.470336,2.173943,0.061916,0.212374,0.651486,1.23425,1.517539,1.096448,-2.817422,0.106337,0.185494,0.250054,0.332698,-0.156254,0.386434,-0.249985,-0.09998 -182,-0.04731,-0.616446,-1.327526,-1.473428,2.167724,0.059309,0.207705,0.654957,1.234884,1.516612,1.091192,-2.818126,0.106942,0.183629,0.250055,0.334424,-0.153629,0.387185,-0.249986,-0.09998 -183,-0.04588,-0.609037,-1.333361,-1.476772,2.161484,0.056984,0.202857,0.658429,1.235519,1.515687,1.085927,-2.818831,0.107547,0.181757,0.250058,0.336067,-0.150952,0.387935,-0.249985,-0.09998 -184,-0.044439,-0.601642,-1.339224,-1.480373,2.155223,0.054948,0.197834,0.66191,1.236157,1.514763,1.080638,-2.819538,0.108154,0.179871,0.250061,0.337625,-0.148225,0.388684,-0.249985,-0.09998 -185,-0.042988,-0.594267,-1.345111,-1.484228,2.148947,0.053204,0.192645,0.665399,1.236797,1.513839,1.075325,-2.820248,0.108764,0.17797,0.250063,0.339097,-0.14545,0.389433,-0.249985,-0.099979 -186,-0.041532,-0.586915,-1.35102,-1.488334,2.142659,0.051755,0.187293,0.668899,1.237439,1.512916,1.069986,-2.82096,0.109376,0.176053,0.250066,0.34048,-0.14263,0.390182,-0.249985,-0.099979 -187,-0.040074,-0.579595,-1.356948,-1.492685,2.136364,0.050603,0.181785,0.672408,1.238084,1.511993,1.064622,-2.821674,0.109991,0.174122,0.250069,0.341775,-0.139768,0.390931,-0.249984,-0.099979 -188,-0.038618,-0.57231,-1.362891,-1.497278,2.130068,0.049751,0.176126,0.675927,1.238732,1.511071,1.059231,-2.822392,0.110609,0.172175,0.250072,0.342978,-0.136867,0.39168,-0.249984,-0.099978 -189,-0.03717,-0.565068,-1.368844,-1.502108,2.123777,0.0492,0.170323,0.679457,1.239383,1.510149,1.053814,-2.823111,0.111229,0.170212,0.250075,0.34409,-0.133929,0.392429,-0.249984,-0.099978 -190,-0.035734,-0.557874,-1.374804,-1.50717,2.117497,0.048953,0.16438,0.682997,1.240036,1.509227,1.04837,-2.823834,0.111852,0.168233,0.250077,0.345109,-0.130958,0.393179,-0.249984,-0.099978 -191,-0.034318,-0.550735,-1.380764,-1.512459,2.111234,0.04901,0.158303,0.686548,1.240691,1.508306,1.042898,-2.824559,0.112477,0.166239,0.25008,0.346035,-0.127956,0.393928,-0.249984,-0.099977 -192,-0.032928,-0.543659,-1.38672,-1.51797,2.104996,0.049372,0.152097,0.69011,1.24135,1.507384,1.037399,-2.825286,0.113106,0.164228,0.250083,0.346865,-0.124927,0.394676,-0.249983,-0.099977 -193,-0.03157,-0.536651,-1.392664,-1.523698,2.098789,0.050041,0.145767,0.693683,1.242011,1.506463,1.031872,-2.826017,0.113737,0.1622,0.250085,0.3476,-0.121873,0.395425,-0.249983,-0.099977 -194,-0.030255,-0.529718,-1.398589,-1.529636,2.092623,0.051016,0.139319,0.697267,1.242675,1.505542,1.026316,-2.82675,0.114371,0.160156,0.250088,0.348238,-0.118797,0.396174,-0.249983,-0.099976 -195,-0.02899,-0.522868,-1.404489,-1.535779,2.086505,0.052298,0.132757,0.700863,1.243342,1.504621,1.020731,-2.827486,0.115008,0.158095,0.25009,0.348779,-0.115703,0.396923,-0.249982,-0.099976 -196,-0.027785,-0.516108,-1.410355,-1.542122,2.080443,0.053887,0.126085,0.704471,1.244011,1.5037,1.015116,-2.828226,0.115647,0.156017,0.250092,0.349223,-0.112593,0.397672,-0.249982,-0.099976 -197,-0.02665,-0.509445,-1.416176,-1.548657,2.074446,0.055783,0.119309,0.708091,1.244683,1.502779,1.009472,-2.828968,0.11629,0.153921,0.250094,0.349569,-0.10947,0.398421,-0.249982,-0.099975 -198,-0.025597,-0.502888,-1.421945,-1.55538,2.068525,0.057984,0.112431,0.711723,1.245359,1.501858,1.003797,-2.829713,0.116935,0.151808,0.250096,0.349816,-0.106339,0.39917,-0.249982,-0.099975 -199,-0.024639,-0.496444,-1.427648,-1.562284,2.062687,0.06049,0.105457,0.715368,1.246037,1.500936,0.99809,-2.830461,0.117584,0.149677,0.250097,0.349965,-0.1032,0.399918,-0.249981,-0.099975 diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp deleted file mode 100644 index 0146c053..00000000 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp +++ /dev/null @@ -1,1496 +0,0 @@ -// -// Created by xtkuang on 2025/7/24. -// - -#include "../include/humanoid_robot.h" -#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" -#include "motor/ti5_motor/ti5_motor.h" -#include "../../../../utils/base/include/abstract_interpolation.h" -#include "data_center/include/motors_info.h" -#include "common/math/include/support_functions.h" - -using namespace std; -using namespace cmvr::device; - -template -HumanoidRobot::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { - try { - id_ = cfg.getAttrString("id"); - - dof_ = DOF; - if (!pathExists(cfg.getAttrString("urdf"))) { - throw runtime_error("urdf file does not exist"); - } - auto rcfg = cmvr::dyn::LoadRobotFromURDF( - cfg.getAttrString("urdf"), cfg.getAttrString("baseLink")); - m_robot_ = std::make_shared >(rcfg); - joint_names_ = splitString(cfg.getAttrString("jointNames"), ","); - link_names_ = splitString(cfg.getAttrString("linkNames"), ","); - if (joint_names_.size() != dof_) { - throw runtime_error("joint names size mismatched with dof"); - } - - m_state_ = m_robot_->MakeState(link_names_, joint_names_); - m_cctrl_ = make_shared >(m_robot_); - upd_freq_ = cfg.getAttrDefault("updFreq", 500); - CSP_buffer_ = make_shared >(cfg.getAttrDefault("bufferSize", 50)); - CSV_buffer_ = make_shared >(cfg.getAttrDefault("bufferSize", 50)); - CSC_buffer_ = make_shared >(cfg.getAttrDefault("bufferSize", 50)); - - - upd_freq_ = 1000; - upd_timer_ = make_shared(); - upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000), - [this] { update_state_(); }); - rsm_.store(ROBOT_READY); - } catch (exception &e) { - LOG(ERROR) << "HumanoidRobot init failed, id=" << id_; - throw runtime_error(e.what()); - } -} - -template -void HumanoidRobot::init() { - motor_manager_ = MotorsInfo::getInstance()->getMotorManager(); - joint_space_planner_ = JointSpacePlannerCreator::create(TOPPRA_BSPLINE); - joint_space_planner_->setPathType(PathType::Quintic); - - joint_space_planner_->setSymmetricLimits(std::vector(7, 1.0), - std::vector(7, 1.0)); - - - ik_solver_ = std::make_shared("/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf", - "PELVIS_S", - "R_WRIST_R_S", - "R_FINGER_TIP_FIXED"); - if (!ik_solver_->init()) { - throw runtime_error("HumanoidRobot speedL init failed: PinocchioDlsIKSolver::init() failed"); - } - - speedl_config_.linear_velocity_max = 0.55; - speedl_config_.linear_acceleration_max = 5.00; - speedl_config_.linear_jerk_max = 10.0; - speedl_config_.angular_velocity_max = 1.00; - speedl_config_.angular_acceleration_max = 5.00; - speedl_config_.angular_jerk_max = 12.0; - speedl_config_.joint_acceleration_max = std::vector(7, 8.0); - speedl_config_.linear_target_replan_threshold = 1e-4; - speedl_config_.angular_target_replan_threshold = 1e-4; - speedl_config_.linear_reverse_cos_threshold = -0.8660254037844386; - speedl_config_.linear_reverse_switch_speed_threshold = 1e-3; - if (!ik_solver_->configureSpeedL(speedl_config_)) { - throw runtime_error("HumanoidRobot speedL init failed: configureSpeedL() failed"); - } - speedl_applied_acceleration_ = speedl_config_.linear_acceleration_max; - rsm_.store(ROBOT_ESTOP); -} - - -template -bool HumanoidRobot::speedL(const std::vector& xd, - double acceleration, - double time, - cmvr::CartesianFrame frame) -{ - if (!ik_solver_) { - throw runtime_error("speedL failed: ik_solver_ is not initialized"); - } - if (xd.size() != 6) { - throw runtime_error("speedL failed: xd size must be 6"); - } - if (acceleration <= 0.0) { - throw runtime_error("speedL failed: acceleration must be positive"); - } - if ((!speedl_thread_ || !speedl_thread_->joinable()) && - is_right_arm_busy_.exchange(true)) { - throw runtime_error("speedL failed: right arm is busy"); - } - - ensureSpeedLWorkerStarted_(); - - Eigen::Matrix target_twist = Eigen::Matrix::Zero(); - std::uint64_t command_version = 0; - for (int i = 0; i < 6; ++i) { - target_twist[i] = xd[static_cast(i)]; - } - - { - std::lock_guard lock(speedl_mutex_); - speedl_target_twist_ = target_twist; - speedl_target_acceleration_ = acceleration; - speedl_target_frame_ = frame; - speedl_command_active_ = true; - command_version = ++speedl_command_version_; - } - speedl_cv_.notify_all(); - - if (time > 0.0) { - std::this_thread::sleep_for(std::chrono::duration(time)); - bool should_stop = false; - { - std::lock_guard lock(speedl_mutex_); - if (speedl_command_version_ == command_version) { - speedl_target_twist_.setZero(); - speedl_target_frame_ = cmvr::CartesianFrame::Base; - speedl_command_active_ = true; - ++speedl_command_version_; - should_stop = true; - } - } - if (should_stop) { - speedl_cv_.notify_all(); - } - } - return true; -} - -template -void HumanoidRobot::stopSpeedL() -{ - if (!speedl_thread_ || !speedl_thread_->joinable()) { - return; - } - - { - std::lock_guard lock(speedl_mutex_); - speedl_target_twist_.setZero(); - speedl_target_frame_ = cmvr::CartesianFrame::Base; - speedl_command_active_ = true; - ++speedl_command_version_; - } - LOG(INFO) << "HumanoidRobot stopSpeedL requested"; - speedl_cv_.notify_all(); -} - -template -Eigen::Matrix HumanoidRobot::getSpeedLCommandTwistBase() -{ - std::lock_guard lock(speedl_mutex_); - return speedl_last_command_twist_base_; -} - -template -void HumanoidRobot::sendJointVelocityCommand_(const std::vector& cmd) { - for (const auto& j : cmd) { - auto motor = motor_manager_->getMotor(j.joint_name); - if (motor != nullptr) { - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); - } - motor->setTarget(j.vel); - } - } -} - -template -bool HumanoidRobot::updateSpeedLAccelerationConfig_(double acceleration) { - if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9) { - return true; - } - - auto config = speedl_config_; - config.linear_acceleration_max = acceleration; - config.angular_acceleration_max = acceleration; - if (!ik_solver_->configureSpeedL(config)) { - return false; - } - - speedl_config_ = config; - speedl_applied_acceleration_ = acceleration; - return true; -} - -template -void HumanoidRobot::ensureSpeedLWorkerStarted_() { - if (speedl_thread_ && speedl_thread_->joinable()) { - return; - } - speedl_stop_requested_.store(false); - LOG(INFO) << "HumanoidRobot speedL worker starting"; - speedl_thread_ = std::make_unique(&HumanoidRobot::speedLWorkerLoop_, this); -} - -template -void HumanoidRobot::stopSpeedLWorker_() { - if (!speedl_thread_ || !speedl_thread_->joinable()) { - return; - } - - { - std::lock_guard lock(speedl_mutex_); - speedl_stop_requested_.store(true); - speedl_command_active_ = false; - speedl_target_twist_.setZero(); - speedl_target_frame_ = cmvr::CartesianFrame::Base; - speedl_last_command_twist_base_.setZero(); - } - speedl_cv_.notify_all(); - speedl_thread_->join(); - LOG(INFO) << "HumanoidRobot speedL worker stopped"; - speedl_thread_.reset(); - speedl_stop_requested_.store(false); - speedl_applied_acceleration_ = speedl_config_.linear_acceleration_max; - is_right_arm_busy_.store(false); -} - -template -void HumanoidRobot::speedLWorkerLoop_() { - static const std::vector kRightArmJointNames = { - "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", - "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R" - }; - const double dt = 1.0 / static_cast(std::max(upd_freq_, 1)); - auto next_tick = std::chrono::steady_clock::now(); - - auto read_right_arm_state = [&](std::vector& q_now, - std::vector& qd_now) -> bool { - std::vector states; - getJointsState(states); - std::unordered_map state_map; - state_map.reserve(states.size()); - for (const auto& js : states) { - state_map[js.name] = js; - } - - q_now.resize(kRightArmJointNames.size()); - qd_now.resize(kRightArmJointNames.size()); - for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { - const auto it = state_map.find(kRightArmJointNames[i]); - if (it == state_map.end()) { - return false; - } - q_now[i] = it->second.position; - qd_now[i] = it->second.velocity; - } - return true; - }; - - auto send_zero = [&]() { - std::vector zero_cmd; - zero_cmd.reserve(kRightArmJointNames.size()); - for (const auto& name : kRightArmJointNames) { - zero_cmd.push_back({name, 0.0}); - } - sendJointVelocityCommand_(zero_cmd); - }; - - LOG(INFO) << "HumanoidRobot speedL worker loop entered"; - - while (true) { - Eigen::Matrix target_twist = Eigen::Matrix::Zero(); - double acceleration = 0.25; - cmvr::CartesianFrame target_frame = cmvr::CartesianFrame::Base; - { - std::unique_lock lock(speedl_mutex_); - speedl_cv_.wait(lock, [&]() { - return speedl_stop_requested_.load() || speedl_command_active_; - }); - if (speedl_stop_requested_.load()) { - LOG(INFO) << "HumanoidRobot speedL worker loop received stop request"; - break; - } - target_twist = speedl_target_twist_; - acceleration = speedl_target_acceleration_; - target_frame = speedl_target_frame_; - } - - next_tick = std::chrono::steady_clock::now(); - while (true) { - { - std::lock_guard lock(speedl_mutex_); - if (speedl_stop_requested_.load()) { - LOG(INFO) << "HumanoidRobot speedL worker loop stopping during active command"; - send_zero(); - is_right_arm_busy_.store(false); - return; - } - if (!speedl_command_active_) { - break; - } - target_twist = speedl_target_twist_; - acceleration = speedl_target_acceleration_; - target_frame = speedl_target_frame_; - } - - if (!updateSpeedLAccelerationConfig_(acceleration)) { - LOG(ERROR) << "speedL worker: configureSpeedL() failed"; - send_zero(); - is_right_arm_busy_.store(false); - return; - } - - std::vector q_now; - std::vector qd_now; - if (!read_right_arm_state(q_now, qd_now)) { - LOG(ERROR) << "speedL worker: failed to read right arm joint state"; - send_zero(); - is_right_arm_busy_.store(false); - return; - } - - std::vector qd_cmd; - if (!ik_solver_->speedLStep(target_twist, - dt, - q_now, - qd_cmd, - target_frame, - true)) { - LOG(ERROR) << "speedL worker: speedLStep() failed"; - send_zero(); - is_right_arm_busy_.store(false); - return; - } - - std::vector qd_send; - qd_send.reserve(kRightArmJointNames.size()); - for (size_t i = 0; i < kRightArmJointNames.size() && i < qd_cmd.size(); ++i) { - qd_send.push_back({kRightArmJointNames[i], qd_cmd[i]}); - } - { - std::lock_guard lock(speedl_mutex_); - speedl_last_command_twist_base_ = ik_solver_->getSpeedLCommandTwistBase(); - } - sendJointVelocityCommand_(qd_send); - - double qd_cmd_norm = 0.0; - double qd_meas_norm = 0.0; - for (double v : qd_cmd) { - qd_cmd_norm += v * v; - } - for (double v : qd_now) { - qd_meas_norm += v * v; - } - qd_cmd_norm = std::sqrt(qd_cmd_norm); - qd_meas_norm = std::sqrt(qd_meas_norm); - - if (target_twist.norm() < 1e-9 && qd_cmd_norm < 1e-3 && qd_meas_norm < 1e-2) { - { - std::lock_guard lock(speedl_mutex_); - speedl_command_active_ = false; - speedl_last_command_twist_base_.setZero(); - } - send_zero(); - is_right_arm_busy_.store(false); - break; - } - - next_tick += std::chrono::duration_cast( - std::chrono::duration(dt)); - std::this_thread::sleep_until(next_tick); - } - } - - send_zero(); - is_right_arm_busy_.store(false); - LOG(INFO) << "HumanoidRobot speedL worker loop exited"; -} - -template -void HumanoidRobot::torqueOff() { - try { - stopSpeedLWorker_(); - for (const auto &pair: motor_manager_->motorsMap()) { - if (pair.second->jointName() != "WAIST_Y" && pair.second->jointName() != "WAIST_P") - pair.second->torqueOff(); - } - } catch (std::exception &e) { - throw runtime_error(e.what()); - } -} - - -template -HumanoidRobot::~HumanoidRobot() { - // TODO: close can interfaces - stopSpeedLWorker_(); - upd_timer_->stop(); - // this->torqueOff(); -} - -template -int HumanoidRobot::getDOF() { - return dof_; -} - -template -std::vector HumanoidRobot::getJointNames() { - return joint_names_; -} - -template -std::unordered_map HumanoidRobot::getJointQ() const { - std::unordered_map joint_qs; - - for (const auto &pair: motor_manager_->motorsMap()) { - auto motor = pair.second; - joint_qs[motor->jointName()] = motor->getQ(); - } - return joint_qs; -} - -template -void HumanoidRobot::getJointQ(std::unordered_map &joint_qs) const { - for (auto &pair: joint_qs) { - auto motor = motor_manager_->getMotor(pair.first); - if (motor) { - pair.second = motor->getQ(); - } else { - pair.second = 0.0; - } - } -} - - -template -std::vector HumanoidRobot::getLinkNames() { - return link_names_; -} - -template -void HumanoidRobot::getJointsState(std::vector &states) { - try { - lock_guard lock(exec_mtx_); - - states.clear(); - JointState state; - for (const auto &pair: motor_manager_->motorsMap()) { - auto motor = pair.second; - state.name = motor->jointName(); - state.position = motor->getQ(); - state.velocity = motor->getQd(); - states.push_back(state); - } - } catch (exception &e) { - throw runtime_error(e.what()); - } -} - -template -void HumanoidRobot::getState(RobotState &state) { - try { - lock_guard lock(exec_mtx_); - // TODO: copy m_state_ date into state - } catch (exception &e) { - throw runtime_error(e.what()); - } -} - -template -void HumanoidRobot::torqueOn() { - stopSpeedLWorker_(); - eStop(); -} - -template -void HumanoidRobot::torqueOn(const std::string &joint_name) { - stopSpeedLWorker_(); - auto motor = motor_manager_->getMotor(joint_name); - motor->brake(); -} - -template -void HumanoidRobot::torqueOff(const std::string &joint_name) { - stopSpeedLWorker_(); - auto motor = motor_manager_->getMotor(joint_name); - motor->torqueOff(); -} - -template -void HumanoidRobot::eStop() { - stopSpeedLWorker_(); - CSP_buffer_->clear(); - CSV_buffer_->clear(); - CSC_buffer_->clear(); - for (const auto &pair: motor_manager_->motorsMap()) { - pair.second->brake(); - } -} - -// template -// void HumanoidRobot::moveJ(std::vector &cmd, double vel, double acc) { -// try { -// for (const auto &j: cmd) { -// auto motor = motor_manager_->getMotor(j.joint_name); -// if (motor != nullptr) { -// // PPM 模式下 这个实际速度会超30% 左右 -// motor->setQd(vel); -// -// if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { -// motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); -// } -// motor->setQ(j.rad); -// } -// } -// -// //3. wait for completion -// bool completion = true; -// do { -// completion = true; -// for (const auto &j: cmd) { -// auto motor = motor_manager_->getMotor(j.joint_name); -// if (motor != nullptr) { -// if (!motor->reachedTargetQ()) { -// completion = false; -// break; -// } -// } -// } -// // 4. while waiting, check flash_cmd_, if it is true, set it false then exit -// if (flash_cmd_.load()) { -// flash_cmd_.store(false); -// return; -// } -// std::this_thread::sleep_for(std::chrono::milliseconds(2)); -// } while (!completion); -// } catch (exception &e) { -// throw runtime_error(e.what()); -// } -// } - - - -template -void HumanoidRobot::speedJ(std::vector &cmd) { - stopSpeedLWorker_(); - - for (const auto &j: cmd) { - auto motor = motor_manager_->getMotor(j.joint_name); - if (motor != nullptr) { - - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); - } - motor->setTarget(j.vel); - } - } -} - -template -void HumanoidRobot::speedJ(double vel) { - stopSpeedLWorker_(); - for (const auto &motor: motor_manager_->motorsMap()) { - motor.second->setTarget(vel); - } -} -template -void HumanoidRobot::moveJ(std::vector &cmd, double vel, double acc) { - stopSpeedLWorker_(); - if (cmd.empty()) return; - - LOG(INFO) << "vel : " << vel << "acc : " << acc << endl; - for (const auto &js: cmd) { - LOG(INFO) << js.joint_name << " , "<< js.rad << endl; - } - - // 获取命令中的第一个电机，判断其所属电机组 - auto &first_jp = cmd.at(0); - std::string group = ""; - - if (first_jp.joint_name.find("L_") != std::string::npos) { - group = "L_"; - } else if (first_jp.joint_name.find("R_") != std::string::npos) { - group = "R_"; - } else if (first_jp.joint_name.find("HEAD_") != std::string::npos) { - group = "HEAD_"; - } else if (first_jp.joint_name.find("WAIST_") != std::string::npos) { - group = "WAIST_"; - } else { - LOG(ERROR) << "Unknown joint group for " << first_jp.joint_name; - return; - } - - // 检查所有电机是否属于同一组 - for (auto &jp : cmd) { - if (jp.joint_name.find(group) == std::string::npos) { - LOG(ERROR) << "Mixed motor groups detected, skipping command."; - return; // 发现不属于同一组，直接返回 - } - } - - // 获取电机组的互斥锁和忙碌状态 - std::mutex* group_mutex = nullptr; - std::atomic* is_busy_flag = nullptr; - - // 根据电机组选择对应的mutex和忙碌标志 - if (group == "L_") { - group_mutex = &left_arm_mutex_; - is_busy_flag = &is_left_arm_busy_; - } else if (group == "R_") { - group_mutex = &right_arm_mutex_; - is_busy_flag = &is_right_arm_busy_; - } else if (group == "HEAD_") { - group_mutex = &head_mutex_; - is_busy_flag = &is_head_busy_; - } else if (group == "WAIST_") { - group_mutex = &waist_mutex_; - is_busy_flag = &is_waist_busy_; - } - - // 检查电机组是否已忙碌 - bool is_busy = is_busy_flag->load(); - if (is_busy) { - LOG(ERROR) << " Another thread is already controlling this motor group :" << group << ", skipping command."; - return; - } - - // 设置为忙碌 - is_busy_flag->store(true); - - // 使用 mutex 锁住当前电机组，保证只有一个线程在控制 - std::lock_guard lock(*group_mutex); - - // 1. 读当前关节角 - std::unordered_map cur_joints_angle; - cur_joints_angle.reserve(cmd.size()); - for (auto &jp: cmd) { - cur_joints_angle.emplace(jp.joint_name, 0.0); // 预填 key - } - - getJointQ(cur_joints_angle); - - std::vector q_start, q_goal; - q_start.reserve(cmd.size()); - q_goal.reserve(cmd.size()); - - for (auto &jp: cmd) { - auto it = cur_joints_angle.find(jp.joint_name); - if (it == cur_joints_angle.end()) { - LOG(ERROR) << "missing current angle for " << jp.joint_name; - return; - } - q_start.push_back(it->second); - q_goal.push_back(jp.rad); - } - if (q_start.size() != q_goal.size()) { - LOG(ERROR) << "q_start size != q_goal size"; - return; - } - - // 2. 轨迹规划 - TrajPtr traj; - joint_space_planner_->setPathType(PathType::Quintic); - joint_space_planner_->setSymmetricLimits(std::vector(cmd.size(), vel), - std::vector(cmd.size(), acc)); - - bool ok = joint_space_planner_->plan(q_start, q_goal, traj); - if (!ok) { - LOG(ERROR) << "planner plan() failed"; - return; - } - - // 3. 采样 - const double dt = 0.001; - auto samples = joint_space_planner_->sampleTrajectory(traj, dt); - if (samples.size() < 2) { - LOG(ERROR) << "trajectory too short"; - return; - } - - // 4. 切模式、设速度 - std::vector> motors; - motors.reserve(cmd.size()); - for (auto &jp: cmd) { - auto motor = motor_manager_->getMotor(jp.joint_name); - if (!motor) { - LOG(ERROR) << "getMotor failed for " << jp.joint_name; - return; - } - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - } - // motor->setQd(vel); - motors.push_back(motor); - } - - // 5. 下发命令 - const auto t0 = std::chrono::steady_clock::now(); - size_t k = 1; - const size_t K = samples.size(); - - while (k < K) { - const auto &s = samples[k]; - for (size_t i = 0; i < motors.size(); ++i) { - // motors[i]->setQ(s.q[i]); - motors[i]->setTarget(s.q[i],s.qd[i]); - } - - ++k; - if (k < K) { - auto next_t = t0 + std::chrono::duration(k * dt); - std::this_thread::sleep_until(next_t); - } - } - - // 6 输出误差 - getJointQ(cur_joints_angle); - for (auto &jp: cmd) { - auto it = cur_joints_angle.find(jp.joint_name); - if (it == cur_joints_angle.end()) { - LOG(ERROR) << "missing current angle for " << jp.joint_name; - return; - } - LOG(INFO) << jp.joint_name << " pos err = " << std::abs(it->second - jp.rad); - } - - // 结束后恢复状态 - is_busy_flag->store(false); -} - - - - - -template -void HumanoidRobot::calibrateZeroQ(const std::string &joint_name) { - stopSpeedLWorker_(); - auto motor = motor_manager_->getMotor(joint_name); - motor->calibrateZeroQ(); -} - -template -void HumanoidRobot::moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, - double acc) { - try { - stopSpeedLWorker_(); - // update m_state_ - Eigen::Vector q_init; - auto q_map = getJointQ(); - q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], - q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], - q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], - q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; - - // LOG(INFO) << "q_init: " << q_init; - m_state_->SetQ(q_init); - m_robot_->ComputeForwardKinematics(m_state_); - - - Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity(); - T_target.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz()); - // 输入为弧度 - T_target(0, 3) = pose.position().x(); - T_target(1, 3) = pose.position().y(); - T_target(2, 3) = pose.position().z(); - - cmvr::ctrl::PoseTarget target; - target.T_target = T_target; - target.w_posrot = 0.5; - target.weight = 1.0; - target.link_name = ee_link; - - // slove ik - Eigen::Vector q_cmd; - bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, - ctrl::CartesianController::Mode::Position, - q_cmd, 10000, 1e-6); - if (!ok) { - throw runtime_error("solve IK failed"); - } - std::vector joint_points{ - {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]}, - {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]}, - {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]}, - {"R_WRIST_R", q_cmd[13]} - }; - for (const auto &j: joint_points) { - auto motor = motor_manager_->getMotor(j.joint_name); - if (motor != nullptr) { - // PPM 模式下 这个实际速度会超30% 左右 - - if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { - motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); - } - motor->setQd(vel); - motor->setQ(j.rad); - } - } - - //3. wait for completion - bool completion = true; - do { - completion = true; - for (const auto &j: joint_points) { - auto motor = motor_manager_->getMotor(j.joint_name); - if (motor != nullptr) { - if (!motor->reachedTargetQ()) { - completion = false; - break; - } - } - } - // 4. while waiting, check flash_cmd_, if it is true, set it false then exit - if (flash_cmd_.load()) { - flash_cmd_.store(false); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } while (!completion); - } catch (exception &e) { - throw runtime_error(e.what()); - } -} - - -template -void HumanoidRobot::moveJ_IK(const std::string &base_link, const std::vector &targets, - double vel, - double acc) { - try { - stopSpeedLWorker_(); - // update m_state_ - Eigen::Vector q_init; - auto q_map = getJointQ(); - q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], - q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], - q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], - q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; - - LOG(INFO) << "q_init: " << q_init; - m_state_->SetQ(q_init); - m_robot_->ComputeForwardKinematics(m_state_); - - // slove ik - Eigen::Vector q_cmd; - bool ok = m_cctrl_->compute(m_state_, base_link, targets, 0.002, ctrl::CartesianController::Mode::Position, - q_cmd, 10000, 1e-6); - if (!ok) { - throw runtime_error("solve IK failed"); - } - std::vector joint_points{ - {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]}, - {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]}, - {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]}, - {"R_WRIST_R", q_cmd[13]} - }; - // for (const auto &j: joint_points) { - // auto motor = motor_manager_->getMotor(j.joint_name); - // if (motor != nullptr) { - // // PPM 模式下 这个实际速度会超30% 左右 - // motor->setQd(vel); - // if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { - // motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); - // } - // motor->setQ(j.rad); - // } - // } - // - // //3. wait for completion - // bool completion = true; - // do { - // completion = true; - // for (const auto &j: joint_points) { - // auto motor = motor_manager_->getMotor(j.joint_name); - // if (motor != nullptr) { - // if (!motor->reachedTargetQ()) { - // completion = false; - // break; - // } - // } - // } - // // 4. while waiting, check flash_cmd_, if it is true, set it false then exit - // if (flash_cmd_.load()) { - // flash_cmd_.store(false); - // return; - // } - // std::this_thread::sleep_for(std::chrono::milliseconds(2)); - // } while (!completion); - } catch (exception &e) { - throw runtime_error(e.what()); - } -} - - -template -void HumanoidRobot::speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) { - try { - stopSpeedLWorker_(); - // 获取电机控制对象 - // 这里的控制函数需要根据你的实际实现来进行填充 - // 获取目标关节的电机 - auto motor = motor_manager_->getMotor(joint_name); - if (motor == nullptr) { - throw runtime_error("Motor not found for joint: " + joint_name); - } - - - // 设置电机的运行模式为速度模式 - if (motor->getMode() != msgs::RUN_MODE_PROFILE_VELOCITY) { - motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); - } - - // 设置加速度和目标速度 - motor->setQd(vel); - - - LOG(INFO) << "开始在关节 " << joint_name << " 上进行速度控制，速度：" << vel << " rad/s，加速度：" << acc << " rad/s²"; - std::this_thread::sleep_for(std::chrono::seconds(10)); - motor->setQd(0); - LOG(INFO) << "速度控制完成，电机已停止。"; - - - // 运动完成后不立即将 rsm_ 置为 READY，防止误操作 - } catch (exception &e) { - LOG(ERROR) << "speedJ 控制失败: " << e.what(); - throw runtime_error("speedJ 控制失败: " + string(e.what())); - } -} - - - -template -void HumanoidRobot::servoJ(std::vector &joints, double dt) { - stopSpeedLWorker_(); - for (const auto &j: joints) { - auto motor = motor_manager_->getMotor(j.joint_name); - if (motor != nullptr) { - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - } - motor->setTarget(j.rad,j.vel); - } - } - rsm_.store(ROBOT_READY); -} - -template -void HumanoidRobot::servoJ(std::vector &joints, double vel, double dt) { - stopSpeedLWorker_(); - for (const auto &j: joints) { - auto motor = motor_manager_->getMotor(j.joint_name); - if (motor != nullptr) { - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - } - motor->setTarget(j.rad,vel); - } - } -} - -template -void HumanoidRobot::servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, - double acc) { - try { - stopSpeedLWorker_(); - // update m_state_ - Eigen::Vector q_init; - auto q_map = getJointQ(); - q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], - q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], - q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], - q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; - - LOG(INFO) << "q_init: " << q_init; - m_state_->SetQ(q_init); - m_robot_->ComputeForwardKinematics(m_state_); - - - Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity(); - T_target.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz()); - // 输入为弧度 - T_target(0, 3) = pose.position().x(); - T_target(1, 3) = pose.position().y(); - T_target(2, 3) = pose.position().z(); - - cmvr::ctrl::PoseTarget target; - target.T_target = T_target; - target.w_posrot = 0.5; - target.weight = 1.0; - target.link_name = ee_link; - - // slove ik - Eigen::Vector q_cmd; - bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, - ctrl::CartesianController::Mode::Position, - q_cmd, 10000, 1e-6); - if (!ok) { - throw runtime_error("solve IK failed"); - } - std::vector joint_points{ - {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]}, - {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]}, - {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]}, - {"R_WRIST_R", q_cmd[13]} - }; - servoJ(joint_points, vel, 0.1); - } catch (exception &e) { - throw runtime_error(e.what()); - } -} - - -template -void HumanoidRobot::servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, - double vel, double acc) { - try { - // 1 : 计算当前位姿 - auto cur_pose = fk(base_link, ee_link); - - // 2 : 计算目标角度 target pos = cur_pose + delta_pose - cmvr::msgs::Pose3d target_pose; - target_pose.mutable_position()->set_x(cur_pose.position().x() + delta_pose.position().x()); - target_pose.mutable_position()->set_y(cur_pose.position().y() + delta_pose.position().y()); - target_pose.mutable_position()->set_z(cur_pose.position().z() + delta_pose.position().z()); - - target_pose.mutable_euler()->set_rx(cur_pose.euler().rx() + delta_pose.euler().rx()); - target_pose.mutable_euler()->set_ry(cur_pose.euler().ry() + delta_pose.euler().ry()); - target_pose.mutable_euler()->set_rz(cur_pose.euler().rz() + delta_pose.euler().rz()); - - //3 : - servoJ(base_link, ee_link, target_pose, vel, acc); - } catch (exception &e) { - throw runtime_error(e.what()); - } -} - - -template -void HumanoidRobot::servoL(std::string &base_link, std::vector &targets, double dt) { - try { - stopSpeedLWorker_(); - Eigen::Vector q_cmd; - bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::CartesianController::Mode::Position, - q_cmd, 60, 1e-4); - if (!ok) { - LOG(WARNING) << "[HumanoidRobot] (servoL): solve IK failed, id=" << id_; - throw runtime_error("IK failed"); - } - std::vector joints(dof_); - for (size_t i = 0; i < dof_; i++) { - joints[i].joint_name = joint_names_[i]; - joints[i].rad = q_cmd[i]; - } - servoJ(joints, dt); - } catch (exception &e) { - throw runtime_error(e.what()); - } -} - -template -bool HumanoidRobot::check_joint_traj_(std::vector > &traj, double dt) { - // TODO: to be implemented - return true; -} - -template -void HumanoidRobot::moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, - double vel, double acc) { - try { - // 1 : 计算当前位姿 - auto cur_pose = fk(base_link, ee_link); - - // 2 : 计算目标角度 target pos = cur_pose + delta_pose - cmvr::msgs::Pose3d target_pose; - target_pose.mutable_position()->set_x(cur_pose.position().x() + delta_pose.position().x()); - target_pose.mutable_position()->set_y(cur_pose.position().y() + delta_pose.position().y()); - target_pose.mutable_position()->set_z(cur_pose.position().z() + delta_pose.position().z()); - - target_pose.mutable_euler()->set_rx(cur_pose.euler().rx() + delta_pose.euler().rx()); - target_pose.mutable_euler()->set_ry(cur_pose.euler().ry() + delta_pose.euler().ry()); - target_pose.mutable_euler()->set_rz(cur_pose.euler().rz() + delta_pose.euler().rz()); - - //3 : - moveJ(base_link, ee_link, target_pose, vel, acc); - } catch (exception &e) { - throw runtime_error(e.what()); - } -} - - -template -void HumanoidRobot::update_state_() { - std::lock_guard lock(state_mtx_); - // TODO: set m_state_ - // m_state_->SetQ(); - // m_state_->SetQdot(); - // m_state_->SetQddot(); -} - -template -Eigen::Matrix3d HumanoidRobot::eulerZYXToRotationMatrix(double rx, double ry, double rz) { - Eigen::Matrix3d R_x; - R_x << 1, 0, 0, - 0, cos(rx), -sin(rx), - 0, sin(rx), cos(rx); - - Eigen::Matrix3d R_y; - R_y << cos(ry), 0, sin(ry), - 0, 1, 0, - -sin(ry), 0, cos(ry); - - Eigen::Matrix3d R_z; - R_z << cos(rz), -sin(rz), 0, - sin(rz), cos(rz), 0, - 0, 0, 1; - - return R_x * R_y * R_z; -} - -template -Eigen::Vector3d HumanoidRobot::rotationMatrixToEulerZYX(const Eigen::Matrix3d &R) { - double rx, ry, rz; - - // 根据 R = R_x * R_y * R_z - // R = | cy*cz -cy*sz sy | - // | sx*sy*cz + cx*sz -sx*sy*sz + cx*cz -sx*cy | - // | -cx*sy*cz + sx*sz cx*sy*sz + sx*cz cx*cy | - - // 提取 ry（绕 Y 的角度） - ry = std::asin(R(0, 2)); // R(0,2) = sin(ry) - - double cy = std::cos(ry); - - if (std::abs(cy) > 1e-6) { - // 正常情况 - rx = std::atan2(-R(1, 2), R(2, 2)); - rz = std::atan2(-R(0, 1), R(0, 0)); - } else { - // 万向节锁：cy ≈ 0 - rx = 0; // 任意选择 - if (ry > 0) { - rz = std::atan2(R(1, 0), R(1, 1)); - } else { - rz = std::atan2(-R(1, 0), R(1, 1)); - } - } - - return Eigen::Vector3d(rx, ry, rz); -} - -template -std::vector HumanoidRobot< - DOF>::ik(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose) { - // update m_state_ - - try { - Eigen::Vector q_init; - auto q_map = getJointQ(); - q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], - q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], - q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], - q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; - - LOG(INFO) << "q_init: " << q_init; - m_state_->SetQ(q_init); - m_robot_->ComputeForwardKinematics(m_state_); - - - Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity(); - T_target.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz()); - // 输入为弧度 - T_target(0, 3) = pose.position().x(); - T_target(1, 3) = pose.position().y(); - T_target(2, 3) = pose.position().z(); - - cmvr::ctrl::PoseTarget target; - target.T_target = T_target; - target.w_posrot = 0.5; - target.weight = 1.0; - target.link_name = ee_link; - - // slove ik - Eigen::Vector q_cmd{}; - bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, - ctrl::CartesianController::Mode::Position, - q_cmd, 10000, 1e-6); - if (!ok) { - throw std::runtime_error("IK solve failed"); - } else { - return std::vector(q_cmd.data(), q_cmd.data() + q_cmd.size()); - } - } catch (std::exception &e) { - throw runtime_error(e.what()); - } -} - - -template -cmvr::msgs::Pose3d HumanoidRobot::fk(const std::string &base_link, const std::string &ee_link) { - cmvr::msgs::Pose3d pose; - static const std::vector kRightArmJointNames = { - "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", - "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R" - }; - - try { - if (!ik_solver_) { - throw std::runtime_error("ik_solver_ is not initialized"); - } - - const auto q_map = getJointQ(); - std::vector q_current; - q_current.reserve(kRightArmJointNames.size()); - for (const auto& joint_name : kRightArmJointNames) { - const auto it = q_map.find(joint_name); - if (it == q_map.end()) { - throw std::runtime_error("missing joint state for " + joint_name); - } - q_current.push_back(it->second); - } - - Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); - if (!ik_solver_->fk(base_link, ee_link, q_current, T)) { - throw std::runtime_error("PinocchioDlsIKSolver::fk failed"); - } - - pose.mutable_position()->set_x(T(0, 3)); - pose.mutable_position()->set_y(T(1, 3)); - pose.mutable_position()->set_z(T(2, 3)); - - Eigen::Matrix3d R = T.block<3, 3>(0, 0); - const Eigen::Quaterniond q(R); - pose.mutable_quaternion()->set_w(q.w()); - pose.mutable_quaternion()->set_x(q.x()); - pose.mutable_quaternion()->set_y(q.y()); - pose.mutable_quaternion()->set_z(q.z()); - - Eigen::Vector3d euler = rotationMatrixToEulerZYX(R); - pose.mutable_euler()->set_rx(euler(0)); - pose.mutable_euler()->set_ry(euler(1)); - pose.mutable_euler()->set_rz(euler(2)); - } catch (const std::exception &e) { - throw std::runtime_error(std::string("FK计算失败: ") + e.what()); - } - - return pose; -} - -template -cmvr::msgs::Pose3d HumanoidRobot::fk(bool is_tcp) { - cmvr::msgs::Pose3d pose; - static const std::vector kRightArmJointNames = { - "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", - "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R" - }; - - try { - if (!ik_solver_) { - throw std::runtime_error("ik_solver_ is not initialized"); - } - - const auto q_map = getJointQ(); - std::vector q_current; - q_current.reserve(kRightArmJointNames.size()); - for (const auto& joint_name : kRightArmJointNames) { - const auto it = q_map.find(joint_name); - if (it == q_map.end()) { - throw std::runtime_error("missing joint state for " + joint_name); - } - q_current.push_back(it->second); - } - - Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); - if (!ik_solver_->fk(q_current, T, is_tcp)) { - throw std::runtime_error("PinocchioDlsIKSolver::fk failed"); - } - - pose.mutable_position()->set_x(T(0, 3)); - pose.mutable_position()->set_y(T(1, 3)); - pose.mutable_position()->set_z(T(2, 3)); - - const Eigen::Matrix3d R = T.block<3, 3>(0, 0); - const Eigen::Quaterniond q(R); - pose.mutable_quaternion()->set_w(q.w()); - pose.mutable_quaternion()->set_x(q.x()); - pose.mutable_quaternion()->set_y(q.y()); - pose.mutable_quaternion()->set_z(q.z()); - - const Eigen::Vector3d euler = rotationMatrixToEulerZYX(R); - pose.mutable_euler()->set_rx(euler(0)); - pose.mutable_euler()->set_ry(euler(1)); - pose.mutable_euler()->set_rz(euler(2)); - } catch (const std::exception &e) { - throw std::runtime_error(std::string("FK计算失败: ") + e.what()); - } - - return pose; -} - - -template -bool HumanoidRobot::moveL(const std::vector &pose, - double speed, - double acceleration, - double jerk, - const std::vector &qd_max, - bool asynchronous) { - stopSpeedLWorker_(); - - if (asynchronous) { - LOG(ERROR) << "moveL: asynchronous=true is not supported yet"; - return false; - } - if (!ik_solver_) { - LOG(ERROR) << "moveL: ik_solver_ is not initialized"; - return false; - } - if (pose.size() != 6) { - LOG(ERROR) << "moveL: pose size must be 6 [x, y, z, rx, ry, rz], got " << pose.size(); - return false; - } - if (speed <= 0.0 || acceleration <= 0.0) { - LOG(ERROR) << "moveL: speed and acceleration must be positive"; - return false; - } - if (jerk <= 0.0) { - LOG(ERROR) << "moveL: jerk must be positive"; - return false; - } - - Eigen::Matrix4d target_pose_base = Eigen::Matrix4d::Identity(); - target_pose_base.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose[3], pose[4], pose[5]); - target_pose_base(0, 3) = pose[0]; - target_pose_base(1, 3) = pose[1]; - target_pose_base(2, 3) = pose[2]; - - const double dt_real = 0.001; - return executeMoveLScurve_(target_pose_base, - speed, - acceleration, - jerk, - qd_max, - dt_real, - true, - cmvr::CartesianFrame::Tool); -} - -template -bool HumanoidRobot::executeMoveLScurve_(const Eigen::Matrix4d& target_pose_input, - double v_tcp_max, - double a_tcp_max, - double j_tcp_max, - const std::vector& qd_max, - double dt_real, - bool is_tcp, - cmvr::CartesianFrame input_frame) { - static const std::vector kRightArmJointNames = { - "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", - "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R" - }; - - if (!ik_solver_) { - LOG(ERROR) << "executeMoveLScurve_: ik_solver_ is not initialized"; - return false; - } - if (v_tcp_max <= 0.0 || a_tcp_max <= 0.0 || j_tcp_max <= 0.0 || dt_real <= 0.0) { - LOG(ERROR) << "executeMoveLScurve_: invalid parameters, v=" << v_tcp_max - << ", a=" << a_tcp_max - << ", j=" << j_tcp_max - << ", dt=" << dt_real; - return false; - } - - std::lock_guard lock(right_arm_mutex_); - if (is_right_arm_busy_.exchange(true)) { - LOG(ERROR) << "executeMoveLScurve_: right arm is busy"; - return false; - } - - struct BusyGuard { - std::atomic& busy_flag; - ~BusyGuard() { busy_flag.store(false); } - } busy_guard{is_right_arm_busy_}; - - try { - std::vector q_start; - q_start.reserve(kRightArmJointNames.size()); - const auto q_map = getJointQ(); - for (const auto& joint_name : kRightArmJointNames) { - const auto it = q_map.find(joint_name); - if (it == q_map.end()) { - LOG(ERROR) << "executeMoveLScurve_: missing joint state for " << joint_name; - return false; - } - q_start.push_back(it->second); - } - - if (!qd_max.empty() && qd_max.size() != kRightArmJointNames.size()) { - LOG(ERROR) << "executeMoveLScurve_: qd_max size mismatch, expect " - << kRightArmJointNames.size() << ", got " << qd_max.size(); - return false; - } - - std::vector> q_traj; - std::vector> qdot_traj; - std::vector t_traj; - if (!ik_solver_->moveL_SCurveLocal(target_pose_input, - q_start, - q_traj, - t_traj, - dt_real, - v_tcp_max, - a_tcp_max, - j_tcp_max, - qd_max, - is_tcp, - input_frame, - &qdot_traj)) { - LOG(ERROR) << "executeMoveLScurve_: moveL_SCurveLocal failed"; - return false; - } - if (q_traj.empty() || t_traj.size() != q_traj.size()) { - LOG(ERROR) << "executeMoveLScurve_: invalid trajectory output"; - return false; - } - if (qdot_traj.size() != q_traj.size()) { - LOG(WARNING) << "executeMoveLScurve_: qdot_traj size mismatch, use zero fallback"; - qdot_traj.assign(q_traj.size(), std::vector(kRightArmJointNames.size(), 0.0)); - } - if (q_traj.size() == 1) { - return true; - } - - auto next_deadline = std::chrono::steady_clock::now(); - for (size_t i = 1; i < q_traj.size(); ++i) { - const double dt_segment = std::max(1e-4, t_traj[i] - t_traj[i - 1]); - std::vector joint_cmd; - joint_cmd.reserve(kRightArmJointNames.size()); - for (size_t j = 0; j < kRightArmJointNames.size(); ++j) { - const double qd_abs = (j < qdot_traj[i].size()) ? std::abs(qdot_traj[i][j]) : 0.0; - joint_cmd.emplace_back(kRightArmJointNames[j], q_traj[i][j], qd_abs); - } - - servoJ(joint_cmd, dt_segment); - next_deadline += std::chrono::duration_cast( - std::chrono::duration(dt_segment)); - std::this_thread::sleep_until(next_deadline); - } - - LOG(INFO) << "executeMoveLScurve_: completed, samples=" << q_traj.size() - << ", duration_s=" << t_traj.back(); - return true; - } catch (const std::exception& e) { - LOG(ERROR) << "executeMoveLScurve_ failed: " << e.what(); - return false; - } -} - -template -cmvr::math::Pose3d HumanoidRobot::getTransform(std::string &base_link, std::string &target_link) { - // 获取gRPC生成的Pose3d消息 - auto grpc_pose = fk(base_link, target_link); - - // 转换为cmvr::math::Pose3d - cmvr::math::Pose3d math_pose; - - // 转换位置信息 - math_pose.position.x = grpc_pose.position().x(); - math_pose.position.y = grpc_pose.position().y(); - math_pose.position.z = grpc_pose.position().z(); - - // 转换四元数 - math_pose.quaternion.w = grpc_pose.quaternion().w(); - math_pose.quaternion.x = grpc_pose.quaternion().x(); - math_pose.quaternion.y = grpc_pose.quaternion().y(); - math_pose.quaternion.z = grpc_pose.quaternion().z(); - - // 转换欧拉角 - math_pose.euler.rx = grpc_pose.euler().rx(); - math_pose.euler.ry = grpc_pose.euler().ry(); - math_pose.euler.rz = grpc_pose.euler().rz(); - - return math_pose; -} - -template class cmvr::device::HumanoidRobot<7>; -template class cmvr::device::HumanoidRobot<14>; -template class cmvr::device::HumanoidRobot<20>; diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp deleted file mode 100644 index d1b47e4e..00000000 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp +++ /dev/null @@ -1,992 +0,0 @@ -// -// Created by lgv on 2025/8/4. -// - - -// #include "humanoid_robot.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "gtest/gtest.h" -#include -#include -#include "../../../../device_manager/include/device_manager.h" -#include - -#include "common/utils/image/image_process.h" -#include "controller/include/ibvs_controller.h" -#include "ik_solver/include/pinocchio_dls_ik_solver.h" -#include "perception/include/apriltag_perception.h" -#include "perception/include/tag_relative_target_3d.h" -#include "cmvr/msgs/can_card_parameter.grpc.pb.h" -#include "../include/humanoid_robot.h" -#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" -#include "../../../../utils/base/include/abstract_interpolation.h" -#include "data_center/include/motors_info.h" - -using namespace cmvr::device; - -namespace { - -constexpr std::array kRightArmJointNames = { - "R_SHOULDER_P", - "R_SHOULDER_R", - "R_SHOULDER_Y", - "R_ELBOW_R", - "R_WRIST_P", - "R_WRIST_Y", - "R_WRIST_R" -}; - -std::vector buildRightArmJointCmd(const std::vector& q_cmd, - double vel) { - std::vector cmd; - cmd.reserve(kRightArmJointNames.size()); - for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { - cmd.emplace_back(kRightArmJointNames[i], q_cmd[i], vel); - } - return cmd; -} - -std::vector buildRightArmJointCmdWithVel(const std::vector& q_cmd, - const std::vector& qd_abs, - double vel_fallback = 0.1) { - std::vector cmd; - cmd.reserve(kRightArmJointNames.size()); - for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { - double vel = vel_fallback; - if (i < qd_abs.size() && std::isfinite(qd_abs[i]) && qd_abs[i] > 0.0) { - vel = qd_abs[i]; - } - cmd.emplace_back(kRightArmJointNames[i], q_cmd[i], vel); - } - return cmd; -} - -std::unordered_map makeRightArmQMap() { - std::unordered_map q_map; - q_map.reserve(kRightArmJointNames.size()); - for (const auto* name : kRightArmJointNames) { - q_map.emplace(name, 0.0); - } - return q_map; -} - -bool readRightArmQ(const std::shared_ptr& robot, std::vector& q_now) { - if (!robot) { - return false; - } - auto q_map_now = makeRightArmQMap(); - robot->getJointQ(q_map_now); - q_now.assign(kRightArmJointNames.size(), 0.0); - for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { - const auto it = q_map_now.find(kRightArmJointNames[i]); - if (it == q_map_now.end()) { - return false; - } - q_now[i] = it->second; - } - return true; -} - -} // namespace - - - - - - - - - -TEST(HumanoidRobotTest, speedLSmokeTest) { - const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); - ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; - auto dmgr_cfg = config.getChild("DeviceManager"); - auto& dmgr = DeviceManager::getInstance(dmgr_cfg); - - auto robot_abs = dmgr.getDevice("hc01"); - auto robot = std::dynamic_pointer_cast>(robot_abs); - ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>"; - - std::vector init_cmd{}; - init_cmd = { - {"R_SHOULDER_P", -0.2423}, - {"R_SHOULDER_R", 1.2929}, - {"R_SHOULDER_Y", 1.61}, - {"R_ELBOW_R", 1.58}, - {"R_WRIST_P", -2.8792}, - {"R_WRIST_Y", 0.1150}, - {"R_WRIST_R", -0.08}, - }; - robot->moveJ(init_cmd,1.0,2.0); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - const std::vector twist_pos = {0.00, -0.08, 0, 0.00, 0, 0}; - const std::vector twist_neg = {0.00, 0.08, 0, 0.00, 0, 0}; - const double acceleration = 3.0; - const double segment_time = 1.0; - const double settle_time = 1.0; - const double sample_dt = 0.02; - - std::vector t_trace; - std::vector target_vx_trace; - std::vector command_vx_trace; - std::vector command_vy_trace; - std::vector command_vz_trace; - std::vector command_speed_trace; - - double t_now = 0.0; - auto sample_phase = [&](const std::vector& target_twist, - double duration, - const char* phase) { - LOG(INFO) << "speedLSmokeTest phase: " << phase; - ASSERT_NO_THROW(robot->speedL(target_twist, acceleration, 0.0, cmvr::CartesianFrame::Tool)); - const int steps = static_cast(std::ceil(duration / sample_dt)); - for (int i = 0; i < steps; ++i) { - const Eigen::Matrix cmd_twist = robot->getSpeedLCommandTwistBase(); - t_trace.push_back(t_now); - target_vx_trace.push_back(target_twist[0]); - command_vx_trace.push_back(cmd_twist[0]); - command_vy_trace.push_back(cmd_twist[1]); - command_vz_trace.push_back(cmd_twist[2]); - command_speed_trace.push_back(cmd_twist.head<3>().norm()); - std::this_thread::sleep_for(std::chrono::duration(sample_dt)); - t_now += sample_dt; - } - }; - - robot->speedL({0.0, 0.0, 0.0, 0.0, 0.00, 0.0}, acceleration, 0.0); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - - sample_phase(twist_pos, segment_time*0.8, "+X"); - sample_phase(twist_neg, segment_time, "-X"); - - - LOG(INFO) << "speedLSmokeTest phase: stop"; - robot->speedL({0.0, 0.0, 0.0, 0.0, 0.00, 0.0}, acceleration, 1); - // robot->moveJ(init_cmd,1.0,2.0); - // ASSERT_NO_THROW(robot->stopSpeedL()); - const int settle_steps = static_cast(std::ceil(settle_time / sample_dt)); - for (int i = 0; i < settle_steps; ++i) { - const Eigen::Matrix cmd_twist = robot->getSpeedLCommandTwistBase(); - t_trace.push_back(t_now); - target_vx_trace.push_back(0.0); - command_vx_trace.push_back(cmd_twist[0]); - command_vy_trace.push_back(cmd_twist[1]); - command_vz_trace.push_back(cmd_twist[2]); - command_speed_trace.push_back(cmd_twist.head<3>().norm()); - std::this_thread::sleep_for(std::chrono::duration(sample_dt)); - t_now += sample_dt; - } - - ASSERT_FALSE(t_trace.empty()); - - using namespace matplot; - auto fig = figure(true); - fig->size(1600, 1000); - fig->font_size(16); - - auto ax1 = subplot(2, 1, 0); - hold(ax1, true); - auto l_target = plot(ax1, t_trace, target_vx_trace, "k--"); - l_target->line_width(2.0f); - auto l_cmd_x = plot(ax1, t_trace, command_vx_trace, "r-"); - l_cmd_x->line_width(2.0f); - auto l_cmd_y = plot(ax1, t_trace, command_vy_trace, "g-"); - l_cmd_y->line_width(2.0f); - auto l_cmd_z = plot(ax1, t_trace, command_vz_trace, "b-"); - l_cmd_z->line_width(2.0f); - title(ax1, "speedL target vx vs command vxyz"); - xlabel(ax1, "time [s]"); - ylabel(ax1, "linear cmd [m/s]"); - legend(ax1, {"target vx", "command vx", "command vy", "command vz"}); - grid(ax1, true); - - auto ax2 = subplot(2, 1, 1); - hold(ax2, true); - auto l_norm = plot(ax2, t_trace, command_speed_trace, "m-"); - l_norm->line_width(2.0f); - title(ax2, "speedL command speed norm"); - xlabel(ax2, "time [s]"); - ylabel(ax2, "norm [m/s]"); - legend(ax2, {"||command v||"}); - grid(ax2, true); - save("/home/lgv/cmvr/cmvr-es/data/plot1.png"); - std::cout << "Hello World" << std::endl; - // show(fig); -} - -TEST(HumanoidRobotTest, moveLSCurveLocalRealRobot) { - const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); - ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; - auto dmgr_cfg = config.getChild("DeviceManager"); - auto& dmgr = DeviceManager::getInstance(dmgr_cfg); - - auto robot_abs = dmgr.getDevice("hc01"); - auto robot = std::dynamic_pointer_cast>(robot_abs); - ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>"; - - std::vector init_cmd = { - {"R_SHOULDER_P", -0.2423}, - {"R_SHOULDER_R", 1.2929}, - {"R_SHOULDER_Y", 1.61}, - {"R_ELBOW_R", 1.58}, - {"R_WRIST_P", -2.8792}, - {"R_WRIST_Y", 0.1150}, - {"R_WRIST_R", 0.1}, - }; - robot->moveJ(init_cmd, 1.0, 2.0); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - struct ReturnHomeGuard { - std::shared_ptr> robot; - std::vector home; - ~ReturnHomeGuard() { - if (!robot) { - return; - } - try { - robot->moveJ(home, 1.0, 2.0); - } catch (...) { - } - } - } guard{robot, init_cmd}; - - std::vector q_start; - ASSERT_TRUE(readRightArmQ(robot_abs, q_start)) << "Failed to read right arm joint state"; - - cmvr::PinocchioDlsIKSolver solver( - "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf", - "PELVIS_S", - "R_WRIST_R_S", - "R_FINGER_TIP_FIXED"); - ASSERT_TRUE(solver.init()) << "PinocchioDlsIKSolver init failed"; - - Eigen::Matrix4d target_pose_input = Eigen::Matrix4d::Identity(); - const cmvr::CartesianFrame input_frame = cmvr::CartesianFrame::Tool; - target_pose_input(1, 3) = -0.08; - - const double dt_gen = 0.001; - const double v_tcp = 0.08; - const double a_tcp = 5; - const double j_tcp = 5; - const bool is_tcp = true; - std::vector qd_max(7, 3.0); - - std::vector> q_traj; - std::vector> qdot_traj; - std::vector t_traj; - ASSERT_TRUE(solver.moveL_SCurveLocal(target_pose_input, - q_start, - q_traj, - t_traj, - dt_gen, - v_tcp, - a_tcp, - j_tcp, - qd_max, - is_tcp, - input_frame, - &qdot_traj)) - << "moveL_SCurveLocal failed"; - - ASSERT_EQ(q_traj.size(), t_traj.size()) << "q_traj and t_traj size mismatch"; - ASSERT_EQ(q_traj.size(), qdot_traj.size()) << "q_traj and qdot_traj size mismatch"; - ASSERT_FALSE(q_traj.empty()) << "Generated trajectory is empty"; - - LOG(INFO) << "moveLSCurveLocalRealRobot generated samples=" << q_traj.size() - << ", duration_s=" << (t_traj.empty() ? 0.0 : t_traj.back()) - << ", frame=" << (input_frame == cmvr::CartesianFrame::Tool ? "Tool" : "Base") - << ", delta_xyz=[" << target_pose_input(0, 3) << ", " - << target_pose_input(1, 3) << ", " - << target_pose_input(2, 3) << "]"; - - auto next_deadline = std::chrono::steady_clock::now(); - for (size_t i = 1; i < q_traj.size(); ++i) { - const double dt_segment = std::max(0.0, t_traj[i] - t_traj[i - 1]); - std::vector qd_abs = qdot_traj[i]; - for (double& v : qd_abs) { - v = std::abs(v); - } - auto joint_cmd = buildRightArmJointCmdWithVel(q_traj[i], qd_abs, 0.05); - ASSERT_NO_THROW(robot->servoJ(joint_cmd, dt_segment)); - - next_deadline += std::chrono::duration_cast( - std::chrono::duration(dt_segment)); - std::this_thread::sleep_until(next_deadline); - } - - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - - std::vector q_end; - ASSERT_TRUE(readRightArmQ(robot_abs, q_end)) << "Failed to read right arm joint state at end"; - - Eigen::Matrix4d T_end = Eigen::Matrix4d::Identity(); - ASSERT_TRUE(solver.fk(q_end, T_end, is_tcp)) << "Failed to compute FK for end joint state"; - - Eigen::Matrix4d T_start = Eigen::Matrix4d::Identity(); - ASSERT_TRUE(solver.fk(q_start, T_start, is_tcp)) << "Failed to compute FK for start joint state"; - - Eigen::Matrix4d T_plan_end = Eigen::Matrix4d::Identity(); - ASSERT_TRUE(solver.fk(q_traj.back(), T_plan_end, is_tcp)) << "Failed to compute FK for planned end joint state"; - - const Eigen::Matrix3d R_start = T_start.block<3,3>(0, 0); - const Eigen::Matrix3d R_plan_end = T_plan_end.block<3,3>(0, 0); - const Eigen::Matrix3d R_end = T_end.block<3,3>(0, 0); - - const Eigen::Vector3d delta_tcp_plan = T_plan_end.block<3,1>(0,3) - T_start.block<3,1>(0,3); - const Eigen::Vector3d delta_tcp = T_end.block<3,1>(0,3) - T_start.block<3,1>(0,3); - const Eigen::Vector3d delta_tcp_error = delta_tcp - delta_tcp_plan; - const double rot_err_start_to_actual_rad = - Eigen::AngleAxisd(R_start.transpose() * R_end).angle(); - const double rot_err_plan_to_actual_rad = - Eigen::AngleAxisd(R_plan_end.transpose() * R_end).angle(); - LOG(INFO) << "moveLSCurveLocalRealRobot tcp_delta_plan=[" << delta_tcp_plan.x() << ", " - << delta_tcp_plan.y() << ", " << delta_tcp_plan.z() << "]"; - LOG(INFO) << "moveLSCurveLocalRealRobot tcp_delta_actual=[" << delta_tcp.x() << ", " - << delta_tcp.y() << ", " << delta_tcp.z() << "]"; - LOG(INFO) << "moveLSCurveLocalRealRobot tcp_delta_error=[" << delta_tcp_error.x() << ", " - << delta_tcp_error.y() << ", " << delta_tcp_error.z() << "]"; - LOG(INFO) << "moveLSCurveLocalRealRobot rot_err_start_to_actual_deg=" - << rot_err_start_to_actual_rad * 180.0 / M_PI; - LOG(INFO) << "moveLSCurveLocalRealRobot rot_err_plan_to_actual_deg=" - << rot_err_plan_to_actual_rad * 180.0 / M_PI; -} - -TEST(HumanoidRobotTest, moveLRealRobot) { - const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); - ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; - auto dmgr_cfg = config.getChild("DeviceManager"); - auto& dmgr = DeviceManager::getInstance(dmgr_cfg); - - auto robot_abs = dmgr.getDevice("hc01"); - auto robot = std::dynamic_pointer_cast>(robot_abs); - ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>"; - - std::vector init_cmd = { - {"R_SHOULDER_P", -0.2423}, - {"R_SHOULDER_R", 1.2929}, - {"R_SHOULDER_Y", 1.61}, - {"R_ELBOW_R", 1.58}, - {"R_WRIST_P", -2.8792}, - {"R_WRIST_Y", 0.1150}, - {"R_WRIST_R", 0.1}, - }; - robot->moveJ(init_cmd, 1.0, 2.0); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - std::vector q_start; - ASSERT_TRUE(readRightArmQ(robot_abs, q_start)) << "Failed to read right arm joint state"; - - cmvr::PinocchioDlsIKSolver solver( - "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf", - "PELVIS_S", - "R_WRIST_R_S", - "R_FINGER_TIP_FIXED"); - ASSERT_TRUE(solver.init()) << "PinocchioDlsIKSolver init failed"; - - Eigen::Matrix4d target_pose_input = Eigen::Matrix4d::Identity(); - const cmvr::CartesianFrame input_frame = cmvr::CartesianFrame::Tool; - target_pose_input(1, 3) = -0.08; - - const double v_tcp = 0.08; - const double a_tcp = 5; - const double j_tcp = 5; - const bool is_tcp = true; - - const std::vector pose_cmd = { - target_pose_input(0, 3), - target_pose_input(1, 3), - target_pose_input(2, 3), - 0.0, - 0.0, - 0.0 - }; - - LOG(INFO) << "moveLRealRobot command" - << ", frame=" << (input_frame == cmvr::CartesianFrame::Tool ? "Tool" : "Base") - << ", delta_xyz=[" << target_pose_input(0, 3) << ", " - << target_pose_input(1, 3) << ", " - << target_pose_input(2, 3) << "]"; - - ASSERT_TRUE(robot->moveL(pose_cmd, v_tcp, a_tcp, j_tcp)) << "robot->moveL failed"; - - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - - std::vector q_end; - ASSERT_TRUE(readRightArmQ(robot_abs, q_end)) << "Failed to read right arm joint state at end"; - - Eigen::Matrix4d T_end = Eigen::Matrix4d::Identity(); - ASSERT_TRUE(solver.fk(q_end, T_end, is_tcp)) << "Failed to compute FK for end joint state"; - - Eigen::Matrix4d T_start = Eigen::Matrix4d::Identity(); - ASSERT_TRUE(solver.fk(q_start, T_start, is_tcp)) << "Failed to compute FK for start joint state"; - - Eigen::Matrix4d T_plan_end = Eigen::Matrix4d::Identity(); - T_plan_end = T_start; - if (input_frame == cmvr::CartesianFrame::Tool) { - T_plan_end.block<3,1>(0,3) = (T_start * target_pose_input).block<3,1>(0,3); - } else { - T_plan_end.block<3,1>(0,3) = target_pose_input.block<3,1>(0,3); - } - - const Eigen::Matrix3d R_start = T_start.block<3,3>(0, 0); - const Eigen::Matrix3d R_plan_end = T_plan_end.block<3,3>(0, 0); - const Eigen::Matrix3d R_end = T_end.block<3,3>(0, 0); - - const Eigen::Vector3d delta_tcp_plan = T_plan_end.block<3,1>(0,3) - T_start.block<3,1>(0,3); - const Eigen::Vector3d delta_tcp = T_end.block<3,1>(0,3) - T_start.block<3,1>(0,3); - const Eigen::Vector3d delta_tcp_error = delta_tcp - delta_tcp_plan; - const double rot_err_start_to_actual_rad = - Eigen::AngleAxisd(R_start.transpose() * R_end).angle(); - const double rot_err_plan_to_actual_rad = - Eigen::AngleAxisd(R_plan_end.transpose() * R_end).angle(); - LOG(INFO) << "moveLRealRobot tcp_delta_plan=[" << delta_tcp_plan.x() << ", " - << delta_tcp_plan.y() << ", " << delta_tcp_plan.z() << "]"; - LOG(INFO) << "moveLRealRobot tcp_delta_actual=[" << delta_tcp.x() << ", " - << delta_tcp.y() << ", " << delta_tcp.z() << "]"; - LOG(INFO) << "moveLRealRobot tcp_delta_error=[" << delta_tcp_error.x() << ", " - << delta_tcp_error.y() << ", " << delta_tcp_error.z() << "]"; - LOG(INFO) << "moveLRealRobot rot_err_start_to_actual_deg=" - << rot_err_start_to_actual_rad * 180.0 / M_PI; - LOG(INFO) << "moveLRealRobot rot_err_plan_to_actual_deg=" - << rot_err_plan_to_actual_rad * 180.0 / M_PI; -} - - -TEST(HumanoidRobotTest,IBVSWithRealRobot) { - const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); - ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; - auto dmgr_cfg = config.getChild("DeviceManager"); - auto& dmgr = DeviceManager::getInstance(dmgr_cfg); - - auto robot = dmgr.getDevice("hc01"); - auto camera = dmgr.getDevice("right_hand_cam"); - ASSERT_NE(robot, nullptr); - ASSERT_NE(camera, nullptr); - - std::vector init_cmd{}; - init_cmd = { - {"R_SHOULDER_P", -0.2423}, - {"R_SHOULDER_R", 1.2929}, - {"R_SHOULDER_Y", 1.61}, - {"R_ELBOW_R", 1.58}, - {"R_WRIST_P", -2.8792}, - {"R_WRIST_Y", 0.1150}, - {"R_WRIST_R", -0.08}, - }; - robot->moveJ(init_cmd,1.0,2.0); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - ASSERT_NO_THROW(camera->start()); - struct RuntimeGuard { - std::shared_ptr camera; - std::shared_ptr robot; - ~RuntimeGuard() { - if (camera) { - try { - camera->stop(); - } catch (...) { - } - } - if (robot) { - try { - robot->eStop(); - } catch (...) { - } - } - } - } guard{camera, robot}; - - const double tag_size_m = 0.12; - const int tracked_tag_id = 0; - const double target_x = 0.0; - const double target_y = 0.0; - const double target_z = 0.40; - const cmvr::IbvsController::DepthMode ibvs_depth_mode = cmvr::IbvsController::DepthMode::MONOCULAR; - - auto perception = std::make_shared(camera); - perception->setTagSize(tag_size_m); - cmvr::perception::AprilTagPerception::Options perception_opt; - switch (ibvs_depth_mode) { - case cmvr::IbvsController::DepthMode::MONOCULAR: - perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE; - break; - case cmvr::IbvsController::DepthMode::PREFER_DEPTH: - perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::PREFER; - break; - case cmvr::IbvsController::DepthMode::DEPTH_ONLY: - perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE; - break; - } - - cmvr::IbvsController ibvs_controller; - ibvs_controller.setMu(0.1); - ibvs_controller.setLambda(0.6); - ibvs_controller.setTrackedTagId(tracked_tag_id); - ibvs_controller.setTarget(target_x, target_y, target_z); - ibvs_controller.setDepthMode(ibvs_depth_mode); - ibvs_controller.setDepthZGain(1.0); - // Eigen::Matrix3d RxPi; - // RxPi << 1,0,0, - // 0,-1,0, - // 0,0,-1; - // ibvs_controller.setAlignCameraToUrdf(RxPi); - // ibvs_controller.setAlignCameraToVisp(RxPi); - - - - // ibvs_controller.setVelocityLimit6({0.3, 0.3, 0.3, 0.3, 0.3, 0.3}); - // ibvs_controller.setVelocityLimit6({0.03,0.03,0.03,0.005,0.005,0.005}); - ibvs_controller.setQdotMax(0.15); - ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25); - - - - ASSERT_TRUE(ibvs_controller.init( - "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf", - "PELVIS_S", - "R_WRIST_R_S", - "R_CAM")) - << "IbvsController init failed"; - ibvs_controller.setPerception(perception); - - auto q_map_now = makeRightArmQMap(); - robot->getJointQ(q_map_now); - std::vector q_now(kRightArmJointNames.size(), 0.0); - auto refreshRightArmQ = [&]() -> bool { - for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { - const auto it = q_map_now.find(kRightArmJointNames[i]); - if (it == q_map_now.end()) { - return false; - } - q_now[i] = it->second; - // std::cout << kRightArmJointNames[i] << " " << q_now[i] << std::endl; - } - return true; - }; - - ASSERT_TRUE(refreshRightArmQ()) - << "Failed to extract right-arm 7 joints from robot state"; - ibvs_controller.reset(q_now); - - - const int max_steps = 30000; - const int log_every = 1; - const int cycle_ms = 10; - - int ok_steps = 0; - int fail_steps = 0; - int perception_fail_steps = 0; - int compute_fail_steps = 0; - auto sendZeroVelocity = [&]() { - robot->speedJ(0); - }; - - for (int i = 0; i < 10; ++i) { - perception->update(perception_opt); - std::this_thread::sleep_for(std::chrono::milliseconds(30)); - } - - for (int step = 0; step < max_steps; ++step) { - robot->getJointQ(q_map_now); - if (!refreshRightArmQ()) { - ++fail_steps; - sendZeroVelocity(); - if ((step % log_every) == 0) { - std::cout << "[IBVS_REAL] step=" << step << " joint extract failed\n"; - } - std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); - continue; - } - - const bool perception_ok = perception->update(perception_opt); - if (!perception_ok) { - ++perception_fail_steps; - ++fail_steps; - sendZeroVelocity(); - if ((step % log_every) == 0) { - std::cout << "[IBVS_REAL] step=" << step - << " perception failed: " - << cmvr::perception::AprilTagPerception::statusToString(perception->lastStatus()) - << std::endl; - } - std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); - continue; - } - - std::vector qdot_next; - const bool ok = ibvs_controller.computeQdot(q_now, static_cast(cycle_ms) / 1000.0, qdot_next); - if (!ok) { - ++compute_fail_steps; - ++fail_steps; - sendZeroVelocity(); - if ((step % log_every) == 0) { - std::cout << "[IBVS_REAL] step=" << step - << " compute failed: " - << cmvr::IbvsController::statusToString(ibvs_controller.lastComputeStatus()); - if (perception->hasTags()) { - std::cout << " visible_tags="; - for (const auto& tag : perception->tags()) { - std::cout << tag.id << " "; - } - } - std::cout << std::endl; - } - std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); - continue; - } - - std::vector qd_send; - for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { - qd_send.push_back({kRightArmJointNames[i], qdot_next[i]}); - } - - robot->speedJ(qd_send); - ++ok_steps; - - if ((step % log_every) == 0) { - const auto& t_co = ibvs_controller.lastTagPositionVisp(); - const auto& v_c = ibvs_controller.lastCameraTwistVisp(); - std::cout << "[IBVS_REAL] step=" << step - << " tag=[" << t_co.x() << ", " << t_co.y() << ", " << t_co.z() << "]" - << " v_c=[" << v_c[0] << ", " << v_c[1] << ", " << v_c[2] - << ", " << v_c[3] << ", " << v_c[4] << ", " << v_c[5] << "]" - << " z_source=" << cmvr::IbvsController::depthUsageToString(ibvs_controller.lastDepthUsage()) - << std::endl; - std::cout << "qd_cmd: " ; - for (const auto& it : qd_send) { - std::cout << it.vel << " "; - } - std::cout << std::endl; - } - - std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); - } - - std::cout << "[IBVS_REAL] finished steps=" << max_steps - << " ok_steps=" << ok_steps - << " fail_steps=" << fail_steps - << " perception_fail_steps=" << perception_fail_steps - << " compute_fail_steps=" << compute_fail_steps - << std::endl; - - robot->speedJ(0); - - EXPECT_GT(ok_steps, 0) << "No successful IBVS control steps."; -} - -TEST(HumanoidRobotTest,IBVSWithRealRobotTrackedPoint) { - const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); - ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; - auto dmgr_cfg = config.getChild("DeviceManager"); - auto& dmgr = DeviceManager::getInstance(dmgr_cfg); - - auto robot = dmgr.getDevice("hc01"); - auto camera = dmgr.getDevice("right_hand_cam"); - ASSERT_NE(robot, nullptr); - ASSERT_NE(camera, nullptr); - - std::vector init_cmd{}; - init_cmd = { - {"R_SHOULDER_P", -0.2423}, - {"R_SHOULDER_R", 1.2929}, - {"R_SHOULDER_Y", 1.61}, - {"R_ELBOW_R", 1.58}, - {"R_WRIST_P", -2.8792}, - {"R_WRIST_Y", 0.1150}, - {"R_WRIST_R", -0.08}, - }; - robot->moveJ(init_cmd, 1.0, 2.0); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - ASSERT_NO_THROW(camera->start()); - struct RuntimeGuard { - std::shared_ptr camera; - std::shared_ptr robot; - ~RuntimeGuard() { - if (camera) { - try { - camera->stop(); - } catch (...) { - } - } - if (robot) { - try { - robot->eStop(); - } catch (...) { - } - } - } - } guard{camera, robot}; - - const double tag_size_m = 0.12; - const int preferred_tag_id = 0; - const Eigen::Vector3d hover_target_in_camera(0.0, 0.0, 0.40); - const cmvr::IbvsController::DepthMode ibvs_depth_mode = cmvr::IbvsController::DepthMode::MONOCULAR; - - auto perception = std::make_shared(camera); - perception->setTagSize(tag_size_m); - cmvr::perception::AprilTagPerception::Options perception_opt; - switch (ibvs_depth_mode) { - case cmvr::IbvsController::DepthMode::MONOCULAR: - perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE; - break; - case cmvr::IbvsController::DepthMode::PREFER_DEPTH: - perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::PREFER; - break; - case cmvr::IbvsController::DepthMode::DEPTH_ONLY: - perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE; - break; - } - - cmvr::perception::TagRelativeTarget3D tracker(perception); - tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE); - tracker.setActiveTagSwitchPolicy(4, 1.2); - tracker.setTrackingCandidateScoreWeights(1.0, 1.0, 0.08); - - cmvr::IbvsController ibvs_controller; - ibvs_controller.setMu(0.1); - ibvs_controller.setLambda(0.6); - ibvs_controller.setDepthMode(ibvs_depth_mode); - ibvs_controller.setDepthZGain(1.0); - - ibvs_controller.setQdotMax(0.15); - ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25); - - ASSERT_TRUE(ibvs_controller.init( - "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf", - "PELVIS_S", - "R_WRIST_R_S", - "R_CAM")) - << "IbvsController init failed"; - ibvs_controller.setPerception(perception); - - auto q_map_now = makeRightArmQMap(); - robot->getJointQ(q_map_now); - std::vector q_now(kRightArmJointNames.size(), 0.0); - auto refreshRightArmQ = [&]() -> bool { - for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { - const auto it = q_map_now.find(kRightArmJointNames[i]); - if (it == q_map_now.end()) { - return false; - } - q_now[i] = it->second; - } - return true; - }; - - ASSERT_TRUE(refreshRightArmQ()) - << "Failed to extract right-arm 7 joints from robot state"; - ibvs_controller.reset(q_now); - - auto sendZeroVelocity = [&]() { - robot->speedJ(0); - }; - - auto projectPreferredTagCenter = [&](int& u, int& v) -> bool { - const auto* tag = perception->findTag(preferred_tag_id); - if (!tag) return false; - Eigen::Vector2d uv = Eigen::Vector2d::Zero(); - const Eigen::Vector3d p_c_tag = tag->T_c_t.block<3, 1>(0, 3); - if (!cmvr::ImageProcess::projectCameraPointToPixel(perception->intrinsics(), p_c_tag, uv)) { - return false; - } - u = static_cast(std::lround(uv.x())); - v = static_cast(std::lround(uv.y())); - const cv::Mat& color = perception->color(); - return !color.empty() && u >= 0 && v >= 0 && u < color.cols && v < color.rows; - }; - - for (int i = 0; i < 10; ++i) { - perception->update(perception_opt); - std::this_thread::sleep_for(std::chrono::milliseconds(30)); - } - - const int max_steps = 30000; - const int log_every = 1; - const int cycle_ms = 10; - - int ok_steps = 0; - int fail_steps = 0; - int perception_fail_steps = 0; - int tracking_fail_steps = 0; - int compute_fail_steps = 0; - bool target_locked = false; - bool ibvs_target_initialized = false; - int lock_u = -1; - int lock_v = -1; - - for (int step = 0; step < max_steps; ++step) { - robot->getJointQ(q_map_now); - if (!refreshRightArmQ()) { - ++fail_steps; - sendZeroVelocity(); - if ((step % log_every) == 0) { - std::cout << "[IBVS_REAL_TRACK] step=" << step << " joint extract failed\n"; - } - std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); - continue; - } - - const bool perception_ok = perception->update(perception_opt); - if (!perception_ok) { - ++perception_fail_steps; - ++fail_steps; - sendZeroVelocity(); - if ((step % log_every) == 0) { - std::cout << "[IBVS_REAL_TRACK] step=" << step - << " perception failed: " - << cmvr::perception::AprilTagPerception::statusToString(perception->lastStatus()) - << std::endl; - } - std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); - continue; - } - - bool tracking_ok = false; - if (!target_locked) { - if (projectPreferredTagCenter(lock_u, lock_v)) { - tracking_ok = tracker.startTrackingFromPixel(lock_u, lock_v); - if (tracking_ok) { - target_locked = true; - ibvs_target_initialized = false; - if ((step % log_every) == 0) { - std::cout << "[IBVS_REAL_TRACK] step=" << step - << " lock point success" - << " uv=(" << lock_u << ", " << lock_v << ")" - << " active_tag=" << tracker.activeTagId() - << std::endl; - } - } - } - } else { - tracking_ok = tracker.track(); - } - - if (!tracking_ok) { - ++tracking_fail_steps; - ++fail_steps; - sendZeroVelocity(); - if ((step % log_every) == 0) { - std::cout << "[IBVS_REAL_TRACK] step=" << step - << " tracker failed: " - << cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus()); - if (perception->hasTags()) { - std::cout << " visible_tags="; - for (const auto& tag : perception->tags()) { - std::cout << tag.id << " "; - } - } - std::cout << std::endl; - } - std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); - continue; - } - - const int active_tag_id = tracker.activeTagId(); - Eigen::Vector3d p_t_target = Eigen::Vector3d::Zero(); - if (active_tag_id < 0 || !tracker.getAnchorInTag(active_tag_id, p_t_target)) { - ++tracking_fail_steps; - ++fail_steps; - sendZeroVelocity(); - if ((step % log_every) == 0) { - std::cout << "[IBVS_REAL_TRACK] step=" << step - << " anchor unavailable, active_tag=" << active_tag_id - << std::endl; - } - std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); - continue; - } - - ibvs_controller.setTrackedTagId(active_tag_id); - if (!ibvs_target_initialized || tracker.lastSwitched()) { - if (!ibvs_controller.setTargetFromPointInTag(p_t_target, hover_target_in_camera)) { - ++tracking_fail_steps; - ++fail_steps; - sendZeroVelocity(); - if ((step % log_every) == 0) { - std::cout << "[IBVS_REAL_TRACK] step=" << step - << " setTargetFromPointInTag failed" - << std::endl; - } - std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); - continue; - } - ibvs_target_initialized = true; - if ((step % log_every) == 0) { - std::cout << "[IBVS_REAL_TRACK] step=" << step - << " setTargetFromPointInTag ok" - << " active_tag=" << active_tag_id - << " p_t_target=[" << p_t_target.x() << ", " - << p_t_target.y() << ", " << p_t_target.z() << "]" - << std::endl; - } - } - - std::vector qdot_next; - const bool ok = ibvs_controller.computeQdot(q_now, static_cast(cycle_ms) / 1000.0, qdot_next); - if (!ok) { - ++compute_fail_steps; - ++fail_steps; - sendZeroVelocity(); - if ((step % log_every) == 0) { - std::cout << "[IBVS_REAL_TRACK] step=" << step - << " compute failed: " - << cmvr::IbvsController::statusToString(ibvs_controller.lastComputeStatus()) - << " active_tag=" << active_tag_id - << std::endl; - } - std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); - continue; - } - - std::vector qd_send; - for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { - qd_send.push_back({kRightArmJointNames[i], qdot_next[i]}); - } - - robot->speedJ(qd_send); - ++ok_steps; - - if ((step % log_every) == 0) { - const auto& t_co = ibvs_controller.lastTagPositionVisp(); - const auto& v_c = ibvs_controller.lastCameraTwistVisp(); - const auto& p_c_target = tracker.lastTargetInCamera(); - std::cout << "[IBVS_REAL_TRACK] step=" << step - << " active_tag=" << active_tag_id - << " switched=" << static_cast(tracker.lastSwitched()) - << " target_p_c=[" << p_c_target.x() << ", " << p_c_target.y() << ", " << p_c_target.z() << "]" - << " tag=[" << t_co.x() << ", " << t_co.y() << ", " << t_co.z() << "]" - << " v_c=[" << v_c[0] << ", " << v_c[1] << ", " << v_c[2] - << ", " << v_c[3] << ", " << v_c[4] << ", " << v_c[5] << "]" - << " z_source=" << cmvr::IbvsController::depthUsageToString(ibvs_controller.lastDepthUsage()) - << std::endl; - } - - std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); - } - - std::cout << "[IBVS_REAL_TRACK] finished steps=" << max_steps - << " ok_steps=" << ok_steps - << " fail_steps=" << fail_steps - << " perception_fail_steps=" << perception_fail_steps - << " tracking_fail_steps=" << tracking_fail_steps - << " compute_fail_steps=" << compute_fail_steps - << std::endl; - - robot->speedJ(0); - - EXPECT_GT(ok_steps, 0) << "No successful tracked-point IBVS control steps."; -} diff --git a/cmvr-es/devices/robot/humanoid_robot/traj_with_vel.csv b/cmvr-es/devices/robot/humanoid_robot/traj_with_vel.csv deleted file mode 100644 index 3d53c7a7..00000000 --- a/cmvr-es/devices/robot/humanoid_robot/traj_with_vel.csv +++ /dev/null @@ -1,201 +0,0 @@ -idx,q0_rad, q1_rad, q2_rad, q3_rad, q4_rad, q5_rad, q6_rad,R_SHOULDER_P_rad,R_SHOULDER_R_rad,R_SHOULDER_Y_rad,R_ELBOW_R_rad,R_WRIST_P_rad,R_WRIST_Y_rad,R_WRIST_R_rad,pLx_rad, pLy_rad, pLz_rad, pRx_rad, pRy_rad, pRz _rad,q0_vel, q1_vel, q2_vel, q3_vel, q4_vel, q5_vel, q6_vel,R_SHOULDER_P_vel,R_SHOULDER_R_vel,R_SHOULDER_Y_vel,R_ELBOW_R_vel,R_WRIST_P_vel,R_WRIST_Y_vel,R_WRIST_R_vel,pLx_vel, pLy_vel, pLz_vel, pRx_vel, pRy_vel, pRz _vel -0,-0.060915,-0.408236,-1.31591,-1.57009,1.96717,0.099621,0.174091,0.076603,1.16244,1.721,1.81069,-2.70959,0.039061,0.356226,0.250063,0.349893,-0.099981,0.250748,-0.25,-0.1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -1,-0.060358,-0.401354,-1.32106,-1.5766,1.96116,0.102057,0.168042,0.079723,1.16252,1.71963,1.80767,-2.71013,0.039137,0.35604,0.250106,0.349963,-0.096917,0.251478,-0.249999,-0.099996,0.0557,0.6882,-0.5152,-0.6504,-0.6012,0.2436,-0.6049,0.312,0.0087,-0.1371,-0.3019,-0.0536,0.0076,-0.0186,0.0043,0.007,0.3064,0.073,0.0001,0.0004 -2,-0.05968,-0.39443,-1.32628,-1.58395,1.95512,0.105347,0.16177,0.082918,1.16262,1.71822,1.80455,-2.71068,0.039224,0.355827,0.250116,0.349818,-0.093782,0.252227,-0.249999,-0.099996,0.0678,0.6924,-0.5221,-0.7352,-0.6043,0.329,-0.6272,0.3195,0.0094,-0.1405,-0.3117,-0.0553,0.0087,-0.0213,0.001,-0.0145,0.3135,0.0749,0,0 -3,-0.059082,-0.387617,-1.33144,-1.5915,1.94916,0.108962,0.155437,0.086114,1.16272,1.71682,1.80143,-2.71123,0.039315,0.355606,0.250119,0.349573,-0.09065,0.252977,-0.249999,-0.099996,0.0598,0.6813,-0.5164,-0.7552,-0.596,0.3615,-0.6333,0.3196,0.01,-0.1403,-0.3127,-0.0553,0.0091,-0.0221,0.0003,-0.0245,0.3132,0.075,0,0 -4,-0.058629,-0.380953,-1.33651,-1.59921,1.94332,0.112876,0.149029,0.089308,1.16282,1.71542,1.79829,-2.71178,0.039412,0.355376,0.250119,0.349229,-0.087527,0.253727,-0.249999,-0.099996,0.0453,0.6664,-0.507,-0.7706,-0.5841,0.3914,-0.6408,0.3194,0.0104,-0.1399,-0.3134,-0.0552,0.0097,-0.023,0,-0.0344,0.3123,0.075,0,0 -5,-0.058345,-0.374454,-1.34147,-1.60705,1.93761,0.117085,0.142542,0.0925,1.16293,1.71403,1.79515,-2.71234,0.039512,0.355139,0.250119,0.348787,-0.084416,0.254477,-0.249999,-0.099996,0.0284,0.6499,-0.4958,-0.7848,-0.5705,0.4209,-0.6487,0.3192,0.0109,-0.1395,-0.3141,-0.0553,0.01,-0.0237,0,-0.0442,0.3111,0.075,0,0 -6,-0.05825,-0.368135,-1.3463,-1.61504,1.93206,0.121585,0.135974,0.095689,1.16304,1.71263,1.79201,-2.71289,0.039617,0.354894,0.250118,0.348247,-0.08132,0.255227,-0.249999,-0.099996,0.0095,0.6319,-0.4831,-0.7982,-0.5554,0.45,-0.6568,0.3189,0.0114,-0.1391,-0.3148,-0.0553,0.0105,-0.0245,-0.0001,-0.054,0.3096,0.075,0,0 -7,-0.058365,-0.362008,-1.35099,-1.62315,1.92667,0.126373,0.129323,0.098876,1.16316,1.71125,1.78885,-2.71344,0.039725,0.354641,0.250116,0.347611,-0.078242,0.255977,-0.249999,-0.099996,-0.0115,0.6127,-0.4688,-0.811,-0.5388,0.4788,-0.6651,0.3187,0.0118,-0.1388,-0.3155,-0.0553,0.0108,-0.0253,-0.0002,-0.0636,0.3078,0.075,0,0 -8,-0.058709,-0.356088,-1.35552,-1.63138,1.92147,0.131446,0.122585,0.102061,1.16328,1.70986,1.78569,-2.714,0.039839,0.354381,0.250113,0.346878,-0.075186,0.256727,-0.249999,-0.099996,-0.0344,0.592,-0.4527,-0.8231,-0.5205,0.5073,-0.6738,0.3185,0.0123,-0.1384,-0.3162,-0.0553,0.0114,-0.026,-0.0003,-0.0733,0.3056,0.075,0,0 -9,-0.059306,-0.350389,-1.35987,-1.63972,1.91646,0.136799,0.115756,0.105243,1.16341,1.70848,1.78252,-2.71455,0.039956,0.354113,0.250109,0.34605,-0.072155,0.257477,-0.249999,-0.099996,-0.0597,0.5699,-0.4349,-0.8344,-0.5006,0.5353,-0.6829,0.3182,0.0127,-0.138,-0.3169,-0.0553,0.0117,-0.0268,-0.0004,-0.0828,0.3031,0.075,0,0 -10,-0.060176,-0.344926,-1.36402,-1.64817,1.91167,0.142428,0.108834,0.108424,1.16354,1.70711,1.77934,-2.7151,0.040077,0.353837,0.250104,0.345128,-0.06915,0.258227,-0.249999,-0.099996,-0.087,0.5463,-0.4151,-0.845,-0.4792,0.5629,-0.6922,0.3181,0.0132,-0.1376,-0.3176,-0.0554,0.0121,-0.0276,-0.0005,-0.0922,0.3005,0.075,0,0 -11,-0.061344,-0.339714,-1.36795,-1.65672,1.90711,0.148328,0.101812,0.111602,1.16368,1.70573,1.77616,-2.71566,0.040203,0.353553,0.250097,0.344111,-0.066176,0.258977,-0.249998,-0.099996,-0.1168,0.5212,-0.3933,-0.855,-0.4559,0.59,-0.7022,0.3178,0.0137,-0.1373,-0.3184,-0.0553,0.0126,-0.0284,-0.0007,-0.1017,0.2974,0.075,0.0001,0 -12,-0.062969,-0.334882,-1.37154,-1.66519,1.9029,0.154381,0.094696,0.114728,1.16382,1.70439,1.77303,-2.7162,0.040328,0.353274,0.250106,0.343021,-0.063264,0.259714,-0.249998,-0.099994,-0.1625,0.4832,-0.3585,-0.8467,-0.4209,0.6053,-0.7116,0.3126,0.0138,-0.1346,-0.3131,-0.0543,0.0125,-0.0279,0.0009,-0.109,0.2912,0.0737,0,0.0002 -13,-0.06483,-0.330233,-1.37495,-1.6739,1.89887,0.160799,0.087449,0.1179,1.16396,1.70302,1.76983,-2.71675,0.040461,0.352976,0.2501,0.341822,-0.060361,0.260464,-0.249998,-0.099994,-0.1861,0.4649,-0.3417,-0.8708,-0.4029,0.6418,-0.7247,0.3172,0.0145,-0.1365,-0.3194,-0.0553,0.0133,-0.0298,-0.0006,-0.1199,0.2903,0.075,0,0 -14,-0.067019,-0.325859,-1.37811,-1.6827,1.89511,0.167483,0.080102,0.121072,1.16411,1.70166,1.76663,-2.71731,0.040599,0.35267,0.250087,0.340532,-0.057495,0.261214,-0.249998,-0.099994,-0.2189,0.4374,-0.3158,-0.8806,-0.3763,0.6684,-0.7347,0.3172,0.0149,-0.1361,-0.3205,-0.0554,0.0138,-0.0306,-0.0013,-0.129,0.2866,0.075,0,0 -15,-0.069586,-0.321792,-1.38097,-1.69158,1.89164,0.174418,0.072639,0.124242,1.16427,1.7003,1.76342,-2.71786,0.040741,0.352355,0.250072,0.339154,-0.054671,0.261964,-0.249998,-0.099993,-0.2567,0.4067,-0.286,-0.888,-0.3466,0.6935,-0.7463,0.317,0.0154,-0.1359,-0.3211,-0.0553,0.0142,-0.0315,-0.0015,-0.1378,0.2824,0.075,0,0.0001 -16,-0.072563,-0.31805,-1.38351,-1.70053,1.8885,0.181599,0.065052,0.12741,1.16442,1.69895,1.7602,-2.71841,0.040887,0.352033,0.250055,0.337689,-0.051892,0.262714,-0.249998,-0.099993,-0.2977,0.3742,-0.2534,-0.8943,-0.3147,0.7181,-0.7587,0.3168,0.0158,-0.1354,-0.3219,-0.0554,0.0146,-0.0322,-0.0017,-0.1465,0.2779,0.075,0,0 -17,-0.075974,-0.314651,-1.38569,-1.70952,1.88569,0.189019,0.057333,0.130576,1.16459,1.6976,1.75697,-2.71897,0.041036,0.351703,0.250035,0.336137,-0.04916,0.263463,-0.249998,-0.099993,-0.3411,0.3399,-0.2184,-0.8998,-0.2809,0.742,-0.7719,0.3166,0.0163,-0.1351,-0.3226,-0.0554,0.0149,-0.033,-0.002,-0.1552,0.2732,0.0749,0,0 -18,-0.079841,-0.31161,-1.3875,-1.71857,1.88323,0.196671,0.049476,0.13374,1.16475,1.69625,1.75374,-2.71952,0.04119,0.351366,0.250014,0.334502,-0.046479,0.264213,-0.249998,-0.099993,-0.3867,0.3041,-0.181,-0.9044,-0.2454,0.7652,-0.7857,0.3164,0.0167,-0.1347,-0.3233,-0.0553,0.0154,-0.0337,-0.0021,-0.1635,0.2681,0.075,0,0 -19,-0.084184,-0.308942,-1.38891,-1.72765,1.88115,0.204548,0.041475,0.136902,1.16492,1.69491,1.7505,-2.72008,0.041348,0.351021,0.24999,0.332783,-0.04385,0.264963,-0.249997,-0.099993,-0.4343,0.2668,-0.1414,-0.9081,-0.2079,0.7877,-0.8001,0.3162,0.0171,-0.1343,-0.3241,-0.0554,0.0158,-0.0345,-0.0024,-0.1719,0.2629,0.075,0.0001,0 -20,-0.089019,-0.306661,-1.38991,-1.73676,1.87946,0.212643,0.033326,0.140062,1.1651,1.69357,1.74725,-2.72063,0.04151,0.350668,0.249965,0.330984,-0.041278,0.265713,-0.249997,-0.099993,-0.4835,0.2281,-0.0996,-0.9109,-0.1689,0.8095,-0.8149,0.316,0.0175,-0.1339,-0.3247,-0.0555,0.0162,-0.0353,-0.0025,-0.1799,0.2572,0.075,0,0 -21,-0.094363,-0.304781,-1.39047,-1.74589,1.87818,0.22095,0.025024,0.14322,1.16528,1.69223,1.744,-2.72118,0.041675,0.350308,0.249939,0.329106,-0.038763,0.266463,-0.249997,-0.099993,-0.5344,0.188,-0.0558,-0.9129,-0.1282,0.8307,-0.8302,0.3158,0.018,-0.1336,-0.3255,-0.0554,0.0165,-0.036,-0.0026,-0.1878,0.2515,0.075,0,0 -22,-0.100423,-0.303377,-1.39048,-1.75511,1.87739,0.229577,0.016412,0.146428,1.16547,1.69088,1.74067,-2.72175,0.041851,0.349924,0.249921,0.327118,-0.036287,0.267226,-0.249998,-0.099995,-0.606,0.1404,-0.001,-0.9224,-0.0791,0.8627,-0.8612,0.3208,0.0187,-0.1355,-0.3325,-0.0564,0.0176,-0.0384,-0.0018,-0.1988,0.2476,0.0763,-0.0001,-0.0002 -23,-0.106906,-0.302383,-1.39005,-1.76423,1.87702,0.238282,0.00775,0.149584,1.16566,1.68955,1.7374,-2.7223,0.042025,0.349548,0.249901,0.325084,-0.033897,0.267976,-0.249998,-0.099995,-0.6483,0.0994,0.0429,-0.9116,-0.0375,0.8705,-0.8662,0.3156,0.0188,-0.133,-0.3272,-0.0555,0.0174,-0.0376,-0.002,-0.2034,0.239,0.075,0,0 -24,-0.1139,-0.301816,-1.38915,-1.77334,1.87708,0.247173,-0.001063,0.152737,1.16585,1.68822,1.73412,-2.72286,0.042202,0.349164,0.249878,0.322977,-0.031573,0.268726,-0.249998,-0.099995,-0.6994,0.0567,0.0904,-0.9111,0.0067,0.8891,-0.8813,0.3153,0.0193,-0.1325,-0.3277,-0.0555,0.0177,-0.0384,-0.0023,-0.2107,0.2324,0.075,0,0 -25,-0.121686,-0.301807,-1.3876,-1.78244,1.87773,0.256313,-0.010194,0.155921,1.16605,1.68689,1.7308,-2.72342,0.042388,0.348762,0.249873,0.320774,-0.029305,0.269485,-0.249998,-0.099997,-0.7786,0.0009,0.1549,-0.9107,0.0645,0.914,-0.9131,0.3184,0.0198,-0.1335,-0.3326,-0.0561,0.0186,-0.0402,-0.0005,-0.2203,0.2268,0.0759,0,-0.0002 -26,-0.129825,-0.302184,-1.38563,-1.79148,1.87878,0.265551,-0.019371,0.159071,1.16625,1.68557,1.7275,-2.72397,0.042573,0.348362,0.249859,0.318524,-0.027119,0.270235,-0.249998,-0.099997,-0.8139,-0.0377,0.1964,-0.9039,0.105,0.9238,-0.9177,0.315,0.02,-0.1318,-0.3293,-0.0555,0.0185,-0.04,-0.0014,-0.225,0.2186,0.075,0,0 -27,-0.13871,-0.303127,-1.38302,-1.80047,1.88042,0.274993,-0.028835,0.162239,1.16645,1.68425,1.72418,-2.72453,0.042765,0.347948,0.249863,0.316188,-0.024998,0.27099,-0.249999,-0.099998,-0.8885,-0.0943,0.2612,-0.8989,0.1641,0.9442,-0.9464,0.3168,0.0206,-0.1323,-0.3326,-0.0559,0.0192,-0.0414,0.0004,-0.2336,0.2121,0.0755,-0.0001,-0.0001 -28,-0.147884,-0.304436,-1.38002,-1.80942,1.88246,0.284547,-0.038334,0.165385,1.16666,1.68294,1.72087,-2.72509,0.042957,0.347533,0.249854,0.313804,-0.022959,0.27174,-0.249999,-0.099998,-0.9174,-0.1309,0.3002,-0.8946,0.2037,0.9554,-0.9499,0.3146,0.0209,-0.131,-0.3308,-0.0556,0.0192,-0.0415,-0.0009,-0.2384,0.2039,0.075,0,0 -29,-0.157749,-0.306312,-1.37638,-1.81828,1.88509,0.29427,-0.048092,0.168543,1.16687,1.68162,1.71754,-2.72565,0.043156,0.347106,0.249864,0.311343,-0.020992,0.272494,-0.249999,-0.099999,-0.9865,-0.1876,0.3642,-0.8859,0.2633,0.9723,-0.9758,0.3158,0.0213,-0.1313,-0.3332,-0.0558,0.0199,-0.0427,0.001,-0.2461,0.1967,0.0754,0,-0.0001 -30,-0.167836,-0.308537,-1.37237,-1.8271,1.88811,0.304106,-0.057867,0.171685,1.16709,1.68032,1.71422,-2.7262,0.043356,0.346676,0.249859,0.308834,-0.019107,0.273244,-0.249999,-0.099999,-1.0087,-0.2225,0.4009,-0.8823,0.3021,0.9836,-0.9775,0.3142,0.0217,-0.1303,-0.3323,-0.0555,0.02,-0.043,-0.0005,-0.2509,0.1885,0.075,0,0 -31,-0.178284,-0.311188,-1.3679,-1.83586,1.8916,0.314072,-0.067738,0.174826,1.16731,1.67902,1.71088,-2.72676,0.043559,0.346238,0.249851,0.306268,-0.017303,0.273993,-0.249999,-0.099999,-1.0448,-0.2651,0.4467,-0.8762,0.3486,0.9966,-0.9871,0.3141,0.022,-0.13,-0.333,-0.0556,0.0203,-0.0438,-0.0008,-0.2566,0.1804,0.0749,0,0 -32,-0.189344,-0.314406,-1.36281,-1.84449,1.89569,0.324168,-0.077835,0.177975,1.16754,1.67772,1.70754,-2.72732,0.043768,0.345789,0.249868,0.303633,-0.015576,0.274746,-0.249999,-0.099999,-1.106,-0.3218,0.5093,-0.8627,0.409,1.0096,-1.0097,0.3149,0.0225,-0.1299,-0.3349,-0.0558,0.0209,-0.0449,0.0017,-0.2635,0.1727,0.0753,0,0 -33,-0.200505,-0.317941,-1.35739,-1.85307,1.90014,0.334361,-0.087902,0.181112,1.16776,1.67643,1.70419,-2.72787,0.043979,0.345336,0.249867,0.300957,-0.013936,0.275496,-0.249999,-0.099999,-1.1161,-0.3535,0.5412,-0.8581,0.4455,1.0193,-1.0067,0.3137,0.0228,-0.1293,-0.3345,-0.0557,0.0211,-0.0453,-0.0001,-0.2676,0.164,0.075,0,0 -34,-0.211906,-0.321871,-1.35157,-1.86156,1.90504,0.344657,-0.098013,0.184249,1.168,1.67514,1.70084,-2.72843,0.044193,0.344875,0.249865,0.298231,-0.012381,0.276246,-0.249999,-0.099999,-1.1401,-0.393,0.5826,-0.8494,0.4899,1.0296,-1.0111,0.3137,0.0232,-0.1288,-0.3353,-0.0556,0.0214,-0.0461,-0.0002,-0.2726,0.1555,0.075,0,0 -35,-0.223529,-0.326198,-1.34533,-1.86995,1.91038,0.355043,-0.10816,0.187384,1.16823,1.67386,1.69748,-2.72899,0.044412,0.344406,0.249864,0.295457,-0.010913,0.276996,-0.249999,-0.099999,-1.1623,-0.4327,0.6238,-0.839,0.5345,1.0386,-1.0147,0.3135,0.0236,-0.1286,-0.336,-0.0557,0.0219,-0.0469,-0.0001,-0.2774,0.1468,0.075,0,0 -36,-0.235332,-0.330907,-1.3387,-1.87823,1.91616,0.365511,-0.118324,0.190517,1.16847,1.67257,1.69411,-2.72954,0.044633,0.34393,0.249864,0.292638,-0.009532,0.277746,-0.249999,-0.099999,-1.1803,-0.4709,0.6633,-0.8277,0.578,1.0468,-1.0164,0.3133,0.024,-0.1281,-0.3368,-0.0557,0.0221,-0.0476,0,-0.2819,0.1381,0.075,0,0 -37,-0.247268,-0.335982,-1.33169,-1.88639,1.92237,0.376051,-0.12848,0.193649,1.16871,1.67129,1.69073,-2.7301,0.044858,0.343447,0.249865,0.289776,-0.008241,0.278496,-0.249999,-0.099999,-1.1936,-0.5075,0.7008,-0.8156,0.6201,1.054,-1.0156,0.3132,0.0243,-0.1279,-0.3376,-0.0557,0.0225,-0.0483,0.0001,-0.2862,0.1291,0.075,0,0 -38,-0.259293,-0.341406,-1.32433,-1.89441,1.92897,0.386654,-0.138607,0.196779,1.16896,1.67002,1.68735,-2.73066,0.045087,0.342955,0.249867,0.286875,-0.00704,0.279246,-0.249999,-0.099999,-1.2025,-0.5424,0.7363,-0.8026,0.6605,1.0603,-1.0127,0.313,0.0247,-0.1274,-0.3383,-0.0558,0.0229,-0.0492,0.0002,-0.2901,0.1201,0.075,0,0 -39,-0.271362,-0.347161,-1.31663,-1.9023,1.93596,0.39731,-0.148681,0.199908,1.16921,1.66875,1.68396,-2.73122,0.045319,0.342456,0.24987,0.283938,-0.00593,0.279996,-0.249999,-0.099999,-1.2069,-0.5755,0.7698,-0.7889,0.6993,1.0656,-1.0074,0.3129,0.0251,-0.1271,-0.3391,-0.0557,0.0232,-0.0499,0.0003,-0.2937,0.111,0.075,0,0 -40,-0.283431,-0.353228,-1.30862,-1.91004,1.94333,0.408009,-0.158679,0.203036,1.16947,1.66748,1.68056,-2.73177,0.045555,0.341949,0.249873,0.280966,-0.004914,0.280746,-0.249999,-0.099999,-1.2069,-0.6067,0.8012,-0.7743,0.7365,1.0699,-0.9998,0.3128,0.0255,-0.1268,-0.3399,-0.0559,0.0236,-0.0507,0.0003,-0.2972,0.1016,0.075,0,0 -41,-0.29546,-0.35959,-1.30031,-1.91763,1.95105,0.418742,-0.168577,0.206163,1.16973,1.66622,1.67715,-2.73233,0.045794,0.341435,0.249877,0.277964,-0.003991,0.281496,-0.249999,-0.099999,-1.2029,-0.6362,0.8308,-0.759,0.7719,1.0733,-0.9898,0.3127,0.0259,-0.1264,-0.3407,-0.0558,0.0239,-0.0514,0.0004,-0.3002,0.0923,0.075,0,0 -42,-0.307408,-0.366228,-1.29173,-1.92506,1.9591,0.429498,-0.178352,0.209288,1.16999,1.66496,1.67374,-2.73289,0.046036,0.340913,0.249882,0.274934,-0.003162,0.282246,-0.249999,-0.099999,-1.1948,-0.6638,0.8584,-0.7429,0.8054,1.0756,-0.9775,0.3125,0.0262,-0.126,-0.3414,-0.0558,0.0242,-0.0522,0.0005,-0.303,0.0829,0.075,0,0 -43,-0.319238,-0.373123,-1.28288,-1.93232,1.96747,0.440268,-0.187983,0.212412,1.17026,1.6637,1.67032,-2.73345,0.046282,0.340383,0.249887,0.271879,-0.002429,0.282996,-0.249999,-0.099999,-1.183,-0.6895,0.8842,-0.7259,0.8371,1.077,-0.9631,0.3124,0.0266,-0.1257,-0.3423,-0.0559,0.0246,-0.053,0.0005,-0.3055,0.0733,0.075,0,0 -44,-0.330669,-0.380116,-1.27396,-1.93946,1.97601,0.451018,-0.197309,0.215525,1.17052,1.66245,1.6669,-2.73401,0.04653,0.33985,0.249868,0.268821,-0.001798,0.283743,-0.249999,-0.099998,-1.1431,-0.6993,0.8922,-0.7142,0.8532,1.075,-0.9326,0.3113,0.0268,-0.125,-0.3418,-0.0558,0.0248,-0.0533,-0.0019,-0.3058,0.0631,0.0747,0,0.0001 -45,-0.342146,-0.387462,-1.26467,-1.94637,1.98494,0.461783,-0.206575,0.218647,1.1708,1.6612,1.66346,-2.73457,0.046782,0.339305,0.249872,0.265727,-0.001259,0.284493,-0.249999,-0.099998,-1.1477,-0.7346,0.9295,-0.6903,0.894,1.0765,-0.9266,0.3122,0.0273,-0.125,-0.3438,-0.0559,0.0252,-0.0545,0.0004,-0.3094,0.0539,0.075,0,0 -46,-0.353431,-0.395026,-1.25515,-1.95307,1.99416,0.472532,-0.215647,0.221767,1.17107,1.65995,1.66002,-2.73513,0.047038,0.338752,0.249878,0.262616,-0.000816,0.285243,-0.249999,-0.099998,-1.1285,-0.7564,0.9515,-0.6702,0.9214,1.0749,-0.9072,0.312,0.0277,-0.1247,-0.3446,-0.056,0.0256,-0.0553,0.0006,-0.3111,0.0443,0.075,0,0 -47,-0.364474,-0.40278,-1.24544,-1.95957,2.00362,0.483254,-0.224493,0.224886,1.17135,1.65871,1.65656,-2.73569,0.047298,0.338191,0.249884,0.259492,-0.000472,0.285993,-0.249999,-0.099998,-1.1043,-0.7754,0.9707,-0.6501,0.9458,1.0722,-0.8846,0.3119,0.028,-0.1243,-0.3454,-0.056,0.026,-0.0561,0.0006,-0.3124,0.0344,0.075,0,0 -48,-0.375249,-0.410707,-1.23556,-1.96586,2.0133,0.493938,-0.233093,0.228004,1.17164,1.65747,1.6531,-2.73625,0.04756,0.337623,0.24989,0.256359,-0.000225,0.286743,-0.249999,-0.099998,-1.0775,-0.7927,0.988,-0.6293,0.9683,1.0684,-0.86,0.3118,0.0283,-0.124,-0.3462,-0.056,0.0262,-0.0568,0.0006,-0.3133,0.0247,0.075,0,0 -49,-0.385729,-0.41879,-1.22553,-1.97194,2.02319,0.504573,-0.24143,0.231121,1.17192,1.65623,1.64963,-2.73681,0.047826,0.337047,0.249897,0.25322,-7.6e-05,0.287493,-0.249999,-0.099998,-1.048,-0.8083,1.0037,-0.6078,0.9889,1.0635,-0.8337,0.3117,0.0288,-0.1236,-0.3471,-0.0561,0.0266,-0.0576,0.0007,-0.3139,0.0149,0.075,0,0 -50,-0.395894,-0.427014,-1.21535,-1.9778,2.03327,0.515151,-0.249487,0.234237,1.17221,1.655,1.64615,-2.73737,0.048095,0.336463,0.249903,0.250077,-2.7e-05,0.288243,-0.249999,-0.099998,-1.0165,-0.8224,1.018,-0.5854,1.0076,1.0578,-0.8057,0.3116,0.029,-0.1233,-0.3478,-0.0561,0.0269,-0.0584,0.0006,-0.3143,0.0049,0.075,0,0 -51,-0.405725,-0.435365,-1.20504,-1.98342,2.04351,0.525659,-0.257248,0.237352,1.17251,1.65377,1.64266,-2.73793,0.048368,0.335872,0.249909,0.246935,-7.5e-05,0.288993,-0.249999,-0.099998,-0.9831,-0.8351,1.0306,-0.5624,1.0245,1.0508,-0.7761,0.3115,0.0294,-0.1229,-0.3487,-0.0561,0.0273,-0.0591,0.0006,-0.3142,-0.0048,0.075,0,0 -52,-0.414974,-0.443688,-1.19478,-1.98885,2.05376,0.53604,-0.264557,0.240452,1.17281,1.65255,1.63919,-2.73849,0.04864,0.33528,0.249893,0.24382,-0.00023,0.289739,-0.249998,-0.099997,-0.9249,-0.8323,1.0261,-0.5434,1.0253,1.0381,-0.7309,0.31,0.0297,-0.1221,-0.3474,-0.056,0.0272,-0.0592,-0.0016,-0.3115,-0.0155,0.0746,0.0001,0.0001 -53,-0.424075,-0.452245,-1.18427,-1.994,2.06428,0.546375,-0.271674,0.243565,1.17311,1.65133,1.63569,-2.73905,0.048919,0.334673,0.249897,0.240688,-0.000476,0.290489,-0.249998,-0.099997,-0.9101,-0.8557,1.0508,-0.5147,1.052,1.0335,-0.7117,0.3113,0.0301,-0.1222,-0.3503,-0.0562,0.0279,-0.0607,0.0004,-0.3132,-0.0246,0.075,0,0 -54,-0.432811,-0.460899,-1.17367,-1.99889,2.07492,0.55661,-0.278462,0.246677,1.17341,1.65011,1.63217,-2.73961,0.049201,0.334059,0.249904,0.237563,-0.00082,0.291239,-0.249998,-0.099997,-0.8736,-0.8654,1.0602,-0.4889,1.0642,1.0235,-0.6788,0.3112,0.0304,-0.122,-0.3511,-0.0562,0.0282,-0.0614,0.0007,-0.3125,-0.0344,0.075,0,0 -55,-0.441151,-0.469628,-1.163,-2.00352,2.08566,0.566733,-0.284898,0.249788,1.17372,1.64889,1.62866,-2.74018,0.049486,0.333437,0.24991,0.234451,-0.001261,0.291989,-0.249998,-0.099997,-0.834,-0.8729,1.0669,-0.4632,1.0736,1.0123,-0.6436,0.3111,0.0308,-0.1216,-0.3519,-0.0563,0.0285,-0.0622,0.0006,-0.3112,-0.0441,0.075,0,0 -56,-0.449084,-0.478419,-1.15228,-2.00789,2.09647,0.576733,-0.290969,0.252898,1.17403,1.64768,1.62513,-2.74074,0.049774,0.332807,0.249916,0.231354,-0.0018,0.292739,-0.249998,-0.099997,-0.7933,-0.8791,1.0721,-0.4369,1.0811,1,-0.6071,0.311,0.0311,-0.1212,-0.3528,-0.0563,0.0288,-0.063,0.0006,-0.3097,-0.0539,0.075,0,0 -57,-0.456598,-0.487259,-1.14152,-2.01199,2.10734,0.586598,-0.296662,0.256008,1.17434,1.64647,1.62159,-2.7413,0.050066,0.332169,0.249922,0.228275,-0.002436,0.293489,-0.249998,-0.099997,-0.7514,-0.884,1.0755,-0.41,1.0866,0.9865,-0.5693,0.311,0.0314,-0.1209,-0.3536,-0.0564,0.0292,-0.0638,0.0006,-0.3079,-0.0636,0.075,0,0 -58,-0.463685,-0.496139,-1.13075,-2.01582,2.11824,0.596319,-0.301967,0.259117,1.17466,1.64527,1.61805,-2.74187,0.05036,0.331524,0.249927,0.225218,-0.003168,0.294239,-0.249998,-0.099997,-0.7087,-0.888,1.0775,-0.3825,1.0903,0.9721,-0.5305,0.3109,0.0318,-0.1206,-0.3544,-0.0564,0.0294,-0.0645,0.0005,-0.3057,-0.0732,0.075,0,0 -59,-0.470337,-0.505045,-1.11997,-2.01936,2.12916,0.605883,-0.306873,0.262225,1.17498,1.64406,1.6145,-2.74243,0.050658,0.33087,0.249933,0.222185,-0.003995,0.294989,-0.249998,-0.099997,-0.6652,-0.8906,1.078,-0.3545,1.0918,0.9564,-0.4906,0.3108,0.0321,-0.1203,-0.3553,-0.0564,0.0298,-0.0654,0.0006,-0.3033,-0.0827,0.075,0,0 -60,-0.476351,-0.51383,-1.10936,-2.02265,2.13993,0.615209,-0.311237,0.26531,1.1753,1.64287,1.61096,-2.74299,0.050954,0.33022,0.24992,0.219211,-0.004924,0.295733,-0.249997,-0.099996,-0.6014,-0.8785,1.0614,-0.3293,1.0773,0.9326,-0.4364,0.3085,0.0322,-0.119,-0.353,-0.0561,0.0296,-0.065,-0.0013,-0.2974,-0.0929,0.0744,0.0001,0.0001 -61,-0.482111,-0.522757,-1.09862,-2.02563,2.15082,0.624426,-0.315312,0.268416,1.17563,1.64168,1.6074,-2.74356,0.051257,0.329552,0.249924,0.216237,-0.00594,0.296483,-0.249997,-0.099996,-0.576,-0.8927,1.0737,-0.2974,1.0889,0.9217,-0.4075,0.3106,0.0328,-0.1196,-0.3568,-0.0565,0.0303,-0.0668,0.0004,-0.2974,-0.1016,0.075,0,0 -62,-0.487437,-0.531691,-1.08791,-2.0283,2.16168,0.633457,-0.318969,0.271523,1.17596,1.64048,1.60382,-2.74412,0.051564,0.328875,0.24993,0.213295,-0.007049,0.297233,-0.249997,-0.099996,-0.5326,-0.8934,1.0705,-0.2676,1.0858,0.9031,-0.3657,0.3107,0.033,-0.1193,-0.3579,-0.0565,0.0307,-0.0677,0.0006,-0.2942,-0.1109,0.075,0,0 -63,-0.492309,-0.540613,-1.07727,-2.03068,2.17248,0.642288,-0.322192,0.274628,1.1763,1.6393,1.60023,-2.74469,0.051874,0.328191,0.249935,0.210389,-0.008249,0.297983,-0.249997,-0.099996,-0.4872,-0.8922,1.0646,-0.2378,1.0797,0.8831,-0.3223,0.3105,0.0334,-0.1189,-0.3587,-0.0566,0.031,-0.0684,0.0005,-0.2906,-0.12,0.075,0,0 -64,-0.496725,-0.549514,-1.0667,-2.03276,2.18319,0.650909,-0.324972,0.277733,1.17663,1.63811,1.59664,-2.74525,0.052187,0.327499,0.24994,0.207522,-0.00954,0.298733,-0.249997,-0.099996,-0.4416,-0.8901,1.057,-0.2077,1.0716,0.8621,-0.278,0.3105,0.0337,-0.1186,-0.3595,-0.0567,0.0313,-0.0692,0.0005,-0.2867,-0.1291,0.075,0,0 -65,-0.500683,-0.558384,-1.05622,-2.03453,2.19381,0.659308,-0.327301,0.280838,1.17697,1.63693,1.59303,-2.74582,0.052503,0.326799,0.249945,0.204697,-0.01092,0.299482,-0.249997,-0.099996,-0.3958,-0.887,1.0478,-0.1774,1.0616,0.8399,-0.2329,0.3105,0.0341,-0.1183,-0.3604,-0.0567,0.0316,-0.07,0.0005,-0.2825,-0.138,0.0749,0,0 -66,-0.504182,-0.567215,-1.04585,-2.036,2.2043,0.667475,-0.329173,0.283942,1.17732,1.63575,1.58942,-2.74639,0.052821,0.326091,0.249949,0.201917,-0.012387,0.300232,-0.249997,-0.099995,-0.3499,-0.8831,1.037,-0.1467,1.0496,0.8167,-0.1872,0.3104,0.0343,-0.1179,-0.3613,-0.0567,0.0318,-0.0708,0.0004,-0.278,-0.1467,0.075,0,0.0001 -67,-0.507223,-0.575999,-1.0356,-2.03716,2.21466,0.675399,-0.330581,0.287045,1.17766,1.63457,1.5858,-2.74696,0.053143,0.325376,0.249954,0.199184,-0.013942,0.300982,-0.249997,-0.099995,-0.3041,-0.8784,1.0246,-0.1159,1.0358,0.7924,-0.1408,0.3103,0.0347,-0.1177,-0.3622,-0.0568,0.0322,-0.0715,0.0005,-0.2733,-0.1555,0.075,0,0 -68,-0.509651,-0.584588,-1.02565,-2.03803,2.22471,0.682981,-0.331412,0.290114,1.17801,1.63341,1.58222,-2.74752,0.05346,0.32467,0.249944,0.196538,-0.015581,0.301723,-0.249996,-0.099993,-0.2428,-0.8589,0.9951,-0.0875,1.0051,0.7582,-0.0831,0.3069,0.0346,-0.116,-0.358,-0.0562,0.0317,-0.0706,-0.001,-0.2646,-0.1639,0.0741,0.0001,0.0002 -69,-0.511777,-0.59325,-1.01571,-2.03857,2.23473,0.690387,-0.331877,0.293215,1.17836,1.63224,1.57858,-2.74809,0.053787,0.323939,0.249947,0.193909,-0.017304,0.302473,-0.249996,-0.099993,-0.2126,-0.8662,0.9945,-0.0541,1.002,0.7406,-0.0465,0.3101,0.0353,-0.1169,-0.3637,-0.0569,0.0327,-0.0731,0.0003,-0.2629,-0.1723,0.075,0,0 -70,-0.513466,-0.60185,-1.00592,-2.0388,2.24457,0.697524,-0.331875,0.296317,1.17872,1.63108,1.57493,-2.74866,0.054117,0.3232,0.249952,0.191333,-0.019107,0.303223,-0.249996,-0.099993,-0.1689,-0.86,0.9784,-0.0225,0.9834,0.7137,0.0002,0.3102,0.0355,-0.1167,-0.3648,-0.0569,0.033,-0.0739,0.0005,-0.2576,-0.1803,0.075,0,0 -71,-0.514706,-0.610372,-0.996328,-2.03871,2.25419,0.704378,-0.331393,0.29942,1.17908,1.62991,1.57127,-2.74923,0.054451,0.322453,0.249956,0.188815,-0.02099,0.303972,-0.249996,-0.099993,-0.124,-0.8522,0.9596,0.0088,0.9623,0.6854,0.0482,0.3103,0.0359,-0.1163,-0.3657,-0.057,0.0334,-0.0747,0.0004,-0.2518,-0.1883,0.0749,0,0 -72,-0.515502,-0.618808,-0.986934,-2.03831,2.26358,0.71094,-0.330427,0.302522,1.17944,1.62875,1.56761,-2.7498,0.054787,0.321699,0.24996,0.186358,-0.022951,0.304722,-0.249996,-0.099993,-0.0796,-0.8436,0.9394,0.04,0.9392,0.6562,0.0966,0.3102,0.0362,-0.1161,-0.3666,-0.057,0.0336,-0.0754,0.0004,-0.2457,-0.1961,0.075,0,0 -73,-0.515859,-0.627152,-0.977758,-2.0376,2.27273,0.717199,-0.328976,0.305623,1.1798,1.62759,1.56393,-2.75037,0.055126,0.320936,0.249963,0.183963,-0.024988,0.305472,-0.249996,-0.099993,-0.0357,-0.8344,0.9176,0.0711,0.9146,0.6259,0.1451,0.3101,0.0365,-0.1157,-0.3676,-0.0571,0.0339,-0.0763,0.0003,-0.2395,-0.2037,0.075,0,0 -74,-0.515782,-0.635396,-0.968814,-2.03658,2.28161,0.723146,-0.327038,0.308725,1.18017,1.62644,1.56025,-2.75094,0.055468,0.320165,0.249966,0.181633,-0.027099,0.306222,-0.249996,-0.099993,0.0077,-0.8244,0.8944,0.1022,0.8882,0.5947,0.1938,0.3102,0.0368,-0.1154,-0.3684,-0.0571,0.0342,-0.0771,0.0003,-0.233,-0.2111,0.075,0,0 -75,-0.515278,-0.643534,-0.960116,-2.03525,2.29021,0.728773,-0.324614,0.311826,1.18054,1.62529,1.55656,-2.75151,0.055813,0.319387,0.249969,0.179371,-0.029283,0.306972,-0.249996,-0.099993,0.0504,-0.8138,0.8698,0.1332,0.8604,0.5627,0.2424,0.3101,0.0371,-0.1151,-0.3693,-0.0572,0.0345,-0.0778,0.0003,-0.2262,-0.2184,0.075,0,0 -76,-0.514266,-0.651419,-0.951823,-2.03365,2.29838,0.733987,-0.321657,0.314873,1.18091,1.62416,1.55293,-2.75207,0.056148,0.318627,0.249962,0.177216,-0.031521,0.307708,-0.249994,-0.09999,0.1012,-0.7885,0.8293,0.1597,0.817,0.5214,0.2957,0.3047,0.0368,-0.1128,-0.3623,-0.0562,0.0335,-0.076,-0.0007,-0.2155,-0.2238,0.0736,0.0002,0.0003 -77,-0.512934,-0.659324,-0.943654,-2.03171,2.30639,0.738952,-0.318267,0.317971,1.18129,1.62302,1.54923,-2.75265,0.056497,0.317835,0.249963,0.175096,-0.03384,0.308457,-0.249994,-0.099989,0.1332,-0.7905,0.8169,0.1938,0.8004,0.4965,0.339,0.3098,0.0377,-0.1143,-0.3707,-0.0573,0.0349,-0.0792,0.0001,-0.212,-0.2319,0.0749,0,0.0001 -78,-0.511211,-0.667114,-0.935758,-2.02947,2.31408,0.743578,-0.314403,0.321072,1.18167,1.62188,1.54551,-2.75322,0.05685,0.317033,0.249966,0.173047,-0.036225,0.309206,-0.249994,-0.099989,0.1723,-0.779,0.7896,0.2243,0.7695,0.4626,0.3864,0.3101,0.0379,-0.1142,-0.372,-0.0573,0.0353,-0.0802,0.0003,-0.2049,-0.2385,0.0749,0,0 -79,-0.509097,-0.674775,-0.928154,-2.02693,2.32145,0.747854,-0.310063,0.324173,1.18205,1.62074,1.54178,-2.75379,0.057206,0.316223,0.249968,0.171074,-0.038672,0.309956,-0.249994,-0.099989,0.2114,-0.7661,0.7604,0.2543,0.7366,0.4276,0.434,0.3101,0.0383,-0.1138,-0.373,-0.0574,0.0356,-0.081,0.0002,-0.1973,-0.2447,0.075,0,0 -80,-0.506602,-0.682299,-0.920854,-2.02409,2.32847,0.751773,-0.305249,0.327275,1.18243,1.6196,1.53804,-2.75437,0.057564,0.315404,0.24997,0.169179,-0.04118,0.310706,-0.249994,-0.099989,0.2495,-0.7524,0.73,0.2838,0.7024,0.3919,0.4814,0.3102,0.0385,-0.1136,-0.374,-0.0575,0.0358,-0.0819,0.0002,-0.1895,-0.2508,0.075,0,0 -81,-0.503736,-0.689683,-0.913866,-2.02096,2.33515,0.755329,-0.299969,0.330376,1.18282,1.61847,1.53429,-2.75494,0.057925,0.314578,0.249972,0.167365,-0.043745,0.311456,-0.249993,-0.099989,0.2866,-0.7384,0.6988,0.3129,0.6673,0.3556,0.528,0.3101,0.0389,-0.1132,-0.3749,-0.0575,0.0361,-0.0826,0.0002,-0.1814,-0.2565,0.075,0.0001,0 -82,-0.500511,-0.69692,-0.907198,-2.01754,2.34146,0.758516,-0.294227,0.333478,1.18321,1.61734,1.53053,-2.75552,0.058289,0.313744,0.249973,0.165631,-0.046367,0.312206,-0.249993,-0.099989,0.3225,-0.7237,0.6668,0.3416,0.6314,0.3187,0.5742,0.3102,0.0391,-0.113,-0.3758,-0.0575,0.0364,-0.0834,0.0001,-0.1734,-0.2622,0.075,0,0 -83,-0.496941,-0.704006,-0.900857,-2.01384,2.34741,0.76133,-0.288034,0.33658,1.18361,1.61621,1.52676,-2.7561,0.058656,0.312901,0.249974,0.163982,-0.049041,0.312956,-0.249993,-0.099989,0.357,-0.7086,0.6341,0.3699,0.5948,0.2814,0.6193,0.3102,0.0394,-0.1126,-0.3768,-0.0577,0.0367,-0.0843,0.0001,-0.1649,-0.2674,0.075,0,0 -84,-0.493039,-0.710935,-0.894849,-2.00987,2.35298,0.763767,-0.281398,0.339682,1.18401,1.61509,1.52298,-2.75667,0.059025,0.312051,0.249975,0.162417,-0.051765,0.313705,-0.249993,-0.099989,0.3902,-0.6929,0.6008,0.3977,0.5577,0.2437,0.6636,0.3102,0.0397,-0.1123,-0.3778,-0.0577,0.0369,-0.085,0.0001,-0.1565,-0.2724,0.0749,0,0 -85,-0.488818,-0.717704,-0.889179,-2.00562,2.35819,0.765823,-0.274329,0.342785,1.18441,1.61397,1.5192,-2.75725,0.059397,0.311192,0.249976,0.160939,-0.054538,0.314455,-0.249993,-0.099989,0.4221,-0.6769,0.567,0.4248,0.5201,0.2056,0.7069,0.3103,0.04,-0.1121,-0.3788,-0.0577,0.0372,-0.0859,0.0001,-0.1478,-0.2773,0.075,0,0 -86,-0.484295,-0.724307,-0.88385,-2.0011,2.36301,0.767496,-0.266839,0.345888,1.18481,1.61285,1.5154,-2.75783,0.059772,0.310325,0.249977,0.15955,-0.057355,0.315205,-0.249993,-0.099989,0.4523,-0.6603,0.5329,0.4515,0.4823,0.1673,0.749,0.3103,0.0402,-0.1117,-0.3797,-0.0579,0.0375,-0.0867,0.0001,-0.1389,-0.2817,0.075,0,0 -87,-0.479483,-0.730741,-0.878866,-1.99633,2.36745,0.768784,-0.258941,0.348991,1.18521,1.61174,1.51159,-2.75841,0.060149,0.30945,0.249977,0.158249,-0.060214,0.315955,-0.249993,-0.099989,0.4812,-0.6434,0.4984,0.4775,0.4441,0.1288,0.7898,0.3103,0.0405,-0.1115,-0.3807,-0.0578,0.0377,-0.0875,0,-0.1301,-0.2859,0.075,0,0 -88,-0.474399,-0.737002,-0.874227,-1.9913,2.37151,0.769686,-0.250648,0.352095,1.18562,1.61063,1.50777,-2.75899,0.060529,0.308567,0.249978,0.15704,-0.063112,0.316705,-0.249993,-0.099989,0.5084,-0.6261,0.4639,0.5029,0.406,0.0902,0.8293,0.3104,0.0408,-0.1111,-0.3817,-0.058,0.038,-0.0883,0.0001,-0.1209,-0.2898,0.075,0,0 -89,-0.469059,-0.743085,-0.869935,-1.98602,2.37519,0.7702,-0.241976,0.355199,1.18603,1.60952,1.50395,-2.75957,0.060912,0.307676,0.249978,0.155922,-0.066047,0.317454,-0.249993,-0.099988,0.534,-0.6083,0.4292,0.5277,0.368,0.0514,0.8672,0.3104,0.0411,-0.1109,-0.3827,-0.058,0.0383,-0.0891,0,-0.1118,-0.2935,0.0749,0,0.0001 -90,-0.46348,-0.748987,-0.865988,-1.9805,2.37849,0.770328,-0.23294,0.358304,1.18645,1.60841,1.50011,-2.76015,0.061297,0.306777,0.249978,0.154898,-0.069016,0.318204,-0.249993,-0.099988,0.5579,-0.5902,0.3947,0.5519,0.3301,0.0128,0.9036,0.3105,0.0414,-0.1105,-0.3837,-0.0581,0.0385,-0.0899,0,-0.1024,-0.2969,0.075,0,0 -91,-0.457679,-0.754705,-0.862385,-1.97475,2.38142,0.77007,-0.223557,0.36141,1.18686,1.60731,1.49626,-2.76073,0.061685,0.305869,0.249978,0.153968,-0.072015,0.318954,-0.249993,-0.099988,0.5801,-0.5718,0.3603,0.5752,0.2926,-0.0258,0.9383,0.3106,0.0416,-0.1103,-0.3847,-0.0581,0.0388,-0.0908,0,-0.093,-0.2999,0.075,0,0 -92,-0.451673,-0.760236,-0.859123,-1.96877,2.38397,0.769427,-0.213844,0.364516,1.18728,1.60621,1.49241,-2.76131,0.062076,0.304953,0.249978,0.153133,-0.075042,0.319704,-0.249993,-0.099988,0.6006,-0.5531,0.3262,0.598,0.2555,-0.0643,0.9713,0.3106,0.0419,-0.11,-0.3857,-0.0582,0.0391,-0.0916,0,-0.0835,-0.3027,0.075,0,0 -93,-0.44548,-0.765575,-0.856199,-1.96257,2.38616,0.768404,-0.20382,0.367623,1.1877,1.60511,1.48854,-2.76189,0.062469,0.304029,0.249977,0.152394,-0.078094,0.320454,-0.249993,-0.099988,0.6193,-0.5339,0.2924,0.6199,0.2189,-0.1023,1.0024,0.3107,0.0421,-0.1097,-0.3867,-0.0583,0.0393,-0.0924,-0.0001,-0.0739,-0.3052,0.075,0,0 -94,-0.439118,-0.770721,-0.853608,-1.95616,2.38799,0.767001,-0.193504,0.37073,1.18813,1.60402,1.48466,-2.76248,0.062864,0.303096,0.249977,0.151751,-0.081167,0.321203,-0.249992,-0.099988,0.6362,-0.5146,0.2591,0.6411,0.1829,-0.1403,1.0316,0.3107,0.0425,-0.1094,-0.3878,-0.0584,0.0395,-0.0933,0,-0.0643,-0.3073,0.0749,0.0001,0 -95,-0.432604,-0.775671,-0.851345,-1.94955,2.38947,0.765224,-0.182916,0.373839,1.18855,1.60293,1.48077,-2.76306,0.063262,0.302155,0.249977,0.151206,-0.084259,0.321953,-0.249992,-0.099988,0.6514,-0.495,0.2263,0.6616,0.1478,-0.1777,1.0588,0.3109,0.0427,-0.1091,-0.3888,-0.0584,0.0398,-0.0941,0,-0.0545,-0.3092,0.075,0,0 -96,-0.425956,-0.780421,-0.849403,-1.94273,2.3906,0.763076,-0.172076,0.376948,1.18898,1.60184,1.47687,-2.76365,0.063663,0.301206,0.249977,0.150759,-0.087367,0.322703,-0.249992,-0.099988,0.6648,-0.475,0.1942,0.6812,0.1133,-0.2148,1.084,0.3109,0.0429,-0.1089,-0.3899,-0.0585,0.0401,-0.0949,0,-0.0447,-0.3108,0.075,0,0 -97,-0.419191,-0.784971,-0.847776,-1.93573,2.3914,0.760563,-0.161004,0.380058,1.18941,1.60075,1.47297,-2.76423,0.064066,0.300249,0.249977,0.150409,-0.090487,0.323453,-0.249992,-0.099988,0.6765,-0.455,0.1627,0.7001,0.0798,-0.2513,1.1072,0.311,0.0432,-0.1086,-0.3908,-0.0586,0.0403,-0.0957,0,-0.035,-0.312,0.075,0,0 -98,-0.412328,-0.789317,-0.846456,-1.92855,2.39187,0.757691,-0.149722,0.383169,1.18985,1.59967,1.46905,-2.76482,0.064472,0.299283,0.249976,0.150159,-0.093617,0.324202,-0.249992,-0.099988,0.6863,-0.4346,0.132,0.7183,0.0472,-0.2872,1.1282,0.3111,0.0435,-0.1082,-0.392,-0.0586,0.0406,-0.0966,-0.0001,-0.025,-0.313,0.0749,0,0 -99,-0.405384,-0.793458,-0.845435,-1.9212,2.39203,0.754465,-0.138251,0.386281,1.19029,1.59859,1.46512,-2.7654,0.06488,0.298308,0.249976,0.150007,-0.096753,0.324952,-0.249992,-0.099988,0.6944,-0.4141,0.1021,0.7354,0.0156,-0.3226,1.1471,0.3112,0.0438,-0.108,-0.393,-0.0587,0.0408,-0.0975,0,-0.0152,-0.3136,0.075,0,0 -100,-0.398282,-0.797423,-0.844734,-1.91355,2.39183,0.750789,-0.126435,0.389455,1.19074,1.59749,1.46108,-2.766,0.065307,0.29729,0.249983,0.149967,-0.099933,0.325718,-0.249994,-0.099991,0.7102,-0.3965,0.0701,0.765,-0.0194,-0.3676,1.1816,0.3174,0.0447,-0.1098,-0.4035,-0.0599,0.0427,-0.1018,0.0007,-0.004,-0.318,0.0766,-0.0002,-0.0003 -101,-0.391221,-0.801146,-0.84429,-1.90586,2.39138,0.746868,-0.114642,0.392573,1.19118,1.59642,1.45713,-2.76659,0.065721,0.296297,0.249984,0.150015,-0.103074,0.326468,-0.249994,-0.099991,0.7061,-0.3723,0.0444,0.7683,-0.045,-0.3921,1.1793,0.3118,0.0443,-0.1075,-0.3956,-0.059,0.0414,-0.0993,0.0001,0.0048,-0.3141,0.075,0,0 -102,-0.384132,-0.80466,-0.844115,-1.89804,2.39065,0.742618,-0.102728,0.395688,1.19162,1.59535,1.45316,-2.76718,0.066137,0.295297,0.249983,0.150161,-0.106211,0.327218,-0.249994,-0.099991,0.7089,-0.3514,0.0175,0.7826,-0.0729,-0.425,1.1914,0.3115,0.0445,-0.1072,-0.3964,-0.0589,0.0416,-0.1,-0.0001,0.0146,-0.3137,0.075,0,0 -103,-0.377028,-0.807962,-0.844202,-1.89007,2.38965,0.738045,-0.090712,0.398805,1.19207,1.59428,1.44919,-2.76777,0.066555,0.294288,0.249983,0.150406,-0.109342,0.327968,-0.249994,-0.099991,0.7104,-0.3302,-0.0087,0.7966,-0.1,-0.4573,1.2016,0.3117,0.0448,-0.107,-0.3974,-0.0591,0.0418,-0.1009,0,0.0245,-0.3131,0.075,0,0 -104,-0.369922,-0.811052,-0.844539,-1.88197,2.38839,0.733157,-0.078613,0.401924,1.19252,1.59321,1.4452,-2.76837,0.066976,0.29327,0.249983,0.150749,-0.112464,0.328717,-0.249994,-0.099991,0.7106,-0.309,-0.0337,0.8096,-0.126,-0.4888,1.2099,0.3119,0.045,-0.1066,-0.3985,-0.0591,0.0421,-0.1018,0,0.0343,-0.3122,0.0749,0,0 -105,-0.362831,-0.813929,-0.845119,-1.87376,2.38689,0.727962,-0.066451,0.405043,1.19297,1.59215,1.44121,-2.76896,0.0674,0.292245,0.249983,0.15119,-0.115574,0.329467,-0.249994,-0.099991,0.7091,-0.2877,-0.058,0.8219,-0.1507,-0.5195,1.2162,0.3119,0.0453,-0.1064,-0.3996,-0.0592,0.0424,-0.1025,0,0.0441,-0.311,0.075,0,0 -106,-0.355767,-0.816592,-0.845929,-1.86542,2.38514,0.722469,-0.054248,0.408164,1.19343,1.59109,1.4372,-2.76955,0.067825,0.29121,0.249983,0.151729,-0.118669,0.330217,-0.249994,-0.099991,0.7064,-0.2663,-0.081,0.8333,-0.1743,-0.5493,1.2203,0.3121,0.0455,-0.1061,-0.4007,-0.0593,0.0425,-0.1035,0,0.0539,-0.3095,0.075,0,0 -107,-0.348744,-0.819041,-0.846959,-1.85699,2.38318,0.716687,-0.042023,0.411287,1.19389,1.59003,1.43318,-2.77014,0.068253,0.290167,0.249983,0.152365,-0.121745,0.330967,-0.249994,-0.099991,0.7023,-0.2449,-0.103,0.8438,-0.1967,-0.5782,1.2225,0.3123,0.0458,-0.1059,-0.4019,-0.0593,0.0428,-0.1043,0,0.0636,-0.3076,0.075,0,0 -108,-0.341775,-0.821275,-0.8482,-1.84845,2.381,0.710626,-0.029794,0.41441,1.19435,1.58897,1.42915,-2.77074,0.068684,0.289115,0.249983,0.153098,-0.124799,0.331717,-0.249994,-0.099991,0.6969,-0.2234,-0.1241,0.8535,-0.2179,-0.6061,1.2229,0.3123,0.0461,-0.1056,-0.403,-0.0595,0.0431,-0.1052,0,0.0733,-0.3054,0.075,0,0 -109,-0.33487,-0.823294,-0.84964,-1.83983,2.37862,0.704296,-0.01758,0.417536,1.19481,1.58792,1.42511,-2.77133,0.069117,0.288054,0.249982,0.153926,-0.12783,0.332466,-0.249994,-0.099991,0.6905,-0.2019,-0.144,0.8624,-0.2379,-0.633,1.2214,0.3126,0.0462,-0.1053,-0.4041,-0.0595,0.0433,-0.1061,-0.0001,0.0828,-0.3031,0.0749,0,0 -110,-0.328042,-0.825099,-0.85127,-1.83112,2.37605,0.697705,-0.0054,0.420663,1.19528,1.58687,1.42106,-2.77193,0.069552,0.286985,0.249982,0.154849,-0.130832,0.333216,-0.249994,-0.099991,0.6828,-0.1805,-0.163,0.8703,-0.2569,-0.6591,1.218,0.3127,0.0466,-0.1051,-0.4053,-0.0597,0.0435,-0.1069,0,0.0923,-0.3002,0.075,0,0 -111,-0.321299,-0.826689,-0.853079,-1.82235,2.3733,0.690863,0.006729,0.423792,1.19574,1.58582,1.41699,-2.77253,0.06999,0.285907,0.249982,0.155866,-0.133805,0.333966,-0.249994,-0.09999,0.6743,-0.159,-0.1809,0.8775,-0.2747,-0.6842,1.2129,0.3129,0.0468,-0.1048,-0.4065,-0.0597,0.0438,-0.1078,0,0.1017,-0.2973,0.075,0,0.0001 -112,-0.314653,-0.828064,-0.855058,-1.81351,2.37039,0.683781,0.018791,0.426922,1.19621,1.58477,1.41291,-2.77312,0.07043,0.28482,0.249982,0.156976,-0.136744,0.334716,-0.249994,-0.09999,0.6646,-0.1375,-0.1979,0.8839,-0.2915,-0.7082,1.2062,0.313,0.047,-0.1045,-0.4076,-0.0598,0.044,-0.1087,0,0.111,-0.2939,0.075,0,0 -113,-0.30811,-0.829226,-0.857197,-1.80462,2.36732,0.676469,0.03077,0.430055,1.19669,1.58373,1.40883,-2.77372,0.070873,0.283724,0.249982,0.158177,-0.139647,0.335466,-0.249993,-0.09999,0.6543,-0.1162,-0.2139,0.8892,-0.3071,-0.7312,1.1979,0.3133,0.0472,-0.1043,-0.4088,-0.0599,0.0443,-0.1096,0,0.1201,-0.2903,0.075,0.0001,0 -114,-0.30168,-0.830174,-0.859486,-1.79568,2.3641,0.668935,0.04265,0.433189,1.19716,1.58269,1.40473,-2.77432,0.071318,0.282619,0.249982,0.15947,-0.14251,0.336215,-0.249993,-0.09999,0.643,-0.0948,-0.2289,0.8939,-0.3218,-0.7534,1.188,0.3134,0.0476,-0.1041,-0.4099,-0.06,0.0445,-0.1105,0,0.1293,-0.2863,0.0749,0,0 -115,-0.295369,-0.83091,-0.861916,-1.7867,2.36075,0.66119,0.054418,0.436325,1.19764,1.58165,1.40062,-2.77493,0.071765,0.281505,0.249982,0.160851,-0.145332,0.336965,-0.249993,-0.09999,0.6311,-0.0736,-0.243,0.8977,-0.3354,-0.7745,1.1768,0.3136,0.0477,-0.1038,-0.4112,-0.0601,0.0447,-0.1114,0,0.1381,-0.2822,0.075,0,0 -116,-0.289184,-0.831434,-0.864478,-1.7777,2.35726,0.653244,0.066059,0.439463,1.19812,1.58062,1.39649,-2.77553,0.072215,0.280383,0.249982,0.162321,-0.148109,0.337715,-0.249993,-0.09999,0.6185,-0.0524,-0.2562,0.9006,-0.3482,-0.7946,1.1641,0.3138,0.048,-0.1036,-0.4124,-0.0601,0.045,-0.1122,0,0.147,-0.2777,0.075,0,0 -117,-0.28313,-0.831747,-0.867164,-1.76867,2.35366,0.645107,0.07756,0.442603,1.1986,1.57958,1.39236,-2.77613,0.072667,0.279251,0.249982,0.163877,-0.150839,0.338465,-0.249993,-0.09999,0.6054,-0.0313,-0.2686,0.9027,-0.3599,-0.8137,1.1501,0.314,0.0482,-0.1033,-0.4136,-0.0603,0.0452,-0.1132,0,0.1556,-0.273,0.075,0,0 -118,-0.277213,-0.83185,-0.869966,-1.75963,2.34996,0.636787,0.08891,0.445745,1.19909,1.57855,1.38821,-2.77673,0.073121,0.27811,0.249982,0.165519,-0.153518,0.339214,-0.249993,-0.09999,0.5917,-0.0103,-0.2802,0.9041,-0.3709,-0.832,1.135,0.3142,0.0485,-0.1031,-0.4148,-0.0604,0.0454,-0.1141,0,0.1642,-0.2679,0.0749,0,0 -119,-0.271436,-0.831746,-0.872875,-1.75058,2.34614,0.628297,0.100096,0.44889,1.19957,1.57752,1.38405,-2.77734,0.073578,0.276961,0.249982,0.167243,-0.156144,0.339964,-0.249993,-0.09999,0.5777,0.0104,-0.2909,0.9044,-0.381,-0.849,1.1186,0.3145,0.0487,-0.1028,-0.4161,-0.0605,0.0457,-0.1149,0,0.1724,-0.2626,0.075,0,0 -120,-0.265803,-0.831434,-0.875883,-1.74154,2.34224,0.619643,0.111108,0.452036,1.20006,1.5765,1.37987,-2.77794,0.074037,0.275802,0.249982,0.169049,-0.158716,0.340714,-0.249993,-0.09999,0.5633,0.0312,-0.3008,0.9042,-0.3904,-0.8654,1.1012,0.3146,0.049,-0.1026,-0.4173,-0.0605,0.0459,-0.1159,0,0.1806,-0.2572,0.075,0,0 -121,-0.260318,-0.830917,-0.878985,-1.73251,2.33825,0.610838,0.121936,0.455185,1.20056,1.57547,1.37569,-2.77855,0.074498,0.274634,0.249982,0.170935,-0.161229,0.341464,-0.249993,-0.09999,0.5485,0.0517,-0.3102,0.9029,-0.3988,-0.8805,1.0828,0.3149,0.0492,-0.1023,-0.4185,-0.0607,0.0461,-0.1168,0,0.1886,-0.2513,0.075,0,0 -122,-0.254982,-0.830196,-0.882173,-1.7235,2.33419,0.601889,0.13257,0.458337,1.20105,1.57445,1.37149,-2.77916,0.074962,0.273456,0.249982,0.172899,-0.163681,0.342214,-0.249993,-0.09999,0.5336,0.0721,-0.3188,0.901,-0.4067,-0.8949,1.0634,0.3152,0.0494,-0.1021,-0.4198,-0.0608,0.0464,-0.1178,0,0.1964,-0.2452,0.075,0,0 -123,-0.249797,-0.829273,-0.885439,-1.71452,2.33005,0.592807,0.143001,0.461491,1.20155,1.57343,1.36728,-2.77977,0.075428,0.27227,0.249982,0.174939,-0.166071,0.342963,-0.249993,-0.099989,0.5185,0.0923,-0.3266,0.8982,-0.4139,-0.9082,1.0431,0.3154,0.0496,-0.1019,-0.4211,-0.0609,0.0466,-0.1186,0,0.204,-0.239,0.0749,0,0.0001 -124,-0.244764,-0.82815,-0.88878,-1.70557,2.32584,0.5836,0.153221,0.464647,1.20204,1.57242,1.36306,-2.78038,0.075896,0.271074,0.249982,0.177054,-0.168396,0.343713,-0.249993,-0.099989,0.5033,0.1123,-0.3341,0.8946,-0.4205,-0.9207,1.022,0.3156,0.0499,-0.1017,-0.4224,-0.0609,0.0468,-0.1196,0,0.2115,-0.2325,0.075,0,0 -125,-0.239884,-0.826828,-0.892187,-1.69667,2.32158,0.574278,0.163223,0.467806,1.20255,1.5714,1.35882,-2.78099,0.076366,0.269868,0.249982,0.17924,-0.170653,0.344463,-0.249993,-0.099989,0.488,0.1322,-0.3407,0.8903,-0.4264,-0.9322,1.0002,0.3159,0.0502,-0.1014,-0.4237,-0.0611,0.047,-0.1206,0,0.2186,-0.2257,0.075,0,0 -126,-0.235158,-0.82531,-0.895657,-1.68782,2.31726,0.564851,0.173,0.470967,1.20305,1.57039,1.35457,-2.7816,0.076839,0.268654,0.249982,0.181496,-0.17284,0.345212,-0.249993,-0.099989,0.4726,0.1518,-0.347,0.885,-0.4319,-0.9427,0.9777,0.3161,0.0503,-0.1012,-0.425,-0.0612,0.0473,-0.1214,0,0.2256,-0.2187,0.0749,0,0 -127,-0.230584,-0.823598,-0.899184,-1.67903,2.31289,0.555326,0.182545,0.474132,1.20355,1.56938,1.35031,-2.78221,0.077314,0.26743,0.249982,0.183819,-0.174956,0.345962,-0.249992,-0.099989,0.4574,0.1712,-0.3527,0.8791,-0.4367,-0.9525,0.9545,0.3165,0.0506,-0.101,-0.4263,-0.0613,0.0475,-0.1224,0,0.2323,-0.2116,0.075,0.0001,0 -128,-0.226163,-0.821694,-0.902764,-1.67031,2.30848,0.545714,0.191852,0.477299,1.20406,1.56837,1.34603,-2.78283,0.077792,0.266196,0.249982,0.186208,-0.176997,0.346712,-0.249992,-0.099989,0.4421,0.1904,-0.358,0.8724,-0.4412,-0.9612,0.9307,0.3167,0.0508,-0.1007,-0.4276,-0.0615,0.0478,-0.1234,0,0.2389,-0.2041,0.075,0,0 -129,-0.221892,-0.819599,-0.906391,-1.66166,2.30403,0.536023,0.200916,0.480469,1.20457,1.56737,1.34174,-2.78344,0.078271,0.264953,0.249982,0.18866,-0.178963,0.347462,-0.249992,-0.099989,0.4271,0.2095,-0.3627,0.8648,-0.4453,-0.9691,0.9064,0.317,0.0511,-0.1005,-0.429,-0.0615,0.0479,-0.1243,0,0.2452,-0.1966,0.075,0,0 -130,-0.217771,-0.817317,-0.910063,-1.65309,2.29954,0.526263,0.209732,0.483641,1.20509,1.56637,1.33744,-2.78406,0.078753,0.2637,0.249983,0.191172,-0.18085,0.348211,-0.249992,-0.099989,0.4121,0.2282,-0.3672,0.8566,-0.4488,-0.976,0.8816,0.3172,0.0513,-0.1003,-0.4303,-0.0616,0.0482,-0.1253,0.0001,0.2512,-0.1887,0.0749,0,0 -131,-0.213798,-0.814849,-0.913775,-1.64462,2.29502,0.516441,0.218297,0.486817,1.2056,1.56536,1.33312,-2.78468,0.079237,0.262438,0.249983,0.193742,-0.182658,0.348961,-0.249992,-0.099989,0.3973,0.2468,-0.3712,0.8475,-0.4521,-0.9822,0.8565,0.3176,0.0515,-0.1001,-0.4317,-0.0618,0.0484,-0.1262,0,0.257,-0.1808,0.075,0,0 -132,-0.20997,-0.812198,-0.917524,-1.63624,2.29047,0.506566,0.226604,0.489996,1.20612,1.56437,1.32879,-2.7853,0.079724,0.261166,0.249983,0.196367,-0.184384,0.349711,-0.249992,-0.099989,0.3828,0.2651,-0.3749,0.8378,-0.4551,-0.9875,0.8307,0.3179,0.0517,-0.0999,-0.4331,-0.0619,0.0487,-0.1272,0,0.2625,-0.1726,0.075,0,0 -133,-0.206308,-0.809373,-0.921271,-1.62812,2.28594,0.49679,0.234542,0.493109,1.20663,1.56339,1.32456,-2.7859,0.080193,0.259931,0.249977,0.199014,-0.186,0.350443,-0.249989,-0.099984,0.3662,0.2825,-0.3747,0.8123,-0.4527,-0.9776,0.7938,0.3113,0.0511,-0.0976,-0.4229,-0.0607,0.0469,-0.1235,-0.0006,0.2647,-0.1616,0.0732,0.0003,0.0005 -134,-0.20276,-0.806363,-0.925086,-1.61996,2.28134,0.486841,0.242327,0.496288,1.20715,1.5624,1.32021,-2.78652,0.080682,0.258644,0.249977,0.201742,-0.187558,0.351191,-0.249989,-0.099984,0.3548,0.301,-0.3815,0.8153,-0.4601,-0.9949,0.7785,0.3179,0.0521,-0.0993,-0.4348,-0.062,0.0489,-0.1287,0,0.2728,-0.1558,0.0748,0,0 -135,-0.199349,-0.803179,-0.92893,-1.61192,2.27671,0.476859,0.24985,0.499476,1.20768,1.5614,1.31584,-2.78714,0.081174,0.257344,0.249977,0.204518,-0.18903,0.351941,-0.249989,-0.099983,0.3411,0.3184,-0.3844,0.804,-0.4624,-0.9982,0.7523,0.3188,0.0525,-0.0992,-0.4371,-0.0622,0.0492,-0.13,0,0.2776,-0.1472,0.075,0,0.0001 -136,-0.196073,-0.799821,-0.932801,-1.60401,2.27207,0.466858,0.257105,0.502667,1.2082,1.56042,1.31146,-2.78777,0.08167,0.256033,0.249978,0.207339,-0.190415,0.35269,-0.249988,-0.099983,0.3276,0.3358,-0.3871,0.7913,-0.4643,-1.0001,0.7255,0.3191,0.0526,-0.099,-0.4386,-0.0623,0.0496,-0.1311,0.0001,0.2821,-0.1385,0.0749,0.0001,0 -137,-0.189338,-0.799968,-0.943864,-1.59614,2.27357,0.453765,0.26,0.506047,1.20875,1.55937,1.30674,-2.78843,0.082223,0.25458,0.25003,0.210233,-0.191671,0.353486,-0.249997,-0.099996,0.6735,-0.0147,-1.1063,0.7873,0.1494,-1.3093,0.2895,0.338,0.0551,-0.1042,-0.4714,-0.0659,0.0553,-0.1453,0.0052,0.2894,-0.1256,0.0796,-0.0009,-0.0013 -138,-0.179941,-0.802736,-0.960093,-1.5885,2.27953,0.438622,0.26,0.509267,1.20929,1.55838,1.30229,-2.78906,0.08273,0.253233,0.250032,0.213137,-0.192872,0.35424,-0.249998,-0.099997,0.9397,-0.2768,-1.6229,0.7634,0.5959,-1.5143,0,0.322,0.0534,-0.0992,-0.4453,-0.063,0.0507,-0.1347,0.0002,0.2904,-0.1201,0.0754,-0.0001,-0.0001 -139,-0.171151,-0.805136,-0.975801,-1.58111,2.28495,0.42378,0.26,0.512455,1.20982,1.5574,1.29788,-2.78968,0.083228,0.251902,0.250039,0.216063,-0.193958,0.354986,-0.249997,-0.099996,0.879,-0.24,-1.5708,0.7393,0.5429,-1.4842,0,0.3188,0.0531,-0.098,-0.4405,-0.0624,0.0498,-0.1331,0.0007,0.2926,-0.1086,0.0746,0.0001,0.0001 -140,-0.162675,-0.807369,-0.991373,-1.57379,2.29014,0.408954,0.26,0.515662,1.21035,1.55642,1.29344,-2.79031,0.083732,0.25055,0.250038,0.219036,-0.194976,0.355736,-0.249997,-0.099996,0.8476,-0.2233,-1.5572,0.7323,0.5184,-1.4826,0,0.3207,0.0536,-0.0982,-0.4447,-0.0629,0.0504,-0.1352,-0.0001,0.2973,-0.1018,0.075,0,0 -141,-0.154652,-0.809315,-1.00659,-1.56662,2.29492,0.394309,0.26,0.518873,1.21089,1.55544,1.28897,-2.79094,0.084239,0.249187,0.250037,0.22204,-0.195904,0.356486,-0.249997,-0.099996,0.8023,-0.1946,-1.5221,0.7167,0.4782,-1.4645,0,0.3211,0.0538,-0.0981,-0.4463,-0.063,0.0507,-0.1363,-0.0001,0.3004,-0.0928,0.075,0,0 -142,-0.147083,-0.810949,-1.02143,-1.55962,2.29928,0.379875,0.26,0.522089,1.21143,1.55446,1.28449,-2.79157,0.084749,0.247814,0.250035,0.225073,-0.196737,0.357236,-0.249997,-0.099996,0.7569,-0.1634,-1.4838,0.6999,0.4358,-1.4434,0,0.3216,0.054,-0.0979,-0.4478,-0.0632,0.051,-0.1373,-0.0002,0.3033,-0.0833,0.075,0,0 -143,-0.13996,-0.812254,-1.03587,-1.5528,2.3032,0.365674,0.26,0.525308,1.21198,1.55348,1.28,-2.7922,0.08526,0.24643,0.250034,0.228129,-0.197474,0.357986,-0.249997,-0.099996,0.7123,-0.1305,-1.4435,0.6825,0.3921,-1.4201,0,0.3219,0.0543,-0.0976,-0.4493,-0.0633,0.0511,-0.1384,-0.0001,0.3056,-0.0737,0.075,0,0 -144,-0.133274,-0.813212,-1.04988,-1.54615,2.30667,0.351724,0.26,0.528532,1.21252,1.55251,1.27549,-2.79284,0.085774,0.245036,0.250032,0.231208,-0.198116,0.358735,-0.249996,-0.099995,0.6686,-0.0958,-1.4016,0.6644,0.3473,-1.395,0,0.3224,0.0545,-0.0975,-0.4508,-0.0634,0.0514,-0.1394,-0.0002,0.3079,-0.0642,0.0749,0.0001,0.0001 -145,-0.127013,-0.813809,-1.06346,-1.5397,2.30969,0.338045,0.26,0.531759,1.21307,1.55154,1.27097,-2.79347,0.08629,0.243631,0.25003,0.234305,-0.198661,0.359485,-0.249996,-0.099995,0.6261,-0.0597,-1.3579,0.6456,0.3015,-1.3679,0,0.3227,0.0547,-0.0973,-0.4525,-0.0635,0.0516,-0.1405,-0.0002,0.3097,-0.0545,0.075,0,0 -146,-0.121294,-0.813902,-1.07638,-1.53353,2.31207,0.324821,0.26,0.534966,1.21361,1.55057,1.26647,-2.79411,0.086801,0.242236,0.250036,0.237402,-0.199082,0.360229,-0.249995,-0.099993,0.5719,-0.0093,-1.2918,0.6167,0.2383,-1.3224,0,0.3207,0.0546,-0.0965,-0.4495,-0.0633,0.0511,-0.1395,0.0006,0.3097,-0.0421,0.0744,0.0001,0.0002 -147,-0.115848,-0.813728,-1.08904,-1.52747,2.31413,0.311744,0.26,0.538202,1.21416,1.5496,1.26192,-2.79474,0.087321,0.240811,0.250035,0.240526,-0.199431,0.360979,-0.249995,-0.099993,0.5446,0.0174,-1.2656,0.6058,0.206,-1.3077,0,0.3236,0.0551,-0.0969,-0.4555,-0.0638,0.052,-0.1425,-0.0001,0.3124,-0.0349,0.075,0,0 -148,-0.110778,-0.813166,-1.10123,-1.52161,2.31572,0.298973,0.26,0.541442,1.21472,1.54863,1.25735,-2.79538,0.087844,0.239375,0.250033,0.243661,-0.199685,0.361728,-0.249995,-0.099993,0.507,0.0562,-1.2199,0.5862,0.159,-1.2771,0,0.324,0.0554,-0.0967,-0.4571,-0.064,0.0523,-0.1436,-0.0002,0.3135,-0.0254,0.0749,0,0 -149,-0.106081,-0.812196,-1.11295,-1.51596,2.31682,0.286536,0.26,0.544687,1.21527,1.54767,1.25276,-2.79603,0.088368,0.237928,0.250031,0.246802,-0.19984,0.362478,-0.249995,-0.099993,0.4697,0.097,-1.1713,0.5651,0.1097,-1.2437,0,0.3245,0.0556,-0.0966,-0.4588,-0.0641,0.0524,-0.1447,-0.0002,0.3141,-0.0155,0.075,0,0 -150,-0.101742,-0.810805,-1.12416,-1.51052,2.31742,0.274449,0.26,0.547937,1.21583,1.5467,1.24816,-2.79667,0.088896,0.23647,0.250029,0.249946,-0.199898,0.363228,-0.249995,-0.099993,0.4339,0.1391,-1.1216,0.5434,0.0598,-1.2087,0,0.325,0.0558,-0.0964,-0.4604,-0.0643,0.0528,-0.1458,-0.0002,0.3144,-0.0058,0.075,0,0 -151,-0.097855,-0.808869,-1.13467,-1.50541,2.31736,0.262888,0.26,0.551154,1.21639,1.54575,1.2436,-2.79731,0.089413,0.23503,0.250033,0.253072,-0.199833,0.363968,-0.249993,-0.09999,0.3887,0.1936,-1.051,0.5119,-0.0056,-1.1561,0,0.3217,0.0556,-0.0952,-0.4554,-0.0637,0.0517,-0.144,0.0004,0.3126,0.0065,0.074,0.0002,0.0003 -152,-0.094191,-0.806611,-1.14487,-1.50042,2.31694,0.251539,0.26,0.554412,1.21695,1.54479,1.23897,-2.79795,0.089944,0.233552,0.250032,0.256213,-0.199693,0.364718,-0.249993,-0.09999,0.3664,0.2258,-1.0193,0.498,-0.0423,-1.1349,0,0.3258,0.0562,-0.0961,-0.4634,-0.0646,0.0531,-0.1478,-0.0001,0.3141,0.014,0.075,0,0 -153,-0.090829,-0.80393,-1.15456,-1.49567,2.31602,0.240555,0.26,0.557676,1.21751,1.54383,1.23431,-2.7986,0.090478,0.232061,0.250029,0.259349,-0.199457,0.365467,-0.249993,-0.09999,0.3362,0.2681,-0.9697,0.4759,-0.0917,-1.0984,0,0.3264,0.0565,-0.0959,-0.4654,-0.0647,0.0534,-0.1491,-0.0003,0.3136,0.0236,0.0749,0,0 -154,-0.087869,-0.800706,-1.16354,-1.49124,2.31446,0.230136,0.26,0.560889,1.21808,1.54289,1.22974,-2.79924,0.090997,0.230602,0.250034,0.262452,-0.199103,0.366204,-0.24999,-0.099986,0.296,0.3224,-0.8974,0.4426,-0.1562,-1.0419,0,0.3213,0.056,-0.0942,-0.4573,-0.0638,0.0519,-0.1459,0.0005,0.3103,0.0354,0.0737,0.0003,0.0004 -155,-0.085092,-0.797135,-1.17219,-1.48696,2.31251,0.219943,0.26,0.564159,1.21864,1.54194,1.22506,-2.79989,0.091534,0.229092,0.250032,0.265565,-0.19867,0.366952,-0.24999,-0.099986,0.2777,0.3571,-0.865,0.4276,-0.1948,-1.0193,0,0.327,0.0569,-0.0955,-0.4681,-0.065,0.0537,-0.151,-0.0002,0.3113,0.0433,0.0748,0,0 -156,-0.082572,-0.793139,-1.18034,-1.48292,2.31007,0.210131,0.26,0.567438,1.21921,1.54098,1.22036,-2.80054,0.092074,0.227569,0.250029,0.268665,-0.198144,0.367702,-0.24999,-0.099985,0.252,0.3996,-0.8153,0.4042,-0.244,-0.9812,0,0.3279,0.0571,-0.0954,-0.4704,-0.0652,0.054,-0.1523,-0.0003,0.31,0.0526,0.075,0,0.0001 -157,-0.080307,-0.788702,-1.18797,-1.47913,2.30712,0.200725,0.26,0.570723,1.21979,1.54003,1.21563,-2.80119,0.092617,0.226033,0.250026,0.271747,-0.197521,0.368451,-0.249989,-0.099985,0.2265,0.4437,-0.7633,0.3793,-0.2946,-0.9406,0,0.3285,0.0574,-0.0953,-0.4722,-0.0653,0.0543,-0.1536,-0.0003,0.3082,0.0623,0.0749,0.0001,0 -158,-0.078367,-0.783783,-1.19491,-1.47567,2.3036,0.191891,0.26,0.57393,1.22035,1.5391,1.21104,-2.80183,0.093137,0.224548,0.25003,0.274775,-0.196793,0.369181,-0.249986,-0.09998,0.194,0.4919,-0.6939,0.346,-0.3522,-0.8834,0,0.3207,0.0565,-0.0929,-0.4596,-0.0639,0.052,-0.1485,0.0004,0.3028,0.0728,0.073,0.0003,0.0005 -159,-0.076574,-0.778469,-1.20151,-1.47239,2.29965,0.183307,0.26,0.577217,1.22093,1.53815,1.2063,-2.80249,0.093681,0.222997,0.250028,0.27781,-0.195978,0.369929,-0.249985,-0.099979,0.1793,0.5314,-0.6595,0.3279,-0.3955,-0.8584,0,0.3287,0.0578,-0.0947,-0.4742,-0.0656,0.0544,-0.1551,-0.0002,0.3035,0.0815,0.0748,0.0001,0.0001 -160,-0.074985,-0.77274,-1.20762,-1.46936,2.29522,0.175117,0.26,0.580517,1.22151,1.5372,1.20152,-2.80314,0.09423,0.221429,0.250025,0.280819,-0.195072,0.370678,-0.249985,-0.099979,0.1589,0.5729,-0.611,0.3027,-0.443,-0.819,0,0.33,0.058,-0.0948,-0.4774,-0.0658,0.0549,-0.1568,-0.0003,0.3009,0.0906,0.0749,0,0 -161,-0.073599,-0.766586,-1.21323,-1.4666,2.2903,0.167338,0.26,0.583823,1.22209,1.53626,1.19673,-2.8038,0.094782,0.219848,0.250021,0.283798,-0.194071,0.371427,-0.249985,-0.099979,0.1386,0.6154,-0.5608,0.2763,-0.4915,-0.7779,0,0.3306,0.0583,-0.0947,-0.4793,-0.066,0.0552,-0.1581,-0.0004,0.2979,0.1001,0.0749,0,0 -162,-0.072387,-0.760035,-1.21837,-1.4641,2.28493,0.15995,0.26,0.587136,1.22268,1.53531,1.19192,-2.80446,0.095336,0.218255,0.250016,0.286745,-0.192982,0.372176,-0.249985,-0.099978,0.1212,0.6551,-0.5141,0.2502,-0.537,-0.7388,0,0.3313,0.0585,-0.0946,-0.4811,-0.0661,0.0554,-0.1593,-0.0005,0.2947,0.1089,0.0749,0,0.0001 -163,-0.071224,-0.75335,-1.22347,-1.4619,2.27946,0.152798,0.259729,0.590454,1.22327,1.53437,1.18709,-2.80513,0.095892,0.21665,0.250015,0.289652,-0.19179,0.372926,-0.249985,-0.099978,0.1163,0.6685,-0.5099,0.2199,-0.5472,-0.7152,-0.0271,0.3318,0.0587,-0.0944,-0.4831,-0.0664,0.0556,-0.1605,-0.0001,0.2907,0.1192,0.075,0,0 -164,-0.070077,-0.746586,-1.2286,-1.45999,2.27395,0.145843,0.259163,0.593779,1.22386,1.53343,1.18224,-2.80579,0.09645,0.215033,0.250018,0.292517,-0.190501,0.373675,-0.249984,-0.099978,0.1147,0.6764,-0.5133,0.1908,-0.551,-0.6955,-0.0566,0.3325,0.059,-0.0942,-0.4849,-0.0665,0.0558,-0.1617,0.0003,0.2865,0.1289,0.0749,0.0001,0 -165,-0.068931,-0.73975,-1.23377,-1.45836,2.2684,0.139081,0.258333,0.59711,1.22445,1.53248,1.17737,-2.80646,0.097011,0.213403,0.25002,0.29534,-0.189123,0.374424,-0.249984,-0.099978,0.1146,0.6836,-0.5169,0.1631,-0.5549,-0.6762,-0.083,0.3331,0.0591,-0.0942,-0.4869,-0.0667,0.0561,-0.163,0.0002,0.2823,0.1378,0.0749,0,0 -166,-0.067781,-0.732847,-1.23897,-1.45701,2.26281,0.132517,0.257241,0.600447,1.22504,1.53154,1.17248,-2.80713,0.097574,0.211762,0.250023,0.298118,-0.187656,0.375174,-0.249984,-0.099977,0.115,0.6903,-0.5205,0.1355,-0.5588,-0.6564,-0.1092,0.3337,0.0594,-0.094,-0.4889,-0.0669,0.0563,-0.1641,0.0003,0.2778,0.1467,0.075,0,0.0001 -167,-0.066626,-0.72588,-1.24422,-1.45593,2.25718,0.126158,0.255893,0.603792,1.22564,1.5306,1.16757,-2.8078,0.098139,0.210108,0.250026,0.300849,-0.186103,0.375923,-0.249984,-0.099977,0.1155,0.6967,-0.5242,0.1076,-0.5629,-0.6359,-0.1348,0.3345,0.0597,-0.0939,-0.4908,-0.0671,0.0565,-0.1654,0.0003,0.2731,0.1553,0.0749,0,0 -168,-0.065462,-0.718854,-1.24949,-1.45513,2.25151,0.12001,0.254291,0.607142,1.22624,1.52967,1.16264,-2.80847,0.098707,0.208441,0.250029,0.303529,-0.184465,0.376672,-0.249984,-0.099977,0.1164,0.7026,-0.5279,0.0796,-0.567,-0.6148,-0.1602,0.335,0.0599,-0.0938,-0.4928,-0.0672,0.0568,-0.1667,0.0003,0.268,0.1638,0.0749,0,0 -169,-0.064288,-0.711771,-1.25481,-1.45462,2.2458,0.11408,0.25244,0.6105,1.22684,1.52873,1.15769,-2.80915,0.099277,0.206762,0.250032,0.306157,-0.182744,0.377421,-0.249983,-0.099977,0.1174,0.7083,-0.5318,0.0516,-0.5712,-0.593,-0.1851,0.3358,0.0601,-0.0937,-0.4949,-0.0675,0.057,-0.1679,0.0003,0.2628,0.1721,0.0749,0.0001,0 -170,-0.063101,-0.704637,-1.26017,-1.45438,2.24005,0.108372,0.250343,0.613865,1.22744,1.52779,1.15273,-2.80982,0.099849,0.20507,0.250035,0.308729,-0.180941,0.378171,-0.249983,-0.099976,0.1187,0.7134,-0.5356,0.0235,-0.5755,-0.5708,-0.2097,0.3365,0.0603,-0.0935,-0.4969,-0.0676,0.0572,-0.1692,0.0003,0.2572,0.1803,0.075,0,0.0001 -171,-0.061899,-0.697456,-1.26556,-1.45443,2.23425,0.102894,0.248005,0.617237,1.22805,1.52686,1.14774,-2.8105,0.100424,0.203365,0.250038,0.311244,-0.179058,0.37892,-0.249983,-0.099976,0.1202,0.7181,-0.5395,-0.0047,-0.5798,-0.5478,-0.2338,0.3372,0.0606,-0.0935,-0.499,-0.0679,0.0575,-0.1705,0.0003,0.2515,0.1883,0.0749,0,0 -172,-0.060681,-0.690231,-1.271,-1.45476,2.22841,0.097651,0.24543,0.620616,1.22866,1.52593,1.14272,-2.81118,0.101001,0.201647,0.250041,0.313698,-0.177097,0.379669,-0.249983,-0.099976,0.1218,0.7225,-0.5434,-0.0328,-0.584,-0.5243,-0.2575,0.3379,0.0608,-0.0933,-0.5011,-0.068,0.0577,-0.1718,0.0003,0.2454,0.1961,0.0749,0,0 -173,-0.059446,-0.682966,-1.27647,-1.45537,2.22252,0.092649,0.242622,0.624002,1.22927,1.52499,1.13769,-2.81187,0.101581,0.199916,0.250044,0.316089,-0.17506,0.380418,-0.249983,-0.099976,0.1235,0.7265,-0.5473,-0.0609,-0.5884,-0.5002,-0.2808,0.3386,0.061,-0.0933,-0.5032,-0.0683,0.058,-0.1731,0.0003,0.2391,0.2037,0.0749,0,0 -174,-0.058191,-0.675667,-1.28198,-1.45626,2.2166,0.087892,0.239587,0.627396,1.22988,1.52406,1.13264,-2.81255,0.102163,0.198172,0.250047,0.318415,-0.172949,0.381167,-0.249982,-0.099975,0.1255,0.7299,-0.5512,-0.089,-0.5926,-0.4757,-0.3035,0.3394,0.0613,-0.0931,-0.5053,-0.0684,0.0582,-0.1744,0.0003,0.2326,0.2111,0.0749,0.0001,0.0001 -175,-0.056917,-0.668337,-1.28753,-1.45742,2.21063,0.083387,0.236329,0.630798,1.23049,1.52313,1.12756,-2.81324,0.102747,0.196414,0.250051,0.320673,-0.170766,0.381916,-0.249982,-0.099975,0.1274,0.733,-0.5551,-0.1169,-0.5968,-0.4505,-0.3258,0.3402,0.0615,-0.0931,-0.5076,-0.0687,0.0584,-0.1758,0.0004,0.2258,0.2183,0.0749,0,0 -176,-0.055623,-0.660981,-1.29312,-1.45887,2.20462,0.079138,0.232854,0.634208,1.23111,1.5222,1.12247,-2.81392,0.103334,0.194643,0.250054,0.322862,-0.168513,0.382666,-0.249982,-0.099975,0.1294,0.7356,-0.559,-0.1447,-0.6008,-0.4249,-0.3475,0.341,0.0618,-0.0929,-0.5097,-0.0688,0.0587,-0.1771,0.0003,0.2189,0.2253,0.075,0,0 -177,-0.054309,-0.653604,-1.29875,-1.4606,2.19857,0.07515,0.229166,0.637626,1.23173,1.52127,1.11735,-2.81462,0.103923,0.192858,0.250057,0.324979,-0.166192,0.383415,-0.249982,-0.099975,0.1314,0.7377,-0.5627,-0.1724,-0.6047,-0.3988,-0.3688,0.3418,0.062,-0.0929,-0.512,-0.0691,0.0589,-0.1785,0.0003,0.2117,0.2321,0.0749,0,0 -178,-0.052974,-0.646209,-1.30441,-1.46259,2.19249,0.071428,0.225271,0.641052,1.23235,1.52035,1.1122,-2.81531,0.104515,0.191059,0.250061,0.327022,-0.163807,0.384164,-0.249982,-0.099974,0.1335,0.7395,-0.5664,-0.1998,-0.6086,-0.3722,-0.3895,0.3426,0.0622,-0.0928,-0.5143,-0.0693,0.0592,-0.1799,0.0004,0.2043,0.2385,0.0749,0,0.0001 -179,-0.05162,-0.638803,-1.31011,-1.46487,2.18637,0.067976,0.221174,0.644486,1.23298,1.51942,1.10704,-2.816,0.105109,0.189246,0.250064,0.328988,-0.161358,0.384913,-0.249981,-0.099974,0.1354,0.7406,-0.5701,-0.2272,-0.6121,-0.3452,-0.4097,0.3434,0.0625,-0.0927,-0.5165,-0.0695,0.0594,-0.1813,0.0003,0.1966,0.2449,0.0749,0.0001,0 -180,-0.050246,-0.631389,-1.31585,-1.46741,2.18021,0.064799,0.216881,0.647929,1.23361,1.51849,1.10185,-2.8167,0.105706,0.18742,0.250068,0.330877,-0.158848,0.385662,-0.249981,-0.099974,0.1374,0.7414,-0.5735,-0.2542,-0.6154,-0.3177,-0.4293,0.3443,0.0628,-0.0926,-0.5189,-0.0698,0.0597,-0.1826,0.0004,0.1889,0.251,0.0749,0,0 -181,-0.048735,-0.623867,-1.32172,-1.47034,2.17394,0.061916,0.212374,0.651486,1.23425,1.51754,1.09645,-2.81742,0.106337,0.185494,0.250054,0.332698,-0.156254,0.386434,-0.249985,-0.09998,0.1511,0.7522,-0.5866,-0.2928,-0.6271,-0.2883,-0.4507,0.3557,0.0643,-0.0953,-0.5402,-0.072,0.0631,-0.1926,-0.0014,0.1821,0.2594,0.0772,-0.0004,-0.0006 -182,-0.04731,-0.616446,-1.32753,-1.47343,2.16772,0.059309,0.207705,0.654957,1.23488,1.51661,1.09119,-2.81813,0.106942,0.183629,0.250055,0.334424,-0.153629,0.387185,-0.249986,-0.09998,0.1425,0.7421,-0.581,-0.3092,-0.6219,-0.2607,-0.4669,0.3471,0.0634,-0.0927,-0.5256,-0.0704,0.0605,-0.1865,0.0001,0.1726,0.2625,0.0751,-0.0001,0 -183,-0.04588,-0.609037,-1.33336,-1.47677,2.16148,0.056984,0.202857,0.658429,1.23552,1.51569,1.08593,-2.81883,0.107547,0.181757,0.250058,0.336067,-0.150952,0.387935,-0.249985,-0.09998,0.143,0.7409,-0.5835,-0.3344,-0.624,-0.2325,-0.4848,0.3472,0.0635,-0.0925,-0.5265,-0.0705,0.0605,-0.1872,0.0003,0.1643,0.2677,0.075,0.0001,0 -184,-0.044439,-0.601642,-1.33922,-1.48037,2.15522,0.054948,0.197834,0.66191,1.23616,1.51476,1.08064,-2.81954,0.108154,0.179871,0.250061,0.337625,-0.148225,0.388684,-0.249985,-0.09998,0.1441,0.7395,-0.5863,-0.3601,-0.6261,-0.2036,-0.5023,0.3481,0.0638,-0.0924,-0.5289,-0.0707,0.0607,-0.1886,0.0003,0.1558,0.2727,0.0749,0,0 -185,-0.042988,-0.594267,-1.34511,-1.48423,2.14895,0.053204,0.192645,0.665399,1.2368,1.51384,1.07533,-2.82025,0.108764,0.17797,0.250063,0.339097,-0.14545,0.389433,-0.249985,-0.099979,0.1451,0.7375,-0.5887,-0.3855,-0.6276,-0.1744,-0.5189,0.3489,0.064,-0.0924,-0.5313,-0.071,0.061,-0.1901,0.0002,0.1472,0.2775,0.0749,0,0.0001 -186,-0.041532,-0.586915,-1.35102,-1.48833,2.14266,0.051755,0.187293,0.668899,1.23744,1.51292,1.06999,-2.82096,0.109376,0.176053,0.250066,0.34048,-0.14263,0.390182,-0.249985,-0.099979,0.1456,0.7352,-0.5909,-0.4106,-0.6288,-0.1449,-0.5352,0.35,0.0642,-0.0923,-0.5339,-0.0712,0.0612,-0.1917,0.0003,0.1383,0.282,0.0749,0,0 -187,-0.040074,-0.579595,-1.35695,-1.49269,2.13636,0.050603,0.181785,0.672408,1.23808,1.51199,1.06462,-2.82167,0.109991,0.174122,0.250069,0.341775,-0.139768,0.390931,-0.249984,-0.099979,0.1458,0.732,-0.5928,-0.4351,-0.6295,-0.1152,-0.5508,0.3509,0.0645,-0.0923,-0.5364,-0.0714,0.0615,-0.1931,0.0003,0.1295,0.2862,0.0749,0.0001,0 -188,-0.038618,-0.57231,-1.36289,-1.49728,2.13007,0.049751,0.176126,0.675927,1.23873,1.51107,1.05923,-2.82239,0.110609,0.172175,0.250072,0.342978,-0.136867,0.39168,-0.249984,-0.099978,0.1456,0.7285,-0.5943,-0.4593,-0.6296,-0.0852,-0.5659,0.3519,0.0648,-0.0922,-0.5391,-0.0718,0.0618,-0.1947,0.0003,0.1203,0.2901,0.0749,0,0.0001 -189,-0.03717,-0.565068,-1.36884,-1.50211,2.12378,0.0492,0.170323,0.679457,1.23938,1.51015,1.05381,-2.82311,0.111229,0.170212,0.250075,0.34409,-0.133929,0.392429,-0.249984,-0.099978,0.1448,0.7242,-0.5953,-0.483,-0.6291,-0.0551,-0.5803,0.353,0.0651,-0.0922,-0.5417,-0.0719,0.062,-0.1963,0.0003,0.1112,0.2938,0.0749,0,0 -190,-0.035734,-0.557874,-1.3748,-1.50717,2.1175,0.048953,0.16438,0.682997,1.24004,1.50923,1.04837,-2.82383,0.111852,0.168233,0.250077,0.345109,-0.130958,0.393179,-0.249984,-0.099978,0.1436,0.7194,-0.596,-0.5062,-0.628,-0.0247,-0.5943,0.354,0.0653,-0.0922,-0.5444,-0.0723,0.0623,-0.1979,0.0002,0.1019,0.2971,0.075,0,0 -191,-0.034318,-0.550735,-1.38076,-1.51246,2.11123,0.04901,0.158303,0.686548,1.24069,1.50831,1.0429,-2.82456,0.112477,0.166239,0.25008,0.346035,-0.127956,0.393928,-0.249984,-0.099977,0.1416,0.7139,-0.596,-0.5289,-0.6263,0.0057,-0.6077,0.3551,0.0655,-0.0921,-0.5472,-0.0725,0.0625,-0.1994,0.0003,0.0926,0.3002,0.0749,0,0.0001 -192,-0.032928,-0.543659,-1.38672,-1.51797,2.105,0.049372,0.152097,0.69011,1.24135,1.50738,1.0374,-2.82529,0.113106,0.164228,0.250083,0.346865,-0.124927,0.394676,-0.249983,-0.099977,0.139,0.7076,-0.5956,-0.5511,-0.6238,0.0362,-0.6206,0.3562,0.0659,-0.0922,-0.5499,-0.0727,0.0629,-0.2011,0.0003,0.083,0.3029,0.0748,0.0001,0 -193,-0.03157,-0.536651,-1.39266,-1.5237,2.09879,0.050041,0.145767,0.693683,1.24201,1.50646,1.03187,-2.82602,0.113737,0.1622,0.250085,0.3476,-0.121873,0.395425,-0.249983,-0.099977,0.1358,0.7008,-0.5944,-0.5728,-0.6207,0.0669,-0.633,0.3573,0.0661,-0.0921,-0.5527,-0.0731,0.0631,-0.2028,0.0002,0.0735,0.3054,0.0749,0,0 -194,-0.030255,-0.529718,-1.39859,-1.52964,2.09262,0.051016,0.139319,0.697267,1.24267,1.50554,1.02632,-2.82675,0.114371,0.160156,0.250088,0.348238,-0.118797,0.396174,-0.249983,-0.099976,0.1315,0.6933,-0.5925,-0.5938,-0.6166,0.0975,-0.6448,0.3584,0.0664,-0.0921,-0.5556,-0.0733,0.0634,-0.2044,0.0003,0.0638,0.3076,0.0749,0,0.0001 -195,-0.02899,-0.522868,-1.40449,-1.53578,2.0865,0.052298,0.132757,0.700863,1.24334,1.50462,1.02073,-2.82749,0.115008,0.158095,0.25009,0.348779,-0.115703,0.396923,-0.249982,-0.099976,0.1265,0.685,-0.59,-0.6143,-0.6118,0.1282,-0.6562,0.3596,0.0667,-0.0921,-0.5585,-0.0736,0.0637,-0.2061,0.0002,0.0541,0.3094,0.0749,0.0001,0 -196,-0.027785,-0.516108,-1.41036,-1.54212,2.08044,0.053887,0.126085,0.704471,1.24401,1.5037,1.01512,-2.82823,0.115647,0.156017,0.250092,0.349223,-0.112593,0.397672,-0.249982,-0.099976,0.1205,0.676,-0.5866,-0.6343,-0.6062,0.1589,-0.6672,0.3608,0.0669,-0.0921,-0.5615,-0.074,0.0639,-0.2078,0.0002,0.0444,0.311,0.0749,0,0 -197,-0.02665,-0.509445,-1.41618,-1.54866,2.07445,0.055783,0.119309,0.708091,1.24468,1.50278,1.00947,-2.82897,0.11629,0.153921,0.250094,0.349569,-0.10947,0.398421,-0.249982,-0.099975,0.1135,0.6663,-0.5821,-0.6535,-0.5997,0.1896,-0.6776,0.362,0.0672,-0.0921,-0.5644,-0.0742,0.0643,-0.2096,0.0002,0.0346,0.3123,0.0749,0,0.0001 -198,-0.025597,-0.502888,-1.42195,-1.55538,2.06853,0.057984,0.112431,0.711723,1.24536,1.50186,1.0038,-2.82971,0.116935,0.151808,0.250096,0.349816,-0.106339,0.39917,-0.249982,-0.099975,0.1053,0.6557,-0.5769,-0.6723,-0.5921,0.2201,-0.6878,0.3632,0.0676,-0.0921,-0.5675,-0.0745,0.0645,-0.2113,0.0002,0.0247,0.3131,0.0749,0,0 -199,-0.024639,-0.496444,-1.42765,-1.56228,2.06269,0.06049,0.105457,0.715368,1.24604,1.50094,0.99809,-2.83046,0.117584,0.149677,0.250097,0.349965,-0.1032,0.399918,-0.249981,-0.099975,0.0958,0.6444,-0.5703,-0.6904,-0.5838,0.2506,-0.6974,0.3645,0.0678,-0.0922,-0.5707,-0.0748,0.0649,-0.2131,0.0001,0.0149,0.3139,0.0748,0.0001,0 diff --git a/cmvr-es/devices/speaker/CMakeLists.txt b/cmvr-es/devices/speaker/CMakeLists.txt index f9f6191c..bcff0972 100644 --- a/cmvr-es/devices/speaker/CMakeLists.txt +++ b/cmvr-es/devices/speaker/CMakeLists.txt @@ -1 +1,13 @@ -add_subdirectory(ffmpeg_speaker) \ No newline at end of file +add_subdirectory(ffmpeg_speaker) + +add_library(speaker INTERFACE) + +target_include_directories(speaker INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(speaker + INTERFACE + cmvr_es::device::ffmpeg_speaker + cmvr_es::proto +) + +add_library(cmvr_es::device::speaker ALIAS speaker) diff --git a/cmvr-es/devices/speaker/abstract_speaker.h b/cmvr-es/devices/speaker/abstract_speaker.h index a6737922..bffaccde 100644 --- a/cmvr-es/devices/speaker/abstract_speaker.h +++ b/cmvr-es/devices/speaker/abstract_speaker.h @@ -7,18 +7,18 @@ #pragma once #include "devices/abstract_device.h" -#include "cmvr/config/speaker_config/speaker_conifg.pb.h" +#include "cmvr/config/speaker_config/speaker_config.pb.h" namespace cmvr::device{ class AbstractSpeaker: public AbstractDevice { public: AbstractSpeaker() = default; - explicit AbstractSpeaker(const XmlNode &config): AbstractDevice(config) {} ~AbstractSpeaker() override = default; + DeviceKind kind() const noexcept override { return DeviceKind::Speaker; } virtual void getState(SpeakerState &state) {} virtual void play(const std::string &file_path) {} virtual void setVolume(int volume) {} - [[nodiscard]] virtual int getVolume() const {return 0;} + virtual int getVolume() const {return 0;} virtual void pause() {} virtual void resume() {} @@ -28,4 +28,4 @@ namespace cmvr::device{ }; } -#endif //CMVR_ES_ABSTRACT_SPEAKER_H \ No newline at end of file +#endif //CMVR_ES_ABSTRACT_SPEAKER_H diff --git a/cmvr-es/devices/speaker/ffmpeg_speaker/CMakeLists.txt b/cmvr-es/devices/speaker/ffmpeg_speaker/CMakeLists.txt index 3e2ddac9..7ff77300 100644 --- a/cmvr-es/devices/speaker/ffmpeg_speaker/CMakeLists.txt +++ b/cmvr-es/devices/speaker/ffmpeg_speaker/CMakeLists.txt @@ -4,6 +4,6 @@ target_include_directories(ffmpeg_speaker PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) add_library(cmvr_es::device::ffmpeg_speaker ALIAS ffmpeg_speaker) -target_link_libraries(ffmpeg_speaker PRIVATE -lpulse-simple -lpulse) +target_link_libraries(ffmpeg_speaker PRIVATE -lpulse-simple -lpulse cmvr_es::proto) -install(TARGETS ffmpeg_speaker LIBRARY DESTINATION lib) \ No newline at end of file +install(TARGETS ffmpeg_speaker LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_ptr.h b/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_ptr.h index f3e4d594..da147bf7 100644 --- a/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_ptr.h +++ b/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_ptr.h @@ -25,6 +25,8 @@ extern "C" { #include } +#include "common/base/logging/logger.h" + #include namespace ffmpeg { @@ -120,17 +122,21 @@ public: ImageType imageType = ImageType::COLOR) : width_(width), height_(height), fps_(fps), bitrate_(bitrate), deviceName_(deviceName), imageType_(imageType) { - init(); + initialized_ = init(); } bool encodeFrame(const cv::Mat& image) { + if (!initialized_ || !codec_ctx_ || !frame_) { + CMVR_LOG(ERROR) << "HEVC encoder is not initialized"; + return false; + } if (!prepareFrame(image)) { - std::cerr << "Failed to prepare frame" << std::endl; + CMVR_LOG(ERROR) << "Failed to prepare frame"; return false; } if (avcodec_send_frame(codec_ctx_.get(), frame_.get()) < 0) { - std::cerr << "Error sending frame to encoder" << std::endl; + CMVR_LOG(ERROR) << "Error sending frame to encoder"; return false; } @@ -139,6 +145,10 @@ public: std::vector receivePacket(bool& is_key) { std::vector packetData; + if (!initialized_ || !codec_ctx_) { + CMVR_LOG(ERROR) << "HEVC encoder is not initialized"; + return packetData; + } FFmpegPtr packet(av_packet_alloc()); while (true) @@ -147,7 +157,7 @@ public: if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) { break; } else if (ret < 0) { - std::cerr << "Error during encoding" << std::endl; + CMVR_LOG(ERROR) << "Error during encoding"; break; } is_key = (packet->flags & AV_PKT_FLAG_KEY) != 0; @@ -163,7 +173,7 @@ public: } private: - void init() { + bool init() { // 1. 创建硬件设备上下文 AVBufferRef* hw_device_ctx = nullptr; int ret = av_hwdevice_ctx_create(&hw_device_ctx, AV_HWDEVICE_TYPE_VAAPI, @@ -171,14 +181,16 @@ private: if (ret < 0) { char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0}; av_strerror(ret, errbuf, sizeof(errbuf)); - throw std::runtime_error("Failed to create VAAPI device context: " + std::string(errbuf)); + CMVR_LOG(ERROR) << "Failed to create VAAPI device context: " << errbuf; + return false; } hw_device_ctx_.reset(hw_device_ctx); // 2. 查找编码器 const AVCodec* codec = avcodec_find_encoder_by_name("hevc_vaapi"); if (!codec) { - throw std::runtime_error("HEVC VAAPI encoder not found"); + CMVR_LOG(ERROR) << "HEVC VAAPI encoder not found"; + return false; } // 3. 设置编码器上下文 @@ -194,7 +206,8 @@ private: // 4. 创建硬件帧上下文 hw_frames_ctx_.reset(av_hwframe_ctx_alloc(hw_device_ctx_.get())); if (!hw_frames_ctx_) { - throw std::runtime_error("Failed to allocate hardware frame context"); + CMVR_LOG(ERROR) << "Failed to allocate hardware frame context"; + return false; } AVHWFramesContext* frames_ctx = (AVHWFramesContext*)hw_frames_ctx_->data; @@ -205,14 +218,16 @@ private: frames_ctx->initial_pool_size = 20; if (av_hwframe_ctx_init(hw_frames_ctx_.get()) < 0) { - throw std::runtime_error("Failed to initialize hardware frame context"); + CMVR_LOG(ERROR) << "Failed to initialize hardware frame context"; + return false; } codec_ctx_->hw_frames_ctx = av_buffer_ref(hw_frames_ctx_.get()); // 5. 打开编码器 if (avcodec_open2(codec_ctx_.get(), codec, nullptr) < 0) { - throw std::runtime_error("Cannot open encoder"); + CMVR_LOG(ERROR) << "Cannot open encoder"; + return false; } // 6. 初始化硬件帧 @@ -222,7 +237,8 @@ private: frame_->height = height_; if (av_hwframe_get_buffer(hw_frames_ctx_.get(), frame_.get(), 0) < 0) { - throw std::runtime_error("Could not allocate hardware frame data"); + CMVR_LOG(ERROR) << "Could not allocate hardware frame data"; + return false; } // 7. 初始化软件帧 @@ -232,8 +248,10 @@ private: sw_frame_->format = (imageType_ == ImageType::DEPTH) ? AV_PIX_FMT_GRAY8 : AV_PIX_FMT_NV12; if (av_frame_get_buffer(sw_frame_.get(), 0) < 0) { - throw std::runtime_error("Could not allocate software frame buffer"); + CMVR_LOG(ERROR) << "Could not allocate software frame buffer"; + return false; } + return true; } bool prepareFrame(const cv::Mat& image) { @@ -248,7 +266,7 @@ private: cv::Mat processedDepth; if (depthImage.channels() != 1) { - std::cerr << "Depth image must be single-channel" << std::endl; + CMVR_LOG(ERROR) << "Depth image must be single-channel"; return false; } @@ -256,7 +274,7 @@ private: if (depthImage.depth() == CV_16U) { depthImage.convertTo(processedDepth, CV_8U, 255.0 / 65535.0); } else { - std::cerr << "Unsupported depth image format" << std::endl; + CMVR_LOG(ERROR) << "Unsupported depth image format"; return false; } } else { @@ -265,7 +283,7 @@ private: // 确保图像尺寸匹配 if (processedDepth.cols != width_ || processedDepth.rows != height_) { - std::cerr << "Depth image size mismatch" << std::endl; + CMVR_LOG(ERROR) << "Depth image size mismatch"; return false; } @@ -282,7 +300,7 @@ private: // 上传到硬件帧 if (av_hwframe_transfer_data(frame_.get(), sw_frame_.get(), 0) < 0) { - std::cerr << "Error transferring depth data to hardware frame" << std::endl; + CMVR_LOG(ERROR) << "Error transferring depth data to hardware frame"; return false; } @@ -292,8 +310,8 @@ private: bool prepareColorOrGrayscaleFrame(const cv::Mat& image) { if (image.cols != width_ || image.rows != height_) { - std::cerr << "Image size mismatch: " << image.cols << "x" << image.rows - << " vs " << width_ << "x" << height_ << std::endl; + CMVR_LOG(ERROR) << "Image size mismatch: " << image.cols << "x" << image.rows + << " vs " << width_ << "x" << height_; return false; } @@ -304,7 +322,7 @@ private: } else if (image.channels() == 1) { src_pix_fmt = AV_PIX_FMT_GRAY8; // 灰度图（1通道） } else { - std::cerr << "Unsupported channel count: " << image.channels() << std::endl; + CMVR_LOG(ERROR) << "Unsupported channel count: " << image.channels(); return false; } @@ -316,7 +334,7 @@ private: nullptr, nullptr, nullptr ); if (!sws_ctx) { - std::cerr << "Failed to create sws context" << std::endl; + CMVR_LOG(ERROR) << "Failed to create sws context"; return false; } @@ -348,7 +366,7 @@ private: sw_frame_->linesize // 目标图像每行字节数 ); if (ret <= 0) { - std::cerr << "sws_scale failed, ret=" << ret << std::endl; + CMVR_LOG(ERROR) << "sws_scale failed, ret=" << ret; sws_freeContext(sws_ctx); return false; } @@ -359,7 +377,7 @@ private: // 7. 设置帧属性并上传到硬件 sw_frame_->pts = frame_counter_++; if (av_hwframe_transfer_data(frame_.get(), sw_frame_.get(), 0) < 0) { - std::cerr << "Error transferring data to hardware frame" << std::endl; + CMVR_LOG(ERROR) << "Error transferring data to hardware frame"; return false; } frame_->pts = sw_frame_->pts; @@ -373,6 +391,7 @@ private: int frame_counter_ = 0; std::string deviceName_; ImageType imageType_; + bool initialized_{false}; FFmpegPtr codec_ctx_; FFmpegPtr frame_; @@ -382,4 +401,4 @@ private: }; } // namespace ffmpeg -#endif //FFMPEG_PTR_H \ No newline at end of file +#endif //FFMPEG_PTR_H diff --git a/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h b/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h index 05b1f1bb..998e526f 100644 --- a/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h +++ b/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h @@ -12,7 +12,6 @@ #include #include #include "ffmpeg_ptr.h" -#include "utils/base/include/os.h" namespace cmvr::device { @@ -21,17 +20,16 @@ namespace cmvr::device { class ffmpegSpeaker final : public AbstractSpeaker { public: - explicit ffmpegSpeaker(const XmlNode& cfg); ffmpegSpeaker(const config::FFMpegSpeakerConfig& cfg); ~ffmpegSpeaker() override; - void init() override; - void start() override; - void stop() override; - void updateParams(const std::pair& param) override; + std::string typeName() const override { return "FFMpegSpeaker"; } + bool init() override; + bool start() override; + bool stop() override; void play(const std::string& audio_path) override; void setVolume(int volume) override; - [[nodiscard]] int getVolume() const override; + int getVolume() const override; void pause() override; void resume() override; void getState(SpeakerState& state) override; @@ -43,7 +41,6 @@ namespace cmvr::device { void decode_audio_(); void play_audio_(); - std::string id_; pa_simple* pulse_simple_ = nullptr; // 修改：PulseAudio 简单 API 句柄 pa_sample_spec sample_spec_{}; // 新增：PulseAudio 采样规格 int channels_ = 0; diff --git a/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp b/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp index 585a1277..366c8ea1 100644 --- a/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp +++ b/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp @@ -1,31 +1,15 @@ +#include "common/base/logging/logger.h" // // Created by xtkuang on 2025/6/9. // +#include + #include "../include/ffmpeg_speaker.h" using namespace std; using namespace cmvr::device; -ffmpegSpeaker::ffmpegSpeaker(const XmlNode& cfg) : AbstractSpeaker(cfg) { - state_.is_initialized = false; - state_.is_running = false; - state_.is_paused = false; - try { - id_ = cfg_.getAttrString("id"); - memset(&sample_spec_, 0, sizeof(sample_spec_)); - // alas_ = cfg_.getAttrString("alas"); - // channels_ = std::clamp(cfg_.getAttrDefault("channels", 2), 1, 8); - // sample_rate_ = cfg_.getAttrDefault("sampleRate", 44100); - // soft_resample_ = cfg_.getAttrDefault("softResample", 1); - // latency_ = cfg_.getAttrDefault("latency", 500000); - state_.volume = clamp(cfg_.getAttrDefault("volume", 100), 0, 100); - } - catch (const exception& e) { - LOG(ERROR) << "[ffmpegSpeaker] ([ffmpegSpeaker]): Failed to parse XML: " << e.what(); - } -} - ffmpegSpeaker::ffmpegSpeaker(const config::FFMpegSpeakerConfig& cfg):config_(cfg) { state_.is_initialized = false; @@ -37,7 +21,7 @@ ffmpegSpeaker::ffmpegSpeaker(const config::FFMpegSpeakerConfig& cfg):config_(cfg // state_.volume = config_.volume(); } catch (const exception& e) { - LOG(ERROR) << "[ffmpegSpeaker] ([ffmpegSpeaker]): Failed to parse XML: " << e.what(); + CMVR_LOG(ERROR) << "[ffmpegSpeaker] ([ffmpegSpeaker]): Failed to parse config: " << e.what(); } } @@ -67,7 +51,7 @@ ffmpegSpeaker::~ffmpegSpeaker() { resetPlayState(); } -void ffmpegSpeaker::init() { +bool ffmpegSpeaker::init() { try { lock_guard lock(mtx_); state_.is_running = false; @@ -76,16 +60,18 @@ void ffmpegSpeaker::init() { av_log_set_level(AV_LOG_QUIET); avformat_network_init(); state_.is_initialized = true; - LOG(INFO) << "[ffmpegSpeaker] (init): Success, id=" << id_; + CMVR_LOG(INFO) << "[ffmpegSpeaker] (init): Success, id=" << id_; + return true; } catch (const exception& e) { - LOG(ERROR) <<"[ffmpegSpeaker] (init): Failed, id=" << id_ << ": " << e.what(); - throw runtime_error("[ffmpegSpeaker] (init): Failed, id=" + id_ + ": " + string(e.what())); + CMVR_LOG(ERROR) <<"[ffmpegSpeaker] (init): Failed, id=" << id_ << ": " << e.what(); + state_.is_initialized = false; + return false; } } -void ffmpegSpeaker::start() { - +bool ffmpegSpeaker::start() { + return true; } void ffmpegSpeaker::resetPlayState() @@ -105,10 +91,10 @@ void ffmpegSpeaker::resetPlayState() state_.is_initialized = false; audio_path_.clear(); - LOG(INFO) << "[ffmpegSpeaker] (resetPlayState): Success, id=" << id_; + CMVR_LOG(INFO) << "[ffmpegSpeaker] (resetPlayState): Success, id=" << id_; } -void ffmpegSpeaker::stop() { +bool ffmpegSpeaker::stop() { { lock_guard lock(mtx_); @@ -128,26 +114,26 @@ void ffmpegSpeaker::stop() { } resetPlayState(); + return true; } void ffmpegSpeaker::pause() { try { lock_guard lock(mtx_); if (!state_.is_running || !state_.is_initialized) { - LOG(ERROR) << "[ffmpegSpeaker] (pause): Speaker not running"; - throw runtime_error("[ffmpegSpeaker] (pause): Speaker not running"); + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (pause): Speaker not running"; + return; } if (state_.is_paused) { - LOG(WARNING) << "[ffmpegSpeaker] (pause): Speaker already paused"; - throw runtime_error("[ffmpegSpeaker] (pause): Speaker already paused"); + CMVR_LOG(WARNING) << "[ffmpegSpeaker] (pause): Speaker already paused"; + return; } state_.is_paused = true; - LOG(INFO) << "[ffmpegSpeaker] (pause): Success, id=" << id_; + CMVR_LOG(INFO) << "[ffmpegSpeaker] (pause): Success, id=" << id_; } catch (const exception& e) { - LOG(ERROR) <<"[ffmpegSpeaker] (pause): Failed, id=" << id_ << ": " << e.what(); - throw runtime_error("[ffmpegSpeaker] (pause): Failed, id=" + id_ + ": " + string(e.what())); + CMVR_LOG(ERROR) <<"[ffmpegSpeaker] (pause): Failed, id=" << id_ << ": " << e.what(); } } @@ -155,20 +141,19 @@ void ffmpegSpeaker::resume() { try { lock_guard lock(mtx_); if (!state_.is_running || !state_.is_initialized) { - LOG(ERROR) << "[ffmpegSpeaker] (pause): Speaker not running"; - throw runtime_error("[ffmpegSpeaker] (pause): Speaker not running"); + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (pause): Speaker not running"; + return; } if (!state_.is_paused) { - LOG(WARNING) << "[ffmpegSpeaker] (pause): Speaker is not paused"; - throw runtime_error("[ffmpegSpeaker] (pause): Speaker is not paused"); + CMVR_LOG(WARNING) << "[ffmpegSpeaker] (pause): Speaker is not paused"; + return; } state_.is_paused = false; - LOG(INFO) << "[ffmpegSpeaker] (resume): Success, id=" << id_; + CMVR_LOG(INFO) << "[ffmpegSpeaker] (resume): Success, id=" << id_; } catch (const exception& e) { - LOG(ERROR) <<"[ffmpegSpeaker] (resume): Failed, id=" << id_ << ": " << e.what(); - throw runtime_error("[ffmpegSpeaker] (resume): Failed, id=" + id_ + ": " + string(e.what())); + CMVR_LOG(ERROR) <<"[ffmpegSpeaker] (resume): Failed, id=" << id_ << ": " << e.what(); } } @@ -177,11 +162,12 @@ void ffmpegSpeaker::play(const std::string& audio_path) { try { if (state_.is_running) { - LOG(WARNING) << "[ffmpegSpeaker] (play): Speaker is playing " << audio_path_ << ", id=" << id_; - throw runtime_error("[ffmpegSpeaker] (play): Speaker is playing " + audio_path_); + CMVR_LOG(WARNING) << "[ffmpegSpeaker] (play): Speaker is playing " << audio_path_ << ", id=" << id_; + return; } - if (!pathExists(audio_path)) { - throw std::runtime_error("[ffmpegSpeaker] (play): Audio file not found: " + audio_path); + if (!std::filesystem::exists(audio_path)) { + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (play): Audio file not found: " << audio_path; + return; } // 如果正在播放，先停止 state_.is_running = false; @@ -211,12 +197,14 @@ void ffmpegSpeaker::play(const std::string& audio_path) { // 初始化音频参数 if (!initAudioParams_(audio_path)) { - throw std::runtime_error("[ffmpegSpeaker] Failed to get audio parameters"); + CMVR_LOG(ERROR) << "[ffmpegSpeaker] Failed to get audio parameters"; + return; } // 初始化 PulseAudio 设备 if (!initPulseDevice_()) { - throw std::runtime_error("[ffmpegSpeaker] PulseAudio device initialization failed"); + CMVR_LOG(ERROR) << "[ffmpegSpeaker] PulseAudio device initialization failed"; + return; } // 设置状态 @@ -229,7 +217,7 @@ void ffmpegSpeaker::play(const std::string& audio_path) { try { this->decode_audio_(); } catch (const std::exception& e) { - std::cerr << "[ffmpegSpeaker] Decode thread exception: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] Decode thread exception: " << e.what(); std::lock_guard lock(mtx_); state_.is_decoding = false; } @@ -243,20 +231,20 @@ void ffmpegSpeaker::play(const std::string& audio_path) { try { this->play_audio_(); } catch (const std::exception& e) { - std::cerr << "[ffmpegSpeaker] Play thread exception: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] Play thread exception: " << e.what(); std::lock_guard lock(mtx_); state_.is_running = false; } }); - std::cout << "[ffmpegSpeaker] Started playing: " << audio_path << std::endl; + CMVR_LOG(INFO) << "[ffmpegSpeaker] Started playing: " << audio_path; } catch (const std::exception& e) { + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (play): Failed, id=" << id_ << ": " << e.what(); state_.is_running = false; state_.is_decoding = false; lock.unlock(); resetPlayState(); - throw; } } @@ -286,12 +274,12 @@ void ffmpegSpeaker::decode_audio_() { std::lock_guard lock(mtx_); state_.is_decoding = false; } - std::cerr << "[ffmpegSpeaker] (decode_audio_): audio_path is empty" << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (decode_audio_): audio_path is empty"; return; } - std::cout << "[ffmpegSpeaker] Decoding with: rate=" << sample_rate_ - << ", channels=" << channels_ << std::endl; + CMVR_LOG(INFO) << "[ffmpegSpeaker] Decoding with: rate=" << sample_rate_ + << ", channels=" << channels_; const AVFormatContextPtr fmt_ctx = make_format_context(audio_path_); if (!fmt_ctx) { @@ -299,7 +287,7 @@ void ffmpegSpeaker::decode_audio_() { std::lock_guard lock(mtx_); state_.is_decoding = false; } - std::cerr << "[ffmpegSpeaker] (decode_audio_): Failed to open format context" << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (decode_audio_): Failed to open format context"; return; } @@ -310,7 +298,7 @@ void ffmpegSpeaker::decode_audio_() { std::lock_guard lock(mtx_); state_.is_decoding = false; } - std::cerr << "[ffmpegSpeaker] (decode_audio_): No audio stream found in file" << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (decode_audio_): No audio stream found in file"; return; } @@ -321,7 +309,7 @@ void ffmpegSpeaker::decode_audio_() { std::lock_guard lock(mtx_); state_.is_decoding = false; } - std::cerr << "[ffmpegSpeaker] (decode_audio_): Failed to create codec context" << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (decode_audio_): Failed to create codec context"; return; } @@ -331,7 +319,7 @@ void ffmpegSpeaker::decode_audio_() { std::lock_guard lock(mtx_); state_.is_decoding = false; } - std::cerr << "[ffmpegSpeaker] (decode_audio_): Failed to init swr context" << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (decode_audio_): Failed to init swr context"; return; } @@ -440,7 +428,7 @@ void ffmpegSpeaker::decode_audio_() { } if (!pushed) { - std::cerr << "[ffmpegSpeaker] Failed to push audio frame to queue after 10 attempts" << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] Failed to push audio frame to queue after 10 attempts"; } } } @@ -452,7 +440,7 @@ end_decode: std::lock_guard lock(mtx_); state_.is_decoding = false; } - std::cout << "[ffmpegSpeaker] Decoding finished" << std::endl; + CMVR_LOG(INFO) << "[ffmpegSpeaker] Decoding finished"; } void ffmpegSpeaker::play_audio_() { @@ -501,8 +489,8 @@ void ffmpegSpeaker::play_audio_() { int ret = pa_simple_write(pulse_simple_, frame->data(), data_size, &pa_error); if (ret < 0) { - std::cerr << "[ffmpegSpeaker] (play_audio_): PulseAudio write failed: " - << pa_strerror(pa_error) << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (play_audio_): PulseAudio write failed: " + << pa_strerror(pa_error); // 尝试重新连接 resetPlayState(); @@ -511,7 +499,7 @@ void ffmpegSpeaker::play_audio_() { std::this_thread::sleep_for(std::chrono::milliseconds(100)); if (!initPulseDevice_()) { - std::cerr << "[ffmpegSpeaker] (play_audio_): Failed to reconnect" << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (play_audio_): Failed to reconnect"; { std::lock_guard lock(mtx_); state_.is_running = false; @@ -540,33 +528,14 @@ void ffmpegSpeaker::play_audio_() { state_.is_running = false; } - std::cout << "[ffmpegSpeaker] Playback finished" << std::endl; -} - -void ffmpegSpeaker::updateParams(const std::pair& param) -{ - - try { - { - std::lock_guard lock(mtx_); - if (param.first == "sampleRate") { - sample_rate_ = stoi(param.second); - } - else if (param.first == "channels") { - channels_ = stoi(param.second); - } - } - } - catch (exception &e) { - throw runtime_error(e.what()); - } + CMVR_LOG(INFO) << "[ffmpegSpeaker] Playback finished"; } bool ffmpegSpeaker::initPulseDevice_() { // 验证参数 if (sample_rate_ <= 0 || channels_ <= 0) { - std::cerr << "[ffmpegSpeaker] (initPulseDevice_): Invalid audio parameters: rate=" - << sample_rate_ << ", channels=" << channels_ << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (initPulseDevice_): Invalid audio parameters: rate=" + << sample_rate_ << ", channels=" << channels_; return false; } @@ -577,9 +546,9 @@ bool ffmpegSpeaker::initPulseDevice_() { // 检查采样规格是否有效 if (!pa_sample_spec_valid(&sample_spec_)) { - std::cerr << "[ffmpegSpeaker] (initPulseDevice_): Invalid sample specification: " + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (initPulseDevice_): Invalid sample specification: " << "rate=" << sample_spec_.rate - << ", channels=" << sample_spec_.channels << std::endl; + << ", channels=" << sample_spec_.channels; return false; } @@ -604,14 +573,14 @@ bool ffmpegSpeaker::initPulseDevice_() { ); if (!pulse_simple_) { - std::cerr << "[ffmpegSpeaker] (initPulseDevice_): Failed to create PulseAudio connection: " - << pa_strerror(pa_error) << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] (initPulseDevice_): Failed to create PulseAudio connection: " + << pa_strerror(pa_error); return false; } state_.is_initialized = true; - std::cout << "[ffmpegSpeaker] PulseAudio initialized: rate=" << sample_rate_ - << ", channels=" << channels_ << std::endl; + CMVR_LOG(INFO) << "[ffmpegSpeaker] PulseAudio initialized: rate=" << sample_rate_ + << ", channels=" << channels_; return true; } @@ -620,13 +589,13 @@ bool ffmpegSpeaker::initAudioParams_(const std::string& audio_path) { auto fmt_ctx = make_format_context(audio_path); if (!fmt_ctx) { - std::cerr << "[ffmpegSpeaker] Failed to open audio file: " << audio_path << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] Failed to open audio file: " << audio_path; return false; } int stream_idx = av_find_best_stream(fmt_ctx.get(), AVMEDIA_TYPE_AUDIO, -1, -1, nullptr, 0); if (stream_idx < 0) { - std::cerr << "[ffmpegSpeaker] No audio stream found" << std::endl; + CMVR_LOG(ERROR) << "[ffmpegSpeaker] No audio stream found"; return false; } @@ -639,9 +608,9 @@ bool ffmpegSpeaker::initAudioParams_(const std::string& audio_path) { if (sample_rate_ <= 0) sample_rate_ = 44100; // 检查并打印详细的音频参数 - std::cout << "[ffmpegSpeaker] Audio parameters: rate=" << sample_rate_ + CMVR_LOG(INFO) << "[ffmpegSpeaker] Audio parameters: rate=" << sample_rate_ << ", channels=" << channels_ << ", format=" << av_get_sample_fmt_name((AVSampleFormat)codec_par->format) - << std::endl; + ; return true; } diff --git a/cmvr-es/devices/speaker/speaker_factory.h b/cmvr-es/devices/speaker/speaker_factory.h new file mode 100644 index 00000000..da790c91 --- /dev/null +++ b/cmvr-es/devices/speaker/speaker_factory.h @@ -0,0 +1,60 @@ +#ifndef CMVR_ES_SPEAKER_FACTORY_H +#define CMVR_ES_SPEAKER_FACTORY_H + +#include +#include +#include + +#include "cmvr/config/speaker_config/speaker_config.pb.h" +#include "common/base/logging/logger.h" +#include "devices/speaker/abstract_speaker.h" +#include "devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h" + +namespace cmvr::device { + +class SpeakerFactory { +public: + static std::shared_ptr create(const config::SpeakerDeviceConfig& cfg) + { + if (cfg.id().empty()) { + const std::string error = "[SpeakerFactory]: Speaker id is empty"; + CMVR_LOG(ERROR) << error; + return nullptr; + } + + switch (cfg.backend_case()) { + case config::SpeakerDeviceConfig::kFfmpeg: + return std::make_shared( + backendWithId_(cfg.id(), cfg.ffmpeg())); + + case config::SpeakerDeviceConfig::BACKEND_NOT_SET: + default: + { + const std::string error = + "[SpeakerFactory]: Speaker backend is not configured: " + cfg.id(); + CMVR_LOG(ERROR) << error; + return nullptr; + } + } + } + +private: + static config::FFMpegSpeakerConfig backendWithId_( + const std::string& id, + const config::FFMpegSpeakerConfig& source) + { + if (!source.id().empty() && source.id() != id) { + const std::string error = + "[SpeakerFactory]: Speaker id does not match backend id: " + id; + CMVR_LOG(ERROR) << error; + return {}; + } + auto backend = source; + backend.set_id(id); + return backend; + } +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_SPEAKER_FACTORY_H diff --git a/cmvr-es/devices/state_define.h b/cmvr-es/devices/state_define.h index a0a0c34b..11b60ad7 100644 --- a/cmvr-es/devices/state_define.h +++ b/cmvr-es/devices/state_define.h @@ -12,7 +12,7 @@ #include #include -#include "../utils/math/include/geometry.h" +#include "common/types/geometry_types.h" namespace cmvr::device{ @@ -153,4 +153,4 @@ namespace cmvr::device{ } -#endif //CMVR_ES_STATE_DEFINE_H \ No newline at end of file +#endif //CMVR_ES_STATE_DEFINE_H diff --git a/cmvr-es/hardware/include/esp32_serial_port.h b/cmvr-es/hardware/include/esp32_serial_port.h index 2a891d80..b5569833 100644 --- a/cmvr-es/hardware/include/esp32_serial_port.h +++ b/cmvr-es/hardware/include/esp32_serial_port.h @@ -18,7 +18,7 @@ namespace cmvr bool open(const std::string &port_name,unsigned int baud_rate); void close(); - bool is_open(); + bool is_open() const; bool wakeupESP32(const std::string &port_name); bool send(const std::vector& data); bool sendRawServoData(const std::vector& raw); @@ -30,4 +30,4 @@ namespace cmvr -#endif //SERIAL_PORT_H \ No newline at end of file +#endif //SERIAL_PORT_H diff --git a/cmvr-es/hardware/src/esp32_serial_port.cpp b/cmvr-es/hardware/src/esp32_serial_port.cpp index 2fc4fc1d..eb4b6ad1 100644 --- a/cmvr-es/hardware/src/esp32_serial_port.cpp +++ b/cmvr-es/hardware/src/esp32_serial_port.cpp @@ -7,7 +7,7 @@ #include #include #include -#include +#include "common/base/logging/logger.h" static void printPacket(const std::vector& packet); namespace cmvr { SerialPort::~SerialPort() { @@ -19,13 +19,13 @@ bool SerialPort::open(const std::string& port_name, unsigned int baud_rate) { fd_ = ::open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); if (fd_ < 0) { - LOG(ERROR) << "[SerialPort] open failed: " << strerror(errno); + CMVR_LOG(ERROR) << "[SerialPort] open failed: " << strerror(errno); return false; } struct termios options{}; if (tcgetattr(fd_, &options) < 0) { - LOG(ERROR) << "[SerialPort] tcgetattr failed: " << strerror(errno); + CMVR_LOG(ERROR) << "[SerialPort] tcgetattr failed: " << strerror(errno); ::close(fd_); fd_ = -1; return false; @@ -40,7 +40,7 @@ bool SerialPort::open(const std::string& port_name, unsigned int baud_rate) { options.c_oflag = 0; tcflush(fd_, TCIOFLUSH); if (tcsetattr(fd_, TCSANOW, &options) != 0) { - LOG(ERROR) << "[SerialPort] tcsetattr failed: " << strerror(errno); + CMVR_LOG(ERROR) << "[SerialPort] tcsetattr failed: " << strerror(errno); ::close(fd_); fd_ = -1; return false; @@ -57,10 +57,14 @@ void SerialPort::close() { } } +bool SerialPort::is_open() const { + return fd_ > 0; +} + bool SerialPort::wakeupESP32(const std::string& port_name) { int temp_fd = ::open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); if (temp_fd < 0) { - LOG(ERROR) << "[SerialPort] wakeupESP32 open failed: " << strerror(errno); + CMVR_LOG(ERROR) << "[SerialPort] wakeupESP32 open failed: " << strerror(errno); return false; } @@ -77,7 +81,7 @@ bool SerialPort::wakeupESP32(const std::string& port_name) { int status; if (ioctl(temp_fd, TIOCMGET, &status) < 0) { - LOG(ERROR) << "[SerialPort] wakeupESP32 ioctl get failed: " << strerror(errno); + CMVR_LOG(ERROR) << "[SerialPort] wakeupESP32 ioctl get failed: " << strerror(errno); ::close(temp_fd); return false; } @@ -97,12 +101,12 @@ bool SerialPort::wakeupESP32(const std::string& port_name) { bool SerialPort::send(const std::vector& data) { if (fd_ <= 0) { - LOG(ERROR) << "[SerialPort] send failed: serial not open"; + CMVR_LOG(ERROR) << "[SerialPort] send failed: serial not open"; return false; } ssize_t n = write(fd_, data.data(), data.size()); if (n != (ssize_t)data.size()) { - LOG(ERROR) << "[SerialPort] send incomplete: " << n << "/" << data.size(); + CMVR_LOG(ERROR) << "[SerialPort] send incomplete: " << n << "/" << data.size(); return false; } return true; @@ -110,7 +114,7 @@ bool SerialPort::send(const std::vector& data) { bool SerialPort::sendRawServoData(const std::vector& raw) { if (raw.size() % 5 != 0) { - LOG(ERROR) << "[SerialPort] Invalid servo data length: must be multiple of 4."; + CMVR_LOG(ERROR) << "[SerialPort] Invalid servo data length: must be multiple of 4."; return false; } @@ -119,7 +123,7 @@ bool SerialPort::send(const std::vector& data) { packet.push_back(0xAA); // 帧头 packet.push_back(command_count); // 命令数量 - // LOG(INFO) << "[SerialPort] Command count: " << command_count; + // CMVR_LOG(INFO) << "[SerialPort] Command count: " << command_count; for (size_t i = 0; i < command_count; ++i) { // uint8_t addr = raw[i * 4]; // uint8_t ch = raw[i * 4 + 1]; diff --git a/cmvr-es/hardware/src/serial_interface.cpp b/cmvr-es/hardware/src/serial_interface.cpp index 83ddd841..ad6766b5 100644 --- a/cmvr-es/hardware/src/serial_interface.cpp +++ b/cmvr-es/hardware/src/serial_interface.cpp @@ -3,7 +3,7 @@ // #include "../include/serial_interface.h" -#include +#include "common/base/logging/logger.h" #include #include #include @@ -11,7 +11,6 @@ #include #include #include -#include #include #include @@ -84,7 +83,7 @@ namespace cmvr { } catch (const std::exception& e) { is_initialized = false; - LOG(ERROR) << "[serial_interface] ([serial_interface]): Failed to init serialport: " << e.what(); + CMVR_LOG(ERROR) << "[serial_interface] ([serial_interface]): Failed to init serialport: " << e.what(); } } @@ -136,11 +135,13 @@ namespace cmvr { io.reset(); if (timeout) { - throw std::runtime_error("串口读取超时"); + CMVR_LOG(ERROR) << "[serial_interface] Serial read timeout"; + return {}; } if (ec) { - throw boost::system::system_error(ec); + CMVR_LOG(ERROR) << "[serial_interface] Serial read failed: " << ec.message(); + return {}; } buffer.resize(bytes_transferred); @@ -165,7 +166,7 @@ namespace cmvr { // 写寄存器函数 void write_register(serial_interface& serial, int address_decimal, const std::string& id_value, const std::vector& values_to_write) { if (values_to_write.size() > 6) { - std::cerr << "超过允许的值数量，最多只能写入6个值！" << std::endl; + CMVR_LOG(ERROR) << "超过允许的值数量，最多只能写入6个值！"; return; } @@ -197,11 +198,11 @@ namespace cmvr { send_buffer.push_back(checksum & 0xFF); // 添加校验和 // // 输出发送的数据 - // std::cout << "发送的写入数据: "; + // CMVR_LOG(INFO) << "发送的写入数据: "; // for (auto byte : send_buffer) { - // std::cout << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " "; + // CMVR_LOG(INFO) << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " "; // } - // std::cout << std::endl; + // CMVR_LOG(INFO); // 写入数据 serial.write(send_buffer); @@ -209,9 +210,9 @@ namespace cmvr { // 读取并丢弃写入后的响应 try { std::vector discard_buffer = serial.read(9); - std::cout << "写操作后的响应数据已丢弃，字节数: " << discard_buffer.size() << std::endl; + CMVR_LOG(INFO) << "写操作后的响应数据已丢弃，字节数: " << discard_buffer.size(); } catch (const std::exception& e) { - std::cerr << "写操作后的响应数据读取失败: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "写操作后的响应数据读取失败: " << e.what(); } } else if (serial.getCommunicationMode() == CommunicationMode::CAN) { @@ -258,11 +259,11 @@ namespace cmvr { send_buffer.push_back(0x55); // 打印并发送 - // std::cout << "发送的完整数据: "; + // CMVR_LOG(INFO) << "发送的完整数据: "; // for (auto byte : send_buffer) { - // std::cout << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " "; + // CMVR_LOG(INFO) << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " "; // } - // std::cout << std::endl; + // CMVR_LOG(INFO); serial.write(send_buffer); @@ -270,7 +271,7 @@ namespace cmvr { try { std::vector discard_buffer = serial.read(32); } catch (const std::exception& e) { - std::cerr << "写操作后的响应数据读取失败: " << e.what() << std::endl; + CMVR_LOG(ERROR) << "写操作后的响应数据读取失败: " << e.what(); } }; @@ -308,11 +309,11 @@ namespace cmvr { send_buffer.push_back(checksum & 0xFF); - // std::cout << "发送的完整数据: "; + // CMVR_LOG(INFO) << "发送的完整数据: "; // for (auto byte : send_buffer) { - // std::cout << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " "; + // CMVR_LOG(INFO) << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " "; // } - // std::cout << std::endl; + // CMVR_LOG(INFO); serial.write(send_buffer); // 写入串口 @@ -320,11 +321,11 @@ namespace cmvr { std::vector received_data = serial.read(length_to_read); // // 打印接收到的原始响应数据 - // std::cout << "接收到的原始响应数据: "; + // CMVR_LOG(INFO) << "接收到的原始响应数据: "; // for (auto byte : received_data) { - // std::cout << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " "; + // CMVR_LOG(INFO) << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " "; // } - // std::cout << std::endl; + // CMVR_LOG(INFO); return received_data; } @@ -509,4 +510,4 @@ namespace cmvr { } return faults; } -} \ No newline at end of file +} diff --git a/cmvr-es/ik_solver/include/bias_srs_ik_slover.h b/cmvr-es/ik_solver/include/bias_srs_ik_slover.h deleted file mode 100644 index 74f8d762..00000000 --- a/cmvr-es/ik_solver/include/bias_srs_ik_slover.h +++ /dev/null @@ -1,94 +0,0 @@ -// -// Created by lgv on 2025/11/3. -// - -#ifndef CMVR_ES_SRS_IK_SLOVER_H -#define CMVR_ES_SRS_IK_SLOVER_H - - -#include - -namespace cmvr { - - class BiasSRSIkSolver { - public: - enum ConfigDirection { - OUTWARD = 1, // 向外 - INWARD = -1 // 向内 - }; - - public: - BiasSRSIkSolver(); - ~BiasSRSIkSolver(){}; - - // std::vector inverse_kinematics(const Eigen::MatrixXd& pose, double psi); - bool inverse_kinematics(const Eigen::MatrixXd& pose, std::vector &joints,double psi); - - Eigen::Matrix4d calc_total_transform(const std::vector& joint_angles); - - bool cal_coefficient_matrix(const Eigen::MatrixXd& pose,Eigen::MatrixXd& s_mat , Eigen::MatrixXd& w_mat); - - void set_shoulder_config(ConfigDirection value) { - shoulder_config_ = value; - } - - void set_elbow_config(ConfigDirection value) { - elbow_config_ = value; - } - - void set_wrist_config(ConfigDirection value) { - wrist_config_ = value; - } - - - - int get_shoulder_config() { - return static_cast(shoulder_config_); - } - - int get_elbow_config() { - return static_cast(elbow_config_); - } - - int get_wrist_config() { - return static_cast(wrist_config_); - } - - std::vector> get_joints_limits() { - return joints_limits_; - } - - - - private: - - - ConfigDirection shoulder_config_{OUTWARD}; - ConfigDirection elbow_config_{OUTWARD}; - ConfigDirection wrist_config_{OUTWARD}; - // DH参数 - Eigen::VectorXd link_lengths_; - Eigen::MatrixXd dh_params_; - - double d_bs_, d_se_, d_ew_, d_wt_; - - // 关节物理限位 - // first : min second : max - std::vector> joints_limits_{}; - - - // 计算参考平面相对于基坐标系的旋转矩阵 - Eigen::Matrix3d reference_plane(const Eigen::Vector3d& S, const Eigen::Vector3d& W); - - // 罗德里格斯公式 - Eigen::Matrix3d calc_rotation_matrix(const Eigen::Vector3d& rotation_axis, double rotation_angle); - - - // 使用 DH 参数计算变换矩阵 - Eigen::Matrix4d calc_dh(double d, double alpha, double a, double theta); - - }; -} - - -#endif //CMVR_ES_SRS_IK_SLOVER_H \ No newline at end of file diff --git a/cmvr-es/ik_solver/include/ik_solver_creator.h b/cmvr-es/ik_solver/include/ik_solver_creator.h deleted file mode 100644 index 9111cd39..00000000 --- a/cmvr-es/ik_solver/include/ik_solver_creator.h +++ /dev/null @@ -1,36 +0,0 @@ -// -// Created by lgv on 11/7/25. -// - -#pragma once - -#include -#include -#include "ik_solver/include/ik_solver.h" - -namespace cmvr -{ - enum IkSolverType - { - IK_SOLVER_UNKNOWN = 0, - - // Limit-Aware Wrist-Bias Analytical IK - LAWBA_IK = 1, - PINOCCHIO_QP = 2, - PINOCCHIO_DLS = 3 - }; - class IkSolverCreator - { - protected: - static std::unordered_map> - registers_; - static std::shared_ptr createNew(const IkSolverType type); - - public: - static void clear() - { - registers_.clear(); - } - static std::shared_ptr create(const IkSolverType type); - }; -} diff --git a/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h b/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h deleted file mode 100644 index 145ca0b5..00000000 --- a/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h +++ /dev/null @@ -1,316 +0,0 @@ -#pragma once - -#include "ik_solver/include/pinocchio_ik_base.h" - - -#include -#include -#include -#include -#include - -#include "planner/cartesian_space_planner/include/cartesian_twist_limiter.h" - -namespace cmvr { - -/** - * @brief 基于 Pinocchio 的 DLS 逆运动学求解器。 - * - * 提供以下能力： - * - 位姿 IK（`ik`） - * - 正运动学 FK（`fk`，继承自基类） - * - 末端 twist 到关节速度的微分 IK（`ik` 重载） - * - 直线 MoveL（S 曲线标量规划 + 局部系微分 IK） - * - speedL（基于 CartesianTwistLimiter + DLS 微分 IK） - * - * 求解链路由 URDF 中 `base_frame_name -> flange_frame_name` 自动提取， - * 并支持可选 TCP frame。 - */ -class PinocchioDlsIKSolver : public PinocchioIKBase { -public: - struct SpeedLConfig { - double linear_velocity_max{0.25}; - double linear_acceleration_max{0.8}; - double linear_jerk_max{4.0}; - - double angular_velocity_max{1.0}; - double angular_acceleration_max{3.0}; - double angular_jerk_max{15.0}; - - std::vector joint_velocity_max = std::vector(7, 2.5); ///< size=chain_v_dof_，为空则仅用 URDF limit - std::vector joint_acceleration_max; ///< size=chain_v_dof_，为空则不做 joint accel 限制 - bool enable_joint_soft_limit_velocity{true}; - double joint_soft_limit_margin{0.05}; - double joint_hard_limit_margin{0.03}; - - double linear_target_replan_threshold{1e-4}; - double angular_target_replan_threshold{1e-4}; - - double linear_reverse_cos_threshold{-0.8660254037844386}; // cos(150deg) - double linear_reverse_switch_speed_threshold{1e-3}; - }; - - PinocchioDlsIKSolver(const std::string &urdf_path, - const std::string &base_frame_name, - const std::string &flange_frame_name, - const std::string &tcp_frame_name = std::string(), - int max_iters = 100, - double pos_eps = 1e-6, - double rot_eps = 1e-6, - double damping = 1e-4); - - ~PinocchioDlsIKSolver() override = default; - - bool init() override; - - bool ik(const Eigen::Matrix4d &target_pose, - std::vector &joints_angle, - bool is_tcp = true) override; - - bool ik(const std::string& base_link, - const std::string& ee_link, - const Eigen::Matrix4d& target_pose, - std::vector& joints_angle); - - bool moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_input, - const std::vector& q_start, - std::vector>& q_traj, - std::vector& t_traj, - double dt, - double v_tcp_max, - double a_tcp_max, - double j_tcp_max, - const std::vector& qd_max, // empty => use URDF joint velocity limits only - bool is_tcp = true, - CartesianFrame input_frame = CartesianFrame::Base, - std::vector>* qdot_traj = nullptr); - - bool ik(const std::string& base_link, - const std::string& ee_link, - const Eigen::Matrix& target_vel, - std::vector& joints_vel, - double damping = -1.0, - double qdot_abs_max = std::numeric_limits::infinity()); - - /** - * @brief 对靠近关节位置限位且仍继续向外运动的关节速度做软压缩。 - * - * 该接口不改变主任务的求解方式，只在得到关节速度命令后做逐轴后处理。 - * 软/硬边界参数来自当前 speedL 配置中的： - * - `joint_soft_limit_margin` - * - `joint_hard_limit_margin` - * - `enable_joint_soft_limit_velocity` - * - * @param q_chain 当前链关节位置，size=chain_dof_，单位 rad。 - * @param qdot_des 待后处理的关节速度命令，size=chain_v_dof_，单位 rad/s。 - * @return 经过软限位速度压缩后的关节速度命令。 - */ - Eigen::VectorXd applyJointSoftLimitVelocity(const Eigen::VectorXd& q_chain, - const Eigen::VectorXd& qdot_des); - - void setJointLimitAvoidance(bool enable, - double gain = 0.2, - double margin_ratio = 0.15, - double max_push = 0.25); - - void setMaxIters(int iters) { max_iters_ = iters; } - void setDamping(double d) { damping_ = d; } - void setEps(double pos_eps, double rot_eps) { pos_eps_ = pos_eps; rot_eps_ = rot_eps; } - - // ==================== speedL ==================== - - /** - * @brief 配置 speedL task-space / joint-space 限幅参数。 - * - * 仅更新 speedL 配置，不主动重置当前运行态。 - * - * 使用注意： - * 1) 运行中再次调用本接口时，当前 target / planner state / 上一拍 qdot 缓存会被保留。 - * 2) 如果你需要“重新配置并从静止状态重新开始”，请先调用 resetSpeedL()，再调用 configureSpeedL()。 - */ - bool configureSpeedL(const SpeedLConfig& config); - - /** - * @brief 重置 speedL 运行状态（不清除配置）。 - */ - void resetSpeedL(); - - /** - * @brief speedL 单步（推荐：真机/仿真闭环版本） - * - * 输入为测得关节状态与目标末端 twist，输出关节速度命令。 - * 使用注意： - * 1) 这是 speedL 的首选接口，真机和 MuJoCo 闭环都应优先使用这个版本。 - * 2) q_measured / qdot_measured 必须与本次控制周期对齐；如果时间戳漂移，task-space limiter - * 的 synchronize 会基于错误状态工作。 - * 3) 返回的 qdot_cmd 仍然是 solver 侧命令值，不等于驱动器最终实际执行速度。 - * - * 流程： - * 1) 用 q_measured / qdot_measured 计算当前 measured twist(base) - * 2) 同步到 CartesianTwistLimiter - * 3) task-space 限幅得到 command twist(base) - * 4) 用 DLS 微分 IK 解出 qdot - * 5) 做 joint velocity / joint acceleration 限幅 - * 6) 回算 executed twist(base) - * - * @param target_twist 目标末端 twist（Base 或 Tool 系） - * @param dt 控制周期 - * @param q_measured 当前测得关节位置，size=chain_dof_ - * @param qdot_measured 当前测得关节速度，size=chain_v_dof_ - * @param qdot_cmd 输出关节速度命令，size=chain_v_dof_ - * @param input_frame target_twist 的输入坐标系 - * @param is_tcp true: TCP；false: flange - */ - bool speedLStep(const Eigen::Matrix& target_twist, - double dt, - const std::vector& q_measured, - const std::vector& qdot_measured, - std::vector& qdot_cmd, - CartesianFrame input_frame = CartesianFrame::Tool, - bool is_tcp = true); - - /** - * @brief speedL 单步（半闭环版本：有 q，无 qd） - * - * 使用测得 q_measured 更新当前姿态， - * 并用上一拍命令速度作为 qdot_measured 的近似反馈。 - * - * 使用注意： - * 1) 只有在能拿到真实 q、但暂时拿不到真实 qd 时再用这个版本。 - * 2) 这里的 qdot_measured 是 speedl_prev_qdot_cmd_ 的近似值；下游存在饱和、延迟或丢包时， - * 这个近似会偏乐观。 - * 3) 为避免把伪造的 qdot_measured 再同步回 task-space planner，这个版本不会执行 - * CartesianTwistLimiter::synchronize(...)。 - * 4) 一旦能拿到真实 qdot，应切回带 q_measured/qdot_measured 的闭环版本。 - */ - bool speedLStep(const Eigen::Matrix& target_twist, - double dt, - const std::vector& q_measured, - std::vector& qdot_cmd, - CartesianFrame input_frame = CartesianFrame::Tool, - bool is_tcp = true); - - /** - * @brief speedL 单步（开环退化版本：无 q，无 qd） - * - * 使用内部 cur_joints_angle_ 与上一拍命令速度作为近似反馈， - * 并在每拍后用本次 qdot_cmd 做一次内部姿态推进。 - * - * 使用注意： - * 1) 只有在当前周期既拿不到真实 q、也拿不到真实 qd 时，才应使用这个版本。 - * 2) 该版本依赖内部预测状态，会随时间累计漂移；不适合长时间连续运行。 - * 3) 仅当 chain_dof_ == chain_v_dof_ 时才允许使用，因为内部使用简单的 q += qdot * dt - * 欧拉推进。 - * 4) 该版本同样不会执行 CartesianTwistLimiter::synchronize(...)。 - * 5) 一旦恢复真实 q 或 qd，请立即切回上面两个 overload。 - */ - bool speedLStep(const Eigen::Matrix& target_twist, - double dt, - std::vector& qdot_cmd, - CartesianFrame input_frame = CartesianFrame::Tool, - bool is_tcp = true); - - const Eigen::Matrix& getSpeedLMeasuredTwistBase() const { - return speedl_twist_measured_base_; - } - - const Eigen::Matrix& getSpeedLCommandTwistBase() const { - return speedl_twist_command_base_; - } - - const Eigen::Matrix& getSpeedLExecutedTwistBase() const { - return speedl_twist_executed_base_; - } - - const std::vector& getSpeedLPrevQdotCmd() const { - return speedl_prev_qdot_cmd_; - } - - bool isSpeedLConfigured() const { return speedl_configured_; } - -private: - Eigen::MatrixXd dampedPseudoInverse(const Eigen::MatrixXd &J, double lambda); - - bool buildFullQFromChain(const Eigen::VectorXd& q_chain, - Eigen::VectorXd& q_full, - const char* context) const; - - Eigen::MatrixXd extractChainJacobian(const Eigen::Matrix& J_full) const; - - const pinocchio::SE3& getBasePoseWorld() const; - - Eigen::VectorXd computeJointLimitAvoidanceVelocity(const Eigen::VectorXd& q_chain) const; - - Eigen::VectorXd projectToNullspace(const Eigen::MatrixXd& J_pinv, - const Eigen::MatrixXd& J, - const Eigen::VectorXd& secondary) const; - - // ==================== speedL helpers ==================== - bool resolveSpeedLEeFrame(bool is_tcp, pinocchio::FrameIndex& ee_id) const; - - bool buildJacobianBaseAtQ(const std::vector& q_chain, - pinocchio::FrameIndex ee_id, - Eigen::MatrixXd& J_base, - Eigen::Matrix3d& base_R_ee, - Eigen::VectorXd* q_full_out = nullptr); - - bool computeMeasuredTwistBase(const std::vector& q_chain, - const std::vector& qdot_chain, - pinocchio::FrameIndex ee_id, - Eigen::Matrix& twist_base, - Eigen::MatrixXd* J_base_out = nullptr, - Eigen::Matrix3d* base_R_ee_out = nullptr, - Eigen::VectorXd* q_full_out = nullptr); - - Eigen::VectorXd applyJointVelocityLimits(const Eigen::VectorXd& qdot_des) const; - - Eigen::VectorXd applyJointAccelerationLimits(const Eigen::VectorXd& qdot_des, - const Eigen::VectorXd& qdot_reference, - double dt) const; - - bool speedLStepImpl(const Eigen::Matrix& target_twist, - double dt, - const std::vector& q_measured, - const std::vector& qdot_measured, - std::vector& qdot_cmd, - CartesianFrame input_frame, - bool is_tcp, - bool enable_twist_sync); - -private: - int chain_q_start_{0}; - int chain_dof_{0}; - int chain_v_start_{0}; - int chain_v_dof_{0}; - - bool limit_avoidance_enabled_{false}; - double limit_avoidance_gain_{0.2}; - double limit_avoidance_margin_ratio_{0.15}; - double limit_avoidance_max_push_{0.25}; - - bool base_pose_cached_{false}; - pinocchio::SE3 oM_base_cached_; - - bool initialized_{false}; - - int max_iters_; - double pos_eps_; - double rot_eps_; - double damping_; - - // ==================== speedL state ==================== - bool speedl_configured_{false}; - bool speedl_last_is_tcp_{true}; - - SpeedLConfig speedl_config_; - CartesianTwistLimiter speedl_twist_limiter_; - - std::vector speedl_prev_qdot_cmd_; ///< size=chain_v_dof_ - - Eigen::Matrix speedl_twist_measured_base_{Eigen::Matrix::Zero()}; - Eigen::Matrix speedl_twist_command_base_{Eigen::Matrix::Zero()}; - Eigen::Matrix speedl_twist_executed_base_{Eigen::Matrix::Zero()}; - bool speedl_joint_soft_limit_active_{false}; -}; - -} // namespace cmvr diff --git a/cmvr-es/ik_solver/src/bias_srs_ik_slover.cpp b/cmvr-es/ik_solver/src/bias_srs_ik_slover.cpp deleted file mode 100644 index 40a40fd2..00000000 --- a/cmvr-es/ik_solver/src/bias_srs_ik_slover.cpp +++ /dev/null @@ -1,360 +0,0 @@ -// -// Created by lgv on 2025/11/3. -// - -#include "ik_solver/include/bias_srs_ik_slover.h" -#include -#include "common/math/include/support_functions.h" -#include "common/consts/constant.h" -using namespace cmvr; -using namespace Eigen; - -BiasSRSIkSolver::BiasSRSIkSolver() { - link_lengths_ = Eigen::VectorXd(4); - link_lengths_ << 0.0945 + 0.0765, 0.1475 + 0.1025, 0.0965 + 0.1525, 0.03; - - // DH 参数 [d_i, alpha_i, a_i(=0), theta_offset_i] - dh_params_ = Eigen::MatrixXd(7, 4); - double half_pi = M_PI / 2; - dh_params_ << link_lengths_[0], -half_pi, 0.0, 0.0, - 0.0, half_pi, 0.0, 0.0, - link_lengths_[1], -half_pi, 0.0, 0.0, - 0.0, half_pi, 0.0, 0.0, - link_lengths_[2], -half_pi, 0.0, half_pi, - 0.0, half_pi, 0.0, half_pi, - link_lengths_[3], 0.0, 0.0, 0.0; - - d_bs_ = link_lengths_[0]; - d_se_ = link_lengths_[1]; - d_ew_ = link_lengths_[2]; - d_wt_ = link_lengths_[3]; - - - - - joints_limits_ = { - {-M_PI, M_PI}, - {-0.78, 1.57}, - {-M_PI, M_PI}, - {0, 2.05}, - {-M_PI, M_PI}, - {-0.78, 0.78}, - {-0.26, 1.57}, - }; - -} - -Eigen::Matrix3d BiasSRSIkSolver::reference_plane(const Eigen::Vector3d &S, const Eigen::Vector3d &W) { - double d_sw = (W - S).norm(); - Eigen::Vector3d v_sw = (W - S).normalized(); - - // 1： 计算E_r 的值 - double x = (d_sw * d_sw + d_se_ * d_se_ - d_ew_ * d_ew_) / (2 * d_sw); - double r = sqrt(std::max(d_se_ * d_se_ - x * x, 0.0)); - Eigen::Vector3d F = S + x * v_sw; - - Vector3d FE; - if (v_sw.head<2>().cwiseAbs().maxCoeff() <= 1e-6) { - FE = Vector3d(-1.0, 0.0, 0.0); - } else { - FE(0) = -v_sw(0) * v_sw(2) / (v_sw(0) * v_sw(0) + v_sw(1) * v_sw(1)); - FE(1) = -v_sw(1) * v_sw(2) / (v_sw(0) * v_sw(0) + v_sw(1) * v_sw(1)); - FE(2) = 1.0; - } - - // 根据 elbow_config 调整 E 的位置 - Vector3d E = F + elbow_config_ * r * FE.normalized(); - - Vector3d v_es = (S - E).normalized(); - Vector3d v_ew = (W - E).normalized(); - - - const Vector3d R30_y = v_es; - Vector3d R30_z = v_ew.cross(v_es); - - - if (elbow_config_ == INWARD) - R30_z = -R30_z; - - auto nz = R30_z.norm(); - if (nz > 1e-12) { - R30_z /= nz; // 或者：R30_z.normalize(); - } else { - // 退化保护：给个正交补方向 - R30_z = Vector3d(-0.0, 1.0, 0.0); - } - - - Vector3d R30_x = R30_y.cross(R30_z); - - - Matrix3d R30; - R30.col(0) = R30_x; - R30.col(1) = R30_y; - R30.col(2) = R30_z; - - return R30; -} -bool BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &pose, std::vector &joints, double psi) { - - joints.resize(7,0); - - // 目标位置 - Eigen::Vector3d P_target = pose.block<3, 1>(0, 3); - - // 肩部 S 位置 ，相对于基坐标系 - Eigen::Vector3d S(0.0, 0.0, d_bs_); - - // P67 ： tcp 相对于 {6} 坐标系（腕心 W）的位置 - Eigen::Vector3d P67(0.0, 0.0, d_wt_); - - // 腕心 W 位置 ，相对于基坐标系 - Eigen::Vector3d W = P_target - pose.block<3, 3>(0, 0) * P67; - - - double d_sw = (W - S).norm(); - const double r_max = d_se_ + d_ew_; // 最远：完全伸直 - const double r_min = std::abs(d_se_ - d_ew_); // 最近：完全折叠 - - // 与上下界的偏差 - double diff_max = d_sw - r_max; // >0 表示比 r_max 还远 - double diff_min = r_min - d_sw; // >0 表示比 r_min 还近 - - // 明显超过可达范围才判定为不可达： - // - diff_max > EPS → 太远 - // - diff_min > EPS → 太近 - if (diff_max > EPS || diff_min > EPS) { - std::cerr << "Pose outside reachable workspace, IK solve failed"; - return false; - } - - // 计算肘部角度 关节3 - double cos_elbow = (d_se_ * d_se_ + d_ew_ * d_ew_ - d_sw * d_sw) / (2 * d_se_ * d_ew_); - cos_elbow = std::clamp(cos_elbow, -1.0, 1.0); - joints[3] = elbow_config_ * (M_PI - std::acos(cos_elbow)); - - // 计算臂平面E处相对于 基坐标系 的旋转矩阵 - Eigen::Matrix3d R30 = reference_plane(S, W); - Eigen::Matrix3d R_axis = calc_rotation_matrix((W - S).normalized(), psi); - Eigen::Matrix3d R3 = R_axis * R30; - - - // 计算肩部角度 (关节0, 1, 2) - // joints[0] = std::atan2(-R3(1, 1) * shoulder_config_, -R3(0, 1) * shoulder_config_); - // joints[1] = std::acos(std::clamp(-R3(2, 1), -1.0, 1.0)) * shoulder_config_; - // joints[2] = std::atan2(R3(2, 2) * shoulder_config_, -R3(2, 0) * shoulder_config_); - - double k = shoulder_config_; - - // c2 = np.clip(-R3[2, 1], -1.0, 1.0) - double c2 = std::clamp(-R3(2, 1), -1.0, 1.0); // cos(theta2) - const double eps = 1e-8; - - if (std::fabs(c2 - 1.0) < eps) { - // ===== theta2 ≈ 0 奇异 ===== - joints[1] = 0.0 * k; - joints[0] = 0.0; - joints[2] = std::atan2(k * R3(1, 0), k * R3(0, 0)); - } else if (std::fabs(c2 + 1.0) < eps) { - // ===== theta2 ≈ π 或 -π 奇异 ===== - joints[1] = k * M_PI; // - joints[2] = std::atan2(k * R3(1, 0), k * R3(1, 2)); - joints[0] = 0.0; - } else { - // ===== 一般情况 ===== - joints[0] = std::atan2(-k * R3(1, 1), -k * R3(0, 1)); - joints[1] = k * std::acos(c2); - joints[2] = std::atan2(k * R3(2, 2), -k * R3(2, 0)); - } - - - // 计算 R04 - Eigen::Matrix3d R04 = Eigen::Matrix3d::Identity(); - for (int i = 0; i < 4; ++i) { - // 从 DH 参数中获取参数 - Eigen::Vector4d dh = dh_params_.row(i); - R04 = R04 * calc_dh(dh[0], dh[1], dh[2], dh[3] + joints[i]).block<3, 3>(0, 0); - } - - - Eigen::Matrix3d R47 = R04.transpose() * pose.block<3, 3>(0, 0); - - - k = wrist_config_; // +1 / -1 - - // ===== 1. 通用 ZYZ 提取===== - double c = std::clamp(R47(2, 2), -1.0, 1.0); // clip - double theta_y = k * std::acos(c); - // 另一种写法（注释里那行）： - // double theta_y = std::atan2(k * std::hypot(R47(2,0), R47(2,1)), R47(2,2)); - - double phi_z = std::atan2(k * R47(1, 2), k * R47(0, 2)); - double psi_z = std::atan2(k * R47(2, 1), -k * R47(2, 0)); - - // ===== 2. 奇异退化处理 ===== - if (std::fabs(c - 1.0) < eps) - { - // theta ≈ 0 - theta_y = 0.0; - phi_z = std::atan2(k * R47(1, 0), k * R47(0, 0)); - psi_z = 0.0; - } - else if (std::fabs(c + 1.0) < eps) - { - // theta ≈ ±π - theta_y = k * M_PI; - phi_z = std::atan2(-k * R47(1, 0), -k * R47(0, 0)); - psi_z = 0.0; - } - - // 调整角度 - joints[4] = SupportFunctions::normalize_angle(phi_z - M_PI / 2); - joints[5] = SupportFunctions::normalize_angle(theta_y - M_PI / 2); - joints[6] = SupportFunctions::normalize_angle(psi_z); - - return true; - -} - - -Eigen::Matrix3d BiasSRSIkSolver::calc_rotation_matrix(const Eigen::Vector3d &rotation_axis, double rotation_angle) { - // 归一化旋转轴 - Eigen::Vector3d normalized_axis = rotation_axis.normalized(); - - // 构造反对称矩阵 - double ux = normalized_axis[0], uy = normalized_axis[1], uz = normalized_axis[2]; - Eigen::Matrix3d u_hat; - u_hat << 0, -uz, uy, - uz, 0, -ux, - -uy, ux, 0; - - // 计算旋转矩阵 - Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity() + std::sin(rotation_angle) * u_hat + ( - 1 - std::cos(rotation_angle)) * (u_hat * u_hat); - - return rotation_matrix; -} - - - - - -Eigen::Matrix4d BiasSRSIkSolver::calc_dh(double d, double alpha, double a, double theta) { - double ca = std::cos(alpha); - double sa = std::sin(alpha); - double ct = std::cos(theta); - double st = std::sin(theta); - - // 构建 DH 变换矩阵 - Eigen::Matrix4d T; - T << ct, -st * ca, st * sa, a * ct, - st, ct * ca, -ct * sa, a * st, - 0.0, sa, ca, d, - 0.0, 0.0, 0.0, 1.0; - - return T; -} - -Eigen::Matrix4d BiasSRSIkSolver::calc_total_transform(const std::vector &joint_angles) { - Eigen::Matrix4d T_total = Eigen::Matrix4d::Identity(); // 初始化为单位矩阵 - - // 遍历每一组 DH 参数 - for (size_t i = 0; i < dh_params_.rows(); ++i) { - // 获取当前关节的 DH 参数 - double d = dh_params_(i, 0); - double alpha = dh_params_(i, 1); - double a = dh_params_(i, 2); - double theta0 = dh_params_(i, 3); - - // 当前关节的实际旋转角度 theta - double theta = theta0 + joint_angles[i]; - - // 计算当前关节的变换矩阵 - Eigen::Matrix4d T = calc_dh(d, alpha, a, theta); - - // 更新总变换矩阵 - T_total = T_total * T; - } - - return T_total; -} - -bool BiasSRSIkSolver::cal_coefficient_matrix(const Eigen::MatrixXd &pose, Eigen::MatrixXd &s_mat, - Eigen::MatrixXd &w_mat) { - if (s_mat.rows() != 3 || s_mat.cols() != 9) s_mat.setZero(3, 9); - if (w_mat.rows() != 3 || w_mat.cols() != 9) w_mat.setZero(3, 9); - - - // Eigen::VectorXd joints(7); - std::vector joints(7, 0); - - // 目标位置 - Eigen::Vector3d P_target = pose.block<3, 1>(0, 3); - - // 肩部 S 位置 ，相对于基坐标系 - Eigen::Vector3d S(0.0, 0.0, d_bs_); - - // P67 ： tcp 相对于 {6} 坐标系（腕心 W）的位置 - Eigen::Vector3d P67(0.0, 0.0, d_wt_); - - // 腕心 W 位置 ，相对于基坐标系 - Eigen::Vector3d W = P_target - pose.block<3, 3>(0, 0) * P67; - - - double d_sw = (W - S).norm(); - const double r_max = d_se_ + d_ew_; // 最远：完全伸直 - const double r_min = std::abs(d_se_ - d_ew_); // 最近：完全折叠 - - // 与上下界的偏差 - double diff_max = d_sw - r_max; // >0 表示比 r_max 还远 - double diff_min = r_min - d_sw; // >0 表示比 r_min 还近 - - // 明显超过可达范围才判定为不可达： - // - diff_max > EPS → 太远 - // - diff_min > EPS → 太近 - if (diff_max > EPS || diff_min > EPS) { - std::cerr << "Pose outside reachable workspace, IK solve failed" << std::endl; - return false; - } - - // 计算肘部角度 关节3 - double cos_elbow = (d_se_ * d_se_ + d_ew_ * d_ew_ - d_sw * d_sw) / (2 * d_se_ * d_ew_); - cos_elbow = std::clamp(cos_elbow, -1.0, 1.0); - joints[3] = elbow_config_ * (M_PI - std::acos(cos_elbow)); - - - // 计算臂平面E处相对于 基坐标系 的旋转矩阵 - Eigen::Matrix3d R30 = reference_plane(S, W); - Eigen::Vector3d normalized_axis = (W - S).normalized(); - - // 构造反对称矩阵 - double ux = normalized_axis[0], uy = normalized_axis[1], uz = normalized_axis[2]; - Eigen::Matrix3d u_hat; - u_hat << 0, -uz, uy, - uz, 0, -ux, - -uy, ux, 0; - - // 计算旋转矩阵 R03 - Eigen::MatrixXd A_s = u_hat * R30; - Eigen::MatrixXd B_s = -u_hat * u_hat * R30; - Eigen::MatrixXd C_s = (Eigen::MatrixXd::Identity(3, 3) + u_hat * u_hat) * R30; - - - // 计算变换矩阵 - Eigen::MatrixXd T34 = calc_dh(dh_params_(3, 0), dh_params_(3, 1), dh_params_(3, 2), - dh_params_(3, 3) + joints[3]); - Eigen::MatrixXd R34 = T34.block(0, 0, 3, 3); - Eigen::MatrixXd A_w = R34.transpose() * A_s.transpose() * pose.block(0, 0, 3, 3); - Eigen::MatrixXd B_w = R34.transpose() * B_s.transpose() * pose.block(0, 0, 3, 3); - Eigen::MatrixXd C_w = R34.transpose() * C_s.transpose() * pose.block(0, 0, 3, 3); - - s_mat.block<3, 3>(0, 0) = A_s; - s_mat.block<3, 3>(0, 3) = B_s; - s_mat.block<3, 3>(0, 6) = C_s; - - w_mat.block<3, 3>(0, 0) = A_w; - w_mat.block<3, 3>(0, 3) = B_w; - w_mat.block<3, 3>(0, 6) = C_w; - - return true; -} diff --git a/cmvr-es/ik_solver/src/ik_solver_creator.cpp b/cmvr-es/ik_solver/src/ik_solver_creator.cpp deleted file mode 100644 index 81dc6b79..00000000 --- a/cmvr-es/ik_solver/src/ik_solver_creator.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// -// Created by lgv on 11/7/25. -// - - -#include "ik_solver/include/ik_solver_creator.h" -#include "ik_solver/include/lawba_ik_solver.h" -namespace cmvr -{ - std::unordered_map> - IkSolverCreator::registers_; - - std::shared_ptr IkSolverCreator::create(const IkSolverType type) - { - static std::unordered_map> - registered_planners; - auto planner_pair = registered_planners.find(type); - if (planner_pair == registered_planners.end()) - { - auto res = createNew(type); - registered_planners.emplace(type, res); - return res; - } - return planner_pair->second; - } - std::shared_ptr - IkSolverCreator::createNew(const IkSolverType type) - { - std::shared_ptr result; - switch (type) - { - case IK_SOLVER_UNKNOWN: - return nullptr; - case LAWBA_IK: - return std::make_shared(); - default: - return nullptr; - } - } -} diff --git a/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp b/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp deleted file mode 100644 index 60864c33..00000000 --- a/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp +++ /dev/null @@ -1,1193 +0,0 @@ -// Created by lgv on 11/28/25. - -#include "ik_solver/include/pinocchio_dls_ik_solver.h" -#include "ik_solver/include/urdf_parser.h" -#include "common/curve/include/s_curve.h" - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -namespace cmvr { - -static inline double clampd(double x, double lo, double hi) { - return std::max(lo, std::min(hi, x)); -} - -static inline double clamp01(double x) { - return std::max(0.0, std::min(1.0, x)); -} - -namespace { - -static inline double smoothstep01(double x) { - x = clamp01(x); - return x * x * (3.0 - 2.0 * x); -} - -} // namespace - -Eigen::MatrixXd PinocchioDlsIKSolver::dampedPseudoInverse(const Eigen::MatrixXd &J, double lambda) { - const int m = J.rows(); - const int n = J.cols(); - const double l2 = lambda * lambda; - - if (m <= n) { - Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m, m); - Eigen::MatrixXd JJt = J * J.transpose() + l2 * I; - return J.transpose() * JJt.inverse(); - } else { - Eigen::MatrixXd I = Eigen::MatrixXd::Identity(n, n); - Eigen::MatrixXd JtJ = J.transpose() * J + l2 * I; - return JtJ.inverse() * J.transpose(); - } -} - -PinocchioDlsIKSolver::PinocchioDlsIKSolver(const std::string &urdf_path, - const std::string &base_frame_name, - const std::string &flange_frame_name, - const std::string &tcp_frame_name, - int max_iters, - double pos_eps, - double rot_eps, - double damping) - : PinocchioIKBase(urdf_path, base_frame_name, flange_frame_name, tcp_frame_name) - , max_iters_(max_iters) - , pos_eps_(pos_eps) - , rot_eps_(rot_eps) - , damping_(damping) -{ -} - -bool PinocchioDlsIKSolver::buildFullQFromChain(const Eigen::VectorXd& q_chain, - Eigen::VectorXd& q_full, - const char* context) const { - if (q_chain.size() != chain_dof_) { - if (context != nullptr) { - std::cerr << "[PinocchioDlsIKSolver] " << context << " chain q size mismatch\n"; - } - return false; - } - q_full = pinocchio::neutral(model_); - q_full.segment(chain_q_start_, chain_dof_) = q_chain; - return true; -} - -Eigen::MatrixXd PinocchioDlsIKSolver::extractChainJacobian( - const Eigen::Matrix& J_full) const { - return J_full.middleCols(chain_v_start_, chain_v_dof_); -} - -const pinocchio::SE3& PinocchioDlsIKSolver::getBasePoseWorld() const { - return base_pose_cached_ ? oM_base_cached_ : data_->oMf[base_frame_id_]; -} - -Eigen::VectorXd PinocchioDlsIKSolver::computeJointLimitAvoidanceVelocity( - const Eigen::VectorXd& q_chain) const { - if (!limit_avoidance_enabled_ || - limit_avoidance_gain_ <= 0.0 || - chain_v_dof_ != chain_dof_ || - q_chain.size() != chain_dof_ || - joint_pos_lower_limits_.size() != chain_dof_ || - joint_pos_upper_limits_.size() != chain_dof_) { - return Eigen::VectorXd::Zero(chain_v_dof_); - } - - Eigen::VectorXd qdot_avoid = Eigen::VectorXd::Zero(chain_v_dof_); - for (int i = 0; i < chain_dof_; ++i) { - const double lo = joint_pos_lower_limits_[i]; - const double hi = joint_pos_upper_limits_[i]; - if (!std::isfinite(lo) || !std::isfinite(hi) || hi <= lo) { - continue; - } - - const double span = hi - lo; - const double margin = std::max(1e-4, limit_avoidance_margin_ratio_ * span); - double push = 0.0; - - if (q_chain[i] < lo + margin) { - const double s = (lo + margin - q_chain[i]) / margin; - push += limit_avoidance_gain_ * s * s; - } else if (q_chain[i] > hi - margin) { - const double s = (q_chain[i] - (hi - margin)) / margin; - push -= limit_avoidance_gain_ * s * s; - } - - if (limit_avoidance_max_push_ > 0.0) { - push = clampd(push, -limit_avoidance_max_push_, limit_avoidance_max_push_); - } - qdot_avoid[i] = push; - } - - return qdot_avoid; -} - -Eigen::VectorXd PinocchioDlsIKSolver::projectToNullspace(const Eigen::MatrixXd& J_pinv, - const Eigen::MatrixXd& J, - const Eigen::VectorXd& secondary) const { - if (secondary.size() != J.cols()) { - return Eigen::VectorXd::Zero(J.cols()); - } - const Eigen::MatrixXd N = - Eigen::MatrixXd::Identity(J.cols(), J.cols()) - J_pinv * J; - return N * secondary; -} - -bool PinocchioDlsIKSolver::init() { - UrdfParser::ChainInfo chain_info; - std::string err; - if (!initPinocchioFromUrdfChain(&chain_info, &err)) { - std::cerr << "[PinocchioDlsIKSolver] Failed to init pinocchio base: " << err << "\n"; - return false; - } - chain_q_start_ = chain_info.q_start; - chain_dof_ = chain_info.q_dof; - chain_v_start_ = chain_info.v_start; - chain_v_dof_ = chain_info.v_dof; - - { - Eigen::VectorXd q0 = pinocchio::neutral(model_); - updateKinematics(q0); - oM_base_cached_ = data_->oMf[base_frame_id_]; - base_pose_cached_ = true; - } - - cur_joints_angle_.assign(chain_dof_, 0.0); - - initialized_ = true; - std::cout << "[PinocchioDlsIKSolver] Chain '" << chain_base_frame_name_ << "' -> '" << chain_tip_frame_name_ - << "': q_start=" << chain_q_start_ << " q_dof=" << chain_dof_ - << ", v_start=" << chain_v_start_ << " v_dof=" << chain_v_dof_ - << ", nq=" << model_.nq << " nv=" << model_.nv << "\n"; - std::cout << "[PinocchioDlsIKSolver] Chain joint position limits (rad):\n"; - for (const auto& seg : chain_info.joints) { - const int q_idx = seg.q_index; - const int nq = seg.nq; - const std::string& jname = seg.name; - if (nq <= 0) continue; - - for (int k = 0; k < nq; ++k) { - const int qi = q_idx - chain_q_start_ + k; - if (qi < 0 || qi >= chain_dof_) continue; - if (nq == 1) { - std::cout << " - " << jname - << ": [" << joint_pos_lower_limits_[qi] << ", " << joint_pos_upper_limits_[qi] << "]\n"; - } else { - std::cout << " - " << jname << "[" << k << "]" - << ": [" << joint_pos_lower_limits_[qi] << ", " << joint_pos_upper_limits_[qi] << "]\n"; - } - } - } - std::cout << "[PinocchioDlsIKSolver] base pose cached (constant)\n"; - return true; -} - -bool PinocchioDlsIKSolver::ik(const Eigen::Matrix4d &target_pose_base, - std::vector &joints_angle, - bool is_tcp) -{ - const std::string& ee_link = (is_tcp && has_tcp_) ? tcp_frame_name_ : chain_tip_frame_name_; - return ik(chain_base_frame_name_, ee_link, target_pose_base, joints_angle); -} - -bool PinocchioDlsIKSolver::ik(const std::string& base_link, - const std::string& ee_link, - const Eigen::Matrix4d& target_pose, - std::vector& joints_angle) { - if (!initialized_) return false; - if ((int)cur_joints_angle_.size() != chain_dof_) return false; - if (!model_.existFrame(base_link)) { - std::cerr << "[PinocchioDlsIKSolver] base frame not found: " << base_link << "\n"; - return false; - } - if (!model_.existFrame(ee_link)) { - std::cerr << "[PinocchioDlsIKSolver] ee frame not found: " << ee_link << "\n"; - return false; - } - const pinocchio::FrameIndex base_frame_id = model_.getFrameId(base_link); - const pinocchio::FrameIndex ee_frame_id = model_.getFrameId(ee_link); - - const pinocchio::JointIndex chain_base_joint = model_.frames[base_frame_id_].parent; - const pinocchio::JointIndex base_joint = model_.frames[base_frame_id].parent; - const pinocchio::JointIndex flange_joint = model_.frames[flange_frame_id_].parent; - const pinocchio::JointIndex ee_joint = model_.frames[ee_frame_id].parent; - - auto jointOnParentPath = [this](pinocchio::JointIndex from, - pinocchio::JointIndex target) { - if (target == 0) return true; - pinocchio::JointIndex j = from; - while (j != 0) { - if (j == target) return true; - j = model_.parents[j]; - } - return false; - }; - - auto jointOnConfiguredBranch = [&](pinocchio::JointIndex j) { - return jointOnParentPath(flange_joint, j) && jointOnParentPath(j, chain_base_joint); - }; - - const bool base_on_branch = jointOnConfiguredBranch(base_joint); - const bool ee_on_branch = jointOnConfiguredBranch(ee_joint); - const bool base_is_ancestor_of_ee = - jointOnParentPath(ee_joint, base_joint); - if (!base_on_branch || !ee_on_branch || !base_is_ancestor_of_ee) { - std::cerr << "[PinocchioDlsIKSolver] base/ee must be on configured single chain and base must be ancestor of ee\n"; - return false; - } - - const pinocchio::SE3 base_M_target = matrix4ToSE3(target_pose); - - Eigen::VectorXd q_chain = Eigen::Map(cur_joints_angle_.data(), chain_dof_); - - bool success = false; - for (int iter = 0; iter < max_iters_; ++iter) { - Eigen::VectorXd q_full; - if (!buildFullQFromChain(q_chain, q_full, "ik(base,ee)")) { - return false; - } - updateKinematics(q_full); - - const pinocchio::SE3& oM_base = - (base_frame_id == base_frame_id_) ? getBasePoseWorld() : data_->oMf[base_frame_id]; - const pinocchio::SE3 oM_target = oM_base * base_M_target; - const pinocchio::SE3 &oM_cur = data_->oMf[ee_frame_id]; - pinocchio::SE3 dM = oM_cur.inverse() * oM_target; - - Eigen::Matrix err = pinocchio::log6(dM).toVector(); - if (err.head<3>().norm() < pos_eps_ && err.tail<3>().norm() < rot_eps_) { - success = true; - break; - } - - Eigen::Matrix J_full(6, model_.nv); - pinocchio::computeFrameJacobian(model_, *data_, q_full, - ee_frame_id, - pinocchio::ReferenceFrame::LOCAL, - J_full); - Eigen::MatrixXd J = extractChainJacobian(J_full); - Eigen::MatrixXd J_pinv = dampedPseudoInverse(J, damping_); - - Eigen::VectorXd dq = J_pinv * err; - q_chain += dq; - q_chain = q_chain.cwiseMax(joint_pos_lower_limits_).cwiseMin(joint_pos_upper_limits_); - } - - if (!success) { - std::cerr << "[PinocchioDlsIKSolver] IK solve failed\n"; - return false; - } - - cur_joints_angle_.assign(q_chain.data(), q_chain.data() + q_chain.size()); - joints_angle = cur_joints_angle_; - return true; -} - -void PinocchioDlsIKSolver::setJointLimitAvoidance(bool enable, - double gain, - double margin_ratio, - double max_push) { - limit_avoidance_enabled_ = enable; - limit_avoidance_gain_ = std::max(0.0, gain); - limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49); - limit_avoidance_max_push_ = max_push; -} - -bool PinocchioDlsIKSolver::ik(const std::string& base_link, - const std::string& ee_link, - const Eigen::Matrix& target_vel, - std::vector& joints_vel, - double damping, - double qdot_abs_max) -{ - if (!initialized_) return false; - if (!model_.existFrame(base_link)) { - std::cerr << "[PinocchioDlsIKSolver] base frame not found: " << base_link << "\n"; - return false; - } - if (ee_link.empty()) { - std::cerr << "[PinocchioDlsIKSolver] ee_frame_name is empty\n"; - return false; - } - if (!model_.existFrame(ee_link)) { - std::cerr << "[PinocchioDlsIKSolver] frame not found: " << ee_link << "\n"; - return false; - } - - if (chain_v_dof_ <= 0) { - std::cerr << "[PinocchioDlsIKSolver] invalid chain_v_dof\n"; - return false; - } - - if (static_cast(cur_joints_angle_.size()) != chain_dof_) { - std::cerr << "[PinocchioDlsIKSolver] ik(velocity) current joint state not initialized\n"; - return false; - } - - const pinocchio::FrameIndex base_id = model_.getFrameId(base_link); - const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link); - - const pinocchio::JointIndex chain_base_joint = model_.frames[base_frame_id_].parent; - const pinocchio::JointIndex base_joint = model_.frames[base_id].parent; - const pinocchio::JointIndex flange_joint = model_.frames[flange_frame_id_].parent; - const pinocchio::JointIndex ee_joint = model_.frames[ee_id].parent; - - auto jointOnParentPath = [this](pinocchio::JointIndex from, - pinocchio::JointIndex target) { - if (target == 0) return true; - pinocchio::JointIndex j = from; - while (j != 0) { - if (j == target) return true; - j = model_.parents[j]; - } - return false; - }; - auto jointOnConfiguredBranch = [&](pinocchio::JointIndex j) { - return jointOnParentPath(flange_joint, j) && jointOnParentPath(j, chain_base_joint); - }; - const bool base_on_branch = jointOnConfiguredBranch(base_joint); - const bool ee_on_branch = jointOnConfiguredBranch(ee_joint); - const bool base_is_ancestor_of_ee = jointOnParentPath(ee_joint, base_joint); - if (!base_on_branch || !ee_on_branch || !base_is_ancestor_of_ee) { - std::cerr << "[PinocchioDlsIKSolver] base/ee must be on configured single chain and base must be ancestor of ee\n"; - return false; - } - - const Eigen::Map q_chain(cur_joints_angle_.data(), chain_dof_); - - Eigen::VectorXd q_full; - if (!buildFullQFromChain(q_chain, q_full, "ik(velocity)")) { - return false; - } - - updateKinematics(q_full); - - const pinocchio::SE3 &oM_base = - (base_id == base_frame_id_) ? getBasePoseWorld() : data_->oMf[base_id]; - const pinocchio::SE3 &oM_ee = data_->oMf[ee_id]; - - const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee; - const Eigen::Matrix3d R_be = base_M_ee.rotation(); - - Eigen::Matrix twist_base; - twist_base.head<3>() = R_be * target_vel.head<3>(); - twist_base.tail<3>() = R_be * target_vel.tail<3>(); - - Eigen::Matrix J_world(6, model_.nv); - pinocchio::computeFrameJacobian(model_, *data_, q_full, - ee_id, - pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, - J_world); - Eigen::MatrixXd J = extractChainJacobian(J_world); - const Eigen::Matrix3d R_bo = oM_base.rotation().transpose(); - J.topRows(3) = R_bo * J.topRows(3); - J.bottomRows(3) = R_bo * J.bottomRows(3); - - const double lambda = (damping > 0.0) ? damping : damping_; - Eigen::Matrix A = J * J.transpose(); - A.diagonal().array() += (lambda * lambda); - Eigen::LDLT> ldlt(A); - if (ldlt.info() != Eigen::Success) { - std::cerr << "[PinocchioDlsIKSolver] ik(velocity) LDLT failed\n"; - return false; - } - Eigen::VectorXd qdot = J.transpose() * ldlt.solve(twist_base); - - const Eigen::VectorXd qdot_avoid = computeJointLimitAvoidanceVelocity(q_chain); - if (qdot_avoid.size() == chain_v_dof_ && qdot_avoid.squaredNorm() > 1e-16) { - const Eigen::Matrix A_inv = - ldlt.solve(Eigen::Matrix::Identity()); - const Eigen::MatrixXd J_pinv = J.transpose() * A_inv; - qdot += projectToNullspace(J_pinv, J, qdot_avoid); - } - - double gamma = 1.0; - for (int i = 0; i < chain_v_dof_; ++i) { - double vel_limit = std::numeric_limits::infinity(); - if (std::isfinite(qdot_abs_max) && qdot_abs_max > 0.0) { - vel_limit = std::min(vel_limit, qdot_abs_max); - } - if (joint_vel_limits_.size() == chain_v_dof_) { - const double urdf_limit = std::abs(joint_vel_limits_[i]); - if (std::isfinite(urdf_limit) && urdf_limit > 0.0) { - vel_limit = std::min(vel_limit, urdf_limit); - } - } - - const double qi_abs = std::abs(qdot[i]); - if (std::isfinite(vel_limit) && vel_limit > 0.0 && qi_abs > vel_limit) { - gamma = std::min(gamma, vel_limit / qi_abs); - } - } - - joints_vel.resize(chain_v_dof_); - const Eigen::VectorXd qdot_limited = gamma * qdot; - for (int i = 0; i < chain_v_dof_; ++i) { - joints_vel[i] = qdot_limited[i]; - } - - return true; -} - -// ============================================================================ -// MoveL with tau time-scaling -// ============================================================================ -bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_input, - const std::vector& q_start, - std::vector>& q_traj, - std::vector& t_traj, - double dt_real, - double v_tcp_max, - double a_tcp_max, - double j_tcp_max, - const std::vector& qd_max, - bool is_tcp, - CartesianFrame input_frame, - std::vector>* qdot_traj) -{ - if (!initialized_) return false; - if ((int)q_start.size() != chain_dof_) return false; - if (!qd_max.empty() && (int)qd_max.size() != chain_dof_) return false; - - dt_real = std::max(dt_real, 1e-4); - - const pinocchio::FrameIndex ee_id = - (is_tcp && has_tcp_) ? tcp_frame_id_ : flange_frame_id_; - - Eigen::Matrix4d T0_base; - if (!fk(q_start, T0_base, is_tcp)) return false; - - const Eigen::Vector3d p0 = T0_base.block<3,1>(0,3); - const Eigen::Matrix3d R_fixed = T0_base.block<3,3>(0,0); - - // moveL_SCurveLocal 当前只对目标平移做直线规划，姿态仍保持起始末端姿态。 - Eigen::Matrix4d target_pose_base = target_pose_input; - if (input_frame == CartesianFrame::Tool) { - target_pose_base = T0_base * target_pose_input; - } - - const Eigen::Vector3d pg = target_pose_base.block<3,1>(0,3); - const Eigen::Vector3d dp = pg - p0; - const double L = dp.norm(); - if (L < 1e-9) { - q_traj = {q_start}; - t_traj = {0.0}; - if (qdot_traj) { - qdot_traj->assign(1, std::vector(static_cast(chain_dof_), 0.0)); - } - return true; - } - const Eigen::Vector3d dir_base = dp / L; - - std::cerr << "dp(base)= " << dp.transpose() - << " frame=" << (input_frame == CartesianFrame::Tool ? "tool" : "base") - << "\n"; - - SCurve curve(v_tcp_max, a_tcp_max, j_tcp_max); - const SCurveProfile prof = curve.calculateProfile(0.0, L); - if (prof.total_time <= 0.0) { - std::cerr << "[moveL_SCurveLocal] invalid profile\n"; - return false; - } - - const double Kp_perp0 = 1.0; - const double Kp_rot0 = 1.0; - const double k_posture0 = 1.0; - const double eps_dq = 1e-9; - - Eigen::VectorXd qd_lim(chain_dof_); - if (qd_max.empty()) { - for (int i = 0; i < chain_dof_; ++i) { - double limit_i = std::numeric_limits::infinity(); - if (joint_vel_limits_.size() == chain_dof_) { - const double urdf_limit = std::abs(joint_vel_limits_[i]); - if (std::isfinite(urdf_limit) && urdf_limit > 0.0) { - limit_i = urdf_limit; - } - } - qd_lim[i] = limit_i; - } - } else { - for (int i = 0; i < chain_dof_; ++i) { - qd_lim[i] = std::max(1e-6, std::abs(qd_max[i])); - } - } - - Eigen::VectorXd q_ref = Eigen::Map(q_start.data(), chain_dof_); - Eigen::VectorXd q_chain = Eigen::Map(q_start.data(), chain_dof_); - - double t = 0.0; - double tau = 0.0; - - q_traj.clear(); - t_traj.clear(); - q_traj.reserve((size_t)(prof.total_time / dt_real) + 10); - t_traj.reserve((size_t)(prof.total_time / dt_real) + 10); - q_traj.push_back(q_start); - t_traj.push_back(0.0); - if (qdot_traj) { - qdot_traj->clear(); - qdot_traj->reserve((size_t)(prof.total_time / dt_real) + 10); - qdot_traj->push_back(std::vector(static_cast(chain_dof_), 0.0)); - } - - double gamma_prev = 1.0; - const double gamma_tau = 0.03; - - int stuck_cnt = 0; - - while (tau < prof.total_time - 1e-12) { - const double step_real = dt_real; - - const double s = curve.getPositionAtTime(prof, tau); - const double sd = curve.getVelocityAtTime(prof, tau); - - const double w_end = smoothstep01(sd / std::max(1e-6, 0.15 * prof.v_cruise)); - const double Kp_perp = Kp_perp0 * w_end; - const double Kp_rot = Kp_rot0 * w_end; - const double k_posture = k_posture0 * w_end; - - const Eigen::Vector3d p_des_base = p0 + dir_base * s; - - Eigen::Matrix4d T_des_base = Eigen::Matrix4d::Identity(); - T_des_base.block<3,3>(0,0) = R_fixed; - T_des_base.block<3,1>(0,3) = p_des_base; - - Eigen::VectorXd q_full; - if (!buildFullQFromChain(q_chain, q_full, "moveL_SCurveLocal")) { - return false; - } - updateKinematics(q_full); - - const pinocchio::SE3& oM_base = getBasePoseWorld(); - const Eigen::Matrix3d R_ob = oM_base.rotation(); - - const pinocchio::SE3 &oM_cur = data_->oMf[ee_id]; - const pinocchio::SE3 oM_des = oM_base * matrix4ToSE3(T_des_base); - - pinocchio::SE3 base_M_cur = oM_base.inverse() * oM_cur; - const Eigen::Vector3d p_cur_base = base_M_cur.translation(); - - const Eigen::Vector3d e_base = p_des_base - p_cur_base; - const Eigen::Vector3d e_perp = e_base - dir_base * (dir_base.dot(e_base)); - - pinocchio::SE3 dM = oM_cur.inverse() * oM_des; - Eigen::Matrix err6 = pinocchio::log6(dM).toVector(); - - Eigen::Matrix J_full(6, model_.nv); - pinocchio::computeFrameJacobian(model_, *data_, q_full, - ee_id, - pinocchio::ReferenceFrame::LOCAL, - J_full); - - Eigen::MatrixXd J = extractChainJacobian(J_full); - Eigen::MatrixXd J_pinv = dampedPseudoInverse(J, damping_); - - double sigma_min = 0.0, sigma_max = 0.0, cond = 0.0; - { - Eigen::JacobiSVD svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV); - const auto& sv = svd.singularValues(); - sigma_max = sv.size() ? sv.maxCoeff() : 0.0; - sigma_min = sv.size() ? sv.minCoeff() : 0.0; - cond = (sigma_min > 1e-12) ? (sigma_max / sigma_min) : 1e12; - } - - const Eigen::Vector3d v_base_ff = dir_base * sd; - const Eigen::Vector3d v_world_ff = R_ob * v_base_ff; - const Eigen::Vector3d v_local_ff = oM_cur.rotation().transpose() * v_world_ff; - - const Eigen::Vector3d v_base_fb = Kp_perp * e_perp; - const Eigen::Vector3d v_world_fb = R_ob * v_base_fb; - const Eigen::Vector3d v_local_fb = oM_cur.rotation().transpose() * v_world_fb; - - Eigen::Matrix xi = Eigen::Matrix::Zero(); - xi.head<3>() = v_local_ff + v_local_fb; - xi.tail<3>() = Kp_rot * err6.tail<3>(); - - const Eigen::VectorXd dq_task = J_pinv * xi; - - const Eigen::VectorXd dq_posture = -k_posture * (q_chain - q_ref); - const Eigen::VectorXd dq_raw = dq_task + projectToNullspace(J_pinv, J, dq_posture); - - double gamma_speed = 1.0; - for (int i = 0; i < chain_dof_; ++i) { - const double denom = std::max(eps_dq, std::abs(dq_raw[i])); - gamma_speed = std::min(gamma_speed, qd_lim[i] / denom); - } - gamma_speed = clamp01(gamma_speed); - - double gamma_lim = 1.0; - for (int i = 0; i < chain_dof_; ++i) { - const double dqi = dq_raw[i]; - if (std::abs(dqi) < eps_dq) continue; - - if (dqi > 0.0) { - const double margin = joint_pos_upper_limits_[i] - q_chain[i]; - const double g = margin / (dqi * step_real); - gamma_lim = std::min(gamma_lim, g); - } else { - const double margin = q_chain[i] - joint_pos_lower_limits_[i]; - const double g = margin / ((-dqi) * step_real); - gamma_lim = std::min(gamma_lim, g); - } - } - gamma_lim = clamp01(gamma_lim); - - double gamma_target = std::min(gamma_speed, gamma_lim); - - const double alpha = step_real / (gamma_tau + step_real); - const double gamma_tot = gamma_prev + alpha * (gamma_target - gamma_prev); - gamma_prev = gamma_tot; - - if (gamma_tot < 1e-6) stuck_cnt++; else stuck_cnt = 0; - if (stuck_cnt > 50) { - std::cerr << "[moveL_SCurveLocal] stuck: gamma_tot too small (limits/speed)\n"; - return false; - } - - const Eigen::VectorXd qdot_cmd = gamma_tot * dq_raw; - - const Eigen::VectorXd q_before_clamp = q_chain + qdot_cmd * step_real; - const Eigen::VectorXd q_after_clamp = - q_before_clamp.cwiseMax(joint_pos_lower_limits_).cwiseMin(joint_pos_upper_limits_); - for (int i = 0; i < chain_dof_; ++i) { - if (std::abs(q_after_clamp[i] - q_before_clamp[i]) > 1e-12) { - std::cerr << "[moveL_SCurveLocal] clamp triggered: step=" << q_traj.size() - << ", joint_index=" << i - << ", before=" << q_before_clamp[i] - << ", after=" << q_after_clamp[i] - << std::endl; - } - } - q_chain = q_after_clamp; - - tau = std::min(prof.total_time, tau + gamma_tot * step_real); - t += step_real; - - std::vector q_out(chain_dof_); - Eigen::Map(q_out.data(), chain_dof_) = q_chain; - q_traj.push_back(std::move(q_out)); - t_traj.push_back(t); - - if (qdot_traj) { - std::vector qdot_out(static_cast(chain_dof_)); - Eigen::Map(qdot_out.data(), chain_dof_) = qdot_cmd; - qdot_traj->push_back(std::move(qdot_out)); - } - } - - return !q_traj.empty(); -} - -// ==================== speedL ==================== - -bool PinocchioDlsIKSolver::configureSpeedL(const SpeedLConfig& config) -{ - if (!initialized_) { - std::cerr << "[PinocchioDlsIKSolver] configureSpeedL failed: solver not initialized\n"; - return false; - } - - if (!config.joint_velocity_max.empty() && - static_cast(config.joint_velocity_max.size()) != chain_v_dof_) { - std::cerr << "[PinocchioDlsIKSolver] configureSpeedL failed: joint_velocity_max size mismatch\n"; - return false; - } - - if (!config.joint_acceleration_max.empty() && - static_cast(config.joint_acceleration_max.size()) != chain_v_dof_) { - std::cerr << "[PinocchioDlsIKSolver] configureSpeedL failed: joint_acceleration_max size mismatch\n"; - return false; - } - - speedl_config_ = config; - - speedl_twist_limiter_.setLinearConstraints(config.linear_velocity_max, - config.linear_acceleration_max, - config.linear_jerk_max); - speedl_twist_limiter_.setAngularConstraints(config.angular_velocity_max, - config.angular_acceleration_max, - config.angular_jerk_max); - speedl_twist_limiter_.setLinearTargetReplanThreshold(config.linear_target_replan_threshold); - speedl_twist_limiter_.setAngularTargetReplanThreshold(config.angular_target_replan_threshold); - speedl_twist_limiter_.setLinearReverseSwitchPolicy(config.linear_reverse_cos_threshold, - config.linear_reverse_switch_speed_threshold); - - speedl_configured_ = true; - return true; -} - -void PinocchioDlsIKSolver::resetSpeedL() -{ - speedl_twist_limiter_.reset(); - speedl_prev_qdot_cmd_.assign(chain_v_dof_, 0.0); - speedl_twist_measured_base_.setZero(); - speedl_twist_command_base_.setZero(); - speedl_twist_executed_base_.setZero(); - speedl_joint_soft_limit_active_ = false; -} - -bool PinocchioDlsIKSolver::resolveSpeedLEeFrame(bool is_tcp, pinocchio::FrameIndex& ee_id) const -{ - if (is_tcp && has_tcp_) { - ee_id = tcp_frame_id_; - return true; - } - ee_id = flange_frame_id_; - return true; -} - -bool PinocchioDlsIKSolver::buildJacobianBaseAtQ(const std::vector& q_chain_std, - pinocchio::FrameIndex ee_id, - Eigen::MatrixXd& J_base, - Eigen::Matrix3d& base_R_ee, - Eigen::VectorXd* q_full_out) -{ - if (static_cast(q_chain_std.size()) != chain_dof_) { - std::cerr << "[PinocchioDlsIKSolver] buildJacobianBaseAtQ: q size mismatch\n"; - return false; - } - - const Eigen::Map q_chain(q_chain_std.data(), chain_dof_); - - Eigen::VectorXd q_full; - if (!buildFullQFromChain(q_chain, q_full, "speedL.buildJacobianBaseAtQ")) { - return false; - } - - updateKinematics(q_full); - - const pinocchio::SE3& oM_base = getBasePoseWorld(); - const pinocchio::SE3& oM_ee = data_->oMf[ee_id]; - const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee; - base_R_ee = base_M_ee.rotation(); - - Eigen::Matrix J_world(6, model_.nv); - pinocchio::computeFrameJacobian(model_, *data_, q_full, - ee_id, - pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, - J_world); - - J_base = extractChainJacobian(J_world); - - const Eigen::Matrix3d R_bo = oM_base.rotation().transpose(); - J_base.topRows(3) = R_bo * J_base.topRows(3); - J_base.bottomRows(3) = R_bo * J_base.bottomRows(3); - - if (q_full_out != nullptr) { - *q_full_out = q_full; - } - - return true; -} - -bool PinocchioDlsIKSolver::computeMeasuredTwistBase(const std::vector& q_chain, - const std::vector& qdot_chain, - pinocchio::FrameIndex ee_id, - Eigen::Matrix& twist_base, - Eigen::MatrixXd* J_base_out, - Eigen::Matrix3d* base_R_ee_out, - Eigen::VectorXd* q_full_out) -{ - if (static_cast(qdot_chain.size()) != chain_v_dof_) { - std::cerr << "[PinocchioDlsIKSolver] computeMeasuredTwistBase: qdot size mismatch\n"; - return false; - } - - Eigen::MatrixXd J_base; - Eigen::Matrix3d base_R_ee = Eigen::Matrix3d::Identity(); - if (!buildJacobianBaseAtQ(q_chain, ee_id, J_base, base_R_ee, q_full_out)) { - return false; - } - - const Eigen::Map qdot(qdot_chain.data(), chain_v_dof_); - twist_base = J_base * qdot; - - if (J_base_out != nullptr) { - *J_base_out = J_base; - } - if (base_R_ee_out != nullptr) { - *base_R_ee_out = base_R_ee; - } - - return true; -} - -Eigen::VectorXd PinocchioDlsIKSolver::applyJointVelocityLimits(const Eigen::VectorXd& qdot_des) const -{ - if (qdot_des.size() != chain_v_dof_) { - return Eigen::VectorXd::Zero(chain_v_dof_); - } - - double gamma = 1.0; - for (int i = 0; i < chain_v_dof_; ++i) { - double limit_i = std::numeric_limits::infinity(); - - if (!speedl_config_.joint_velocity_max.empty()) { - const double cfg_lim = std::abs(speedl_config_.joint_velocity_max[i]); - if (cfg_lim > 0.0) { - limit_i = std::min(limit_i, cfg_lim); - } - } - - if (joint_vel_limits_.size() == chain_v_dof_) { - const double urdf_lim = std::abs(joint_vel_limits_[i]); - if (std::isfinite(urdf_lim) && urdf_lim > 0.0) { - limit_i = std::min(limit_i, urdf_lim); - } - } - - const double qi = std::abs(qdot_des[i]); - if (std::isfinite(limit_i) && limit_i > 0.0 && qi > limit_i) { - gamma = std::min(gamma, limit_i / qi); - } - } - - return gamma * qdot_des; -} - -Eigen::VectorXd PinocchioDlsIKSolver::applyJointSoftLimitVelocity(const Eigen::VectorXd& q_chain, - const Eigen::VectorXd& qdot_des) -{ - if (!speedl_config_.enable_joint_soft_limit_velocity || - q_chain.size() != chain_dof_ || - qdot_des.size() != chain_v_dof_ || - chain_dof_ != chain_v_dof_ || - joint_pos_lower_limits_.size() != chain_dof_ || - joint_pos_upper_limits_.size() != chain_dof_) { - return qdot_des; - } - - const double hard_margin = std::max(1e-4, speedl_config_.joint_hard_limit_margin); - const double soft_margin = std::max(hard_margin + 1e-4, speedl_config_.joint_soft_limit_margin); - - auto apply_scalar = [&](double q, - double v, - double q_min, - double q_max) -> double { - if (q_max <= q_min) { - return 0.0; - } - - if (v < 0.0) { - const double q_hard = q_min + hard_margin; - const double q_soft = q_min + soft_margin; - if (q <= q_hard) { - return 0.0; - } - if (q < q_soft) { - const double s = std::clamp((q - q_hard) / (q_soft - q_hard), 0.0, 1.0); - return v * s; - } - } else if (v > 0.0) { - const double q_hard = q_max - hard_margin; - const double q_soft = q_max - soft_margin; - if (q >= q_hard) { - return 0.0; - } - if (q > q_soft) { - const double s = std::clamp((q_hard - q) / (q_hard - q_soft), 0.0, 1.0); - return v * s; - } - } - return v; - }; - - Eigen::VectorXd qdot_limited = qdot_des; - bool clamped_any = false; - std::ostringstream oss; - - for (int i = 0; i < chain_v_dof_; ++i) { - const double v_before = qdot_des[i]; - const double v_after = apply_scalar(q_chain[i], - v_before, - joint_pos_lower_limits_[i], - joint_pos_upper_limits_[i]); - qdot_limited[i] = v_after; - - if (std::abs(v_after - v_before) > 1e-9) { - clamped_any = true; - if (oss.tellp() > 0) { - oss << " | "; - } - const std::string joint_name = - (i < static_cast(chain_joint_names_.size())) ? chain_joint_names_[i] : ("joint_" + std::to_string(i)); - oss << joint_name - << " q=" << q_chain[i] - << " v:" << v_before << "->" << v_after; - } - } - - if (clamped_any && !speedl_joint_soft_limit_active_) { - std::cerr << "[PinocchioDlsIKSolver] speedL soft joint-limit velocity clamp active: " - << oss.str() << "\n"; - } else if (!clamped_any && speedl_joint_soft_limit_active_) { - std::cerr << "[PinocchioDlsIKSolver] speedL soft joint-limit velocity clamp released\n"; - } - speedl_joint_soft_limit_active_ = clamped_any; - - return qdot_limited; -} - -Eigen::VectorXd PinocchioDlsIKSolver::applyJointAccelerationLimits(const Eigen::VectorXd& qdot_des, - const Eigen::VectorXd& qdot_reference, - double dt) const -{ - if (qdot_des.size() != chain_v_dof_ || qdot_reference.size() != chain_v_dof_ || dt <= 0.0) { - return qdot_des; - } - - if (speedl_config_.joint_acceleration_max.empty()) { - return qdot_des; - } - - double gamma = 1.0; - for (int i = 0; i < chain_v_dof_; ++i) { - const double amax = std::abs(speedl_config_.joint_acceleration_max[i]); - if (amax <= 0.0) { - continue; - } - - const double delta = qdot_des[i] - qdot_reference[i]; - const double max_delta = amax * dt; - const double abs_delta = std::abs(delta); - - if (abs_delta > max_delta) { - gamma = std::min(gamma, max_delta / abs_delta); - } - } - - return qdot_reference + gamma * (qdot_des - qdot_reference); -} - -bool PinocchioDlsIKSolver::speedLStep(const Eigen::Matrix& target_twist, - double dt, - const std::vector& q_measured, - const std::vector& qdot_measured, - std::vector& qdot_cmd, - CartesianFrame input_frame, - bool is_tcp) -{ - return speedLStepImpl(target_twist, - dt, - q_measured, - qdot_measured, - qdot_cmd, - input_frame, - is_tcp, - true); -} - -bool PinocchioDlsIKSolver::speedLStepImpl(const Eigen::Matrix& target_twist, - double dt, - const std::vector& q_measured, - const std::vector& qdot_measured, - std::vector& qdot_cmd, - CartesianFrame input_frame, - bool is_tcp, - bool enable_twist_sync) -{ - if (!initialized_) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep failed: solver not initialized\n"; - return false; - } - if (!speedl_configured_) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep failed: speedL not configured\n"; - return false; - } - if (dt <= 0.0) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep failed: invalid dt\n"; - return false; - } - if (static_cast(q_measured.size()) != chain_dof_) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep failed: q_measured size mismatch\n"; - return false; - } - if (static_cast(qdot_measured.size()) != chain_v_dof_) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep failed: qdot_measured size mismatch\n"; - return false; - } - - pinocchio::FrameIndex ee_id = flange_frame_id_; - if (!resolveSpeedLEeFrame(is_tcp, ee_id)) { - return false; - } - speedl_last_is_tcp_ = is_tcp; - - cur_joints_angle_ = q_measured; - - Eigen::MatrixXd J_base; - Eigen::Matrix3d base_R_ee = Eigen::Matrix3d::Identity(); - Eigen::VectorXd q_full; - if (!computeMeasuredTwistBase(q_measured, - qdot_measured, - ee_id, - speedl_twist_measured_base_, - &J_base, - &base_R_ee, - &q_full)) { - return false; - } - - if (enable_twist_sync) { - speedl_twist_limiter_.synchronize(speedl_twist_measured_base_, dt, true); - } - - // 2) 设置目标 twist - speedl_twist_limiter_.setTargetTwist(target_twist, input_frame); - - // 3) task-space 限幅得到命令 twist(base) - const Eigen::Matrix twist_cmd_base = - speedl_twist_limiter_.update(dt, base_R_ee); - speedl_twist_command_base_ = twist_cmd_base; - - // 4) DLS 微分 IK：qdot_des = J^T (J J^T + λ²I)^-1 * twist_cmd_base - const double lambda = damping_; - Eigen::Matrix A = J_base * J_base.transpose(); - A.diagonal().array() += (lambda * lambda); - - Eigen::LDLT> ldlt(A); - if (ldlt.info() != Eigen::Success) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep failed: LDLT decomposition failed\n"; - return false; - } - - Eigen::VectorXd qdot_des = J_base.transpose() * ldlt.solve(twist_cmd_base); - - // 4.1) null-space joint-limit avoidance - const Eigen::Map q_chain(q_measured.data(), chain_dof_); - const Eigen::VectorXd qdot_avoid = computeJointLimitAvoidanceVelocity(q_chain); - if (qdot_avoid.size() == chain_v_dof_ && qdot_avoid.squaredNorm() > 1e-16) { - const Eigen::Matrix A_inv = - ldlt.solve(Eigen::Matrix::Identity()); - const Eigen::MatrixXd J_pinv = J_base.transpose() * A_inv; - qdot_des += projectToNullspace(J_pinv, J_base, qdot_avoid); - } - - // 5) joint velocity limit（整体缩放） - qdot_des = applyJointVelocityLimits(qdot_des); - - // 5.1) joint soft position-limit velocity clamp（逐轴压缩靠近限位且继续往外的速度） - qdot_des = applyJointSoftLimitVelocity(q_chain, qdot_des); - - // 6) joint acceleration limit（相对 measured qdot 整体缩放） - const Eigen::Map qdot_meas_vec(qdot_measured.data(), chain_v_dof_); - qdot_des = applyJointAccelerationLimits(qdot_des, qdot_meas_vec, dt); - - // 7) 输出 - qdot_cmd.resize(chain_v_dof_); - for (int i = 0; i < chain_v_dof_; ++i) { - qdot_cmd[i] = qdot_des[i]; - } - - speedl_prev_qdot_cmd_.assign(qdot_cmd.begin(), qdot_cmd.end()); - - // 8) 回算 executed twist(base) - speedl_twist_executed_base_ = J_base * qdot_des; - - return true; -} - -bool PinocchioDlsIKSolver::speedLStep(const Eigen::Matrix& target_twist, - double dt, - const std::vector& q_measured, - std::vector& qdot_cmd, - CartesianFrame input_frame, - bool is_tcp) -{ - if (!initialized_) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep(q-only) failed: solver not initialized\n"; - return false; - } - if (!speedl_configured_) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep(q-only) failed: speedL not configured\n"; - return false; - } - if (dt <= 0.0) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep(q-only) failed: invalid dt\n"; - return false; - } - if (static_cast(q_measured.size()) != chain_dof_) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep(q-only) failed: q_measured size mismatch\n"; - return false; - } - - std::vector qdot_measured(chain_v_dof_, 0.0); - if (static_cast(speedl_prev_qdot_cmd_.size()) == chain_v_dof_) { - qdot_measured = speedl_prev_qdot_cmd_; - } - - return speedLStepImpl(target_twist, - dt, - q_measured, - qdot_measured, - qdot_cmd, - input_frame, - is_tcp, - false); -} - -bool PinocchioDlsIKSolver::speedLStep(const Eigen::Matrix& target_twist, - double dt, - std::vector& qdot_cmd, - CartesianFrame input_frame, - bool is_tcp) -{ - if (!initialized_) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep(open-loop) failed: solver not initialized\n"; - return false; - } - if (!speedl_configured_) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep(open-loop) failed: speedL not configured\n"; - return false; - } - if (dt <= 0.0) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep(open-loop) failed: invalid dt\n"; - return false; - } - if (static_cast(cur_joints_angle_.size()) != chain_dof_) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep(open-loop) failed: current joints not initialized\n"; - return false; - } - if (chain_dof_ != chain_v_dof_) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep(open-loop) failed: internal q += qdot * dt propagation requires chain_dof == chain_v_dof_\n"; - return false; - } - - std::vector q_measured = cur_joints_angle_; - const bool ok = speedLStep(target_twist, - dt, - q_measured, - qdot_cmd, - input_frame, - is_tcp); - if (!ok) { - return false; - } - if (static_cast(qdot_cmd.size()) != chain_v_dof_) { - std::cerr << "[PinocchioDlsIKSolver] speedLStep(open-loop) failed: qdot_cmd size mismatch after closed-loop solve\n"; - return false; - } - - Eigen::Map q_chain(cur_joints_angle_.data(), chain_dof_); - const Eigen::Map qdot_cmd_vec(qdot_cmd.data(), chain_v_dof_); - q_chain += qdot_cmd_vec * dt; - - if (joint_pos_lower_limits_.size() == chain_dof_ && - joint_pos_upper_limits_.size() == chain_dof_) { - q_chain = q_chain.cwiseMax(joint_pos_lower_limits_).cwiseMin(joint_pos_upper_limits_); - } - - return true; -} - -} // namespace cmvr diff --git a/cmvr-es/ik_solver/src/pinocchio_ik_base.cpp b/cmvr-es/ik_solver/src/pinocchio_ik_base.cpp deleted file mode 100644 index 4b2fc805..00000000 --- a/cmvr-es/ik_solver/src/pinocchio_ik_base.cpp +++ /dev/null @@ -1,154 +0,0 @@ -// Created by Codex on 2026/3/3. - -#include "ik_solver/include/pinocchio_ik_base.h" - -#include -#include - -#include - -namespace cmvr { - -PinocchioIKBase::PinocchioIKBase(const std::string& urdf_path, - const std::string& base_frame_name, - const std::string& flange_frame_name, - const std::string& tcp_frame_name) - : IKSolver(urdf_path, base_frame_name, flange_frame_name) - , tcp_frame_name_(tcp_frame_name) { -} - -PinocchioIKBase::PinocchioIKBase(std::shared_ptr parser, - const std::string& base_frame_name, - const std::string& flange_frame_name, - const std::string& tcp_frame_name) - : IKSolver(parser, base_frame_name, flange_frame_name) - , tcp_frame_name_(tcp_frame_name) { -} - -bool PinocchioIKBase::initPinocchioFromUrdfChain(UrdfParser::ChainInfo* chain_info_out, - std::string* error) { - if (!urdf_parser_ || !urdf_parser_->loaded()) { - if (error) *error = "urdf parser not initialized"; - return false; - } - if (chain_base_frame_name_.empty() || chain_tip_frame_name_.empty()) { - if (error) *error = "chain base/tip frame name is empty"; - return false; - } - if (!urdf_chain_cached_) { - if (!initUrdfChain(urdf_parser_, chain_base_frame_name_, chain_tip_frame_name_)) { - if (error) *error = "initUrdfChain failed"; - return false; - } - } - - model_ = urdf_parser_->model(); - data_ = std::make_unique(model_); - - UrdfParser::ChainInfo chain_info; - std::string chain_err; - if (!urdf_parser_->extractChain(chain_base_frame_name_, chain_tip_frame_name_, chain_info, &chain_err)) { - if (error) *error = chain_err; - return false; - } - - base_frame_id_ = chain_info.base_frame_id; - flange_frame_id_ = chain_info.tip_frame_id; - chain_q_start_ = chain_info.q_start; - chain_q_dof_ = chain_info.q_dof; - chain_v_start_ = chain_info.v_start; - chain_v_dof_ = chain_info.v_dof; - - has_tcp_ = false; - tcp_frame_id_ = (pinocchio::FrameIndex)(-1); - if (!tcp_frame_name_.empty() && model_.existFrame(tcp_frame_name_)) { - tcp_frame_id_ = model_.getFrameId(tcp_frame_name_); - has_tcp_ = true; - } - - if (chain_info_out != nullptr) { - *chain_info_out = chain_info; - } - return true; -} - -bool PinocchioIKBase::buildFullQFromInput(const std::vector& joints, - Eigen::VectorXd& q_full, - const char* context) const { - const int size = static_cast(joints.size()); - if (size != chain_q_dof_ && size != model_.nq) { - if (context != nullptr) { - std::cerr << "[PinocchioIKBase] " << context << " joints size mismatch\n"; - } - return false; - } - - q_full = pinocchio::neutral(model_); - if (size == model_.nq) { - q_full = Eigen::Map(joints.data(), model_.nq); - } else { - Eigen::Map q_chain(joints.data(), chain_q_dof_); - q_full.segment(chain_q_start_, chain_q_dof_) = q_chain; - } - return true; -} - -void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) { - pinocchio::forwardKinematics(model_, *data_, q_full); - pinocchio::updateFramePlacements(model_, *data_); -} - -bool PinocchioIKBase::fk(const std::vector& joints_angle, - Eigen::Matrix4d& cur_pose, - bool is_tcp) { - const std::string& ee_link = (is_tcp && has_tcp_) ? tcp_frame_name_ : chain_tip_frame_name_; - return fk(chain_base_frame_name_, ee_link, joints_angle, cur_pose); -} - -bool PinocchioIKBase::fk(const std::string& base_link, - const std::string& ee_link, - const std::vector& joints_angle, - Eigen::Matrix4d& cur_pose) { - if (!data_) { - std::cerr << "[PinocchioIKBase] fk called before pinocchio init\n"; - return false; - } - if (!model_.existFrame(base_link)) { - std::cerr << "[PinocchioIKBase] base frame not found: " << base_link << "\n"; - return false; - } - if (!model_.existFrame(ee_link)) { - std::cerr << "[PinocchioIKBase] ee frame not found: " << ee_link << "\n"; - return false; - } - - Eigen::VectorXd q_full; - if (!buildFullQFromInput(joints_angle, q_full, "fk")) { - return false; - } - updateKinematics(q_full); - - const pinocchio::FrameIndex base_id = model_.getFrameId(base_link); - const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link); - const pinocchio::SE3& oM_base = data_->oMf[base_id]; - const pinocchio::SE3& oM_ee = data_->oMf[ee_id]; - const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee; - cur_pose = se3ToMatrix4(base_M_ee); - return true; -} - -pinocchio::SE3 PinocchioIKBase::matrix4ToSE3(const Eigen::Matrix4d& T) { - pinocchio::SE3 M; - M.rotation() = T.block<3,3>(0,0); - M.translation() = T.block<3,1>(0,3); - return M; -} - -Eigen::Matrix4d PinocchioIKBase::se3ToMatrix4(const pinocchio::SE3& M) { - Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); - T.block<3,3>(0,0) = M.rotation(); - T.block<3,1>(0,3) = M.translation(); - return T; -} - -} // namespace cmvr diff --git a/cmvr-es/main.cpp b/cmvr-es/main.cpp index ab0f76d3..4affd835 100644 --- a/cmvr-es/main.cpp +++ b/cmvr-es/main.cpp @@ -4,33 +4,146 @@ #include #include -#include -#include -#include -#include "service/grpc/include/server_runner.h" -#include "utils/base/include/logger.h" -using namespace cmvr::service; -int main(int argc, char* argv[]) { - std::string config_path; - if(argc == 1) { - char exe_path[PATH_MAX] = {0}; - ssize_t count = readlink("/proc/self/exe", exe_path, PATH_MAX); - if (count == -1) { - std::cerr << "Failed to read /proc/self/exe" << std::endl; - return 1; - } - std::string exe_dir = dirname(exe_path); - config_path = exe_dir + "/config/cabin_robot.xml"; - std::cout << "Using default config path: " << config_path << std::endl; +#include +#include +#include +#include "common/base/logging/logger.h" +#include "cmvr/config/cmvr_es_config/cmvr_es_config.pb.h" +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "cmvr/config/task_manager_config/task_manager_config.pb.h" +#include "common/config/config_files.h" +#include "common/io/proto_file_io.h" +#include "manager/device_manager/include/device_manager.h" +#include "manager/task_manager/include/task_manager.h" +#include "task/grpc_server_task/include/grpc_server_task.h" + +namespace { + +void logSection(const char* title) +{ + CMVR_LOG(INFO) << "---------------- " << title << " ----------------"; +} + +bool executableDirectory(std::filesystem::path& directory) +{ + char executable_path[PATH_MAX] = {0}; + const ssize_t count = readlink("/proc/self/exe", executable_path, PATH_MAX - 1); + if (count < 0) { + return false; } - else if (argc == 2){ - config_path = argv[1]; + executable_path[count] = '\0'; + directory = std::filesystem::path(executable_path).parent_path(); + return true; +} + +} // namespace + +int main(int argc, char* argv[]) +{ + std::filesystem::path executable_dir; + if (!executableDirectory(executable_dir)) { + CMVR_LOG(ERROR) << "Failed to read /proc/self/exe"; + return 1; } - ServerRunner runner; - const XmlNode config(config_path); - runner.start(config); - runner.join(); - google::ShutdownGoogleLogging(); + std::string config_path; + if (argc == 1) { + config_path = (executable_dir / "config/cmvr_es.pb.txt").string(); + CMVR_LOG(INFO) << "Using default config path: " << config_path; + } else if (argc == 2) { + config_path = argv[1]; + } else { + CMVR_LOG(ERROR) << "Usage: " << argv[0] << " [cmvr_es.pb.txt]"; + return 1; + } + + cmvr::ConfigHelper::setConfigRootFromFile(config_path); + cmvr::config::CMVRESRootConfig root_config; + if (!ProtoMessageIo::getProtoFromAsciiFile(config_path, &root_config)) { + CMVR_LOG(ERROR) << "Failed to load config: " << config_path; + return 1; + } + + const auto& app_config = root_config.cmvr_es(); + if (app_config.logger_config_file().empty()) { + CMVR_LOG(ERROR) << "Logger config file is empty"; + return 1; + } + + cmvr::config::LoggerRootConfig logger_root; + if (!cmvr::ConfigHelper::loadConfigFile(app_config.logger_config_file(), logger_root)) { + CMVR_LOG(ERROR) << "Failed to load logger config: " << app_config.logger_config_file(); + return 1; + } + if (!cmvr::logging::initLogging(logger_root.logger(), "cmvr_es", executable_dir)) { + CMVR_LOG(FATAL) << "Failed to initialize logging"; + return 1; + } + + logSection("Startup"); + CMVR_LOG(INFO) << "Logging initialized"; + CMVR_LOG(INFO) << "[Startup] Runtime config file: " << config_path; + CMVR_LOG(INFO) << "[Startup] Logger config file: " + << cmvr::ConfigHelper::resolveConfigFile(app_config.logger_config_file()); + sigset_t shutdown_signals; + sigemptyset(&shutdown_signals); + sigaddset(&shutdown_signals, SIGINT); + sigaddset(&shutdown_signals, SIGTERM); + if (pthread_sigmask(SIG_BLOCK, &shutdown_signals, nullptr) != 0) { + CMVR_LOG(ERROR) << "Failed to block shutdown signals"; + cmvr::logging::shutdownLogging(); + return 1; + } + + if (app_config.device_manager_config_file().empty()) { + CMVR_LOG(ERROR) << "DeviceManager config file is empty"; + cmvr::logging::shutdownLogging(); + return 1; + } + logSection("DeviceManager"); + CMVR_LOG(INFO) << "[Startup] Load DeviceManager config: " + << cmvr::ConfigHelper::resolveConfigFile(app_config.device_manager_config_file()); + cmvr::config::DeviceManagerRootConfig device_manager_root; + if (!cmvr::ConfigHelper::loadConfigFile(app_config.device_manager_config_file(), device_manager_root)) { + CMVR_LOG(ERROR) << "Failed to load DeviceManager config: " << app_config.device_manager_config_file(); + cmvr::logging::shutdownLogging(); + return 1; + } + CMVR_LOG(INFO) << "[Startup] Initialize DeviceManager"; + cmvr::device::DeviceManager::getInstance(device_manager_root.device_manager()); + + cmvr::task::registerGrpcServerTaskFactory(); + + if (app_config.task_manager_config_file().empty()) { + CMVR_LOG(ERROR) << "TaskManager config file is empty"; + cmvr::logging::shutdownLogging(); + return 1; + } + logSection("TaskManager"); + CMVR_LOG(INFO) << "[Startup] Load TaskManager config: " + << cmvr::ConfigHelper::resolveConfigFile(app_config.task_manager_config_file()); + cmvr::config::TaskManagerRootConfig task_manager_root; + if (!cmvr::ConfigHelper::loadConfigFile(app_config.task_manager_config_file(), task_manager_root)) { + CMVR_LOG(ERROR) << "Failed to load TaskManager config: " << app_config.task_manager_config_file(); + cmvr::logging::shutdownLogging(); + return 1; + } + + CMVR_LOG(INFO) << "[Startup] Initialize TaskManager"; + auto& task_manager = cmvr::task::TaskManager::getInstance(task_manager_root.task_manager()); + logSection("Start Tasks"); + CMVR_LOG(INFO) << "[Startup] Start tasks"; + task_manager.startRunTask(); + + int received_signal = 0; + if (sigwait(&shutdown_signals, &received_signal) != 0) { + CMVR_LOG(ERROR) << "Failed to wait for shutdown signal"; + task_manager.stopRunTask(); + cmvr::logging::shutdownLogging(); + return 1; + } + + task_manager.stopRunTask(); + cmvr::logging::shutdownLogging(); return 0; } diff --git a/cmvr-es/device_manager/CMakeLists.txt b/cmvr-es/manager/device_manager/CMakeLists.txt similarity index 55% rename from cmvr-es/device_manager/CMakeLists.txt rename to cmvr-es/manager/device_manager/CMakeLists.txt index 0b97b456..d1547b13 100644 --- a/cmvr-es/device_manager/CMakeLists.txt +++ b/cmvr-es/manager/device_manager/CMakeLists.txt @@ -1,22 +1,21 @@ add_library(device_manager STATIC src/device_factory.cpp src/device_manager.cpp - src/system_monitor.cpp ) target_include_directories(device_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(device_manager PRIVATE - cmvr_es::device::uvc_camera -# cmvr_es::device::mechmind - cmvr_es::device::ffmpeg_speaker - cmvr_es::device::ffmpeg_microphone - cmvr_es::device::realsense_camera - cmvr_es::device::rh56dftp_dexhand - cmvr_es::device::px_6ax_gen3 + cmvr_es::proto + cmvr_es::device::camera + cmvr_es::device::agv + cmvr_es::device::speaker + cmvr_es::device::microphone + cmvr_es::device::dexhand cmvr::device::head_esp32 + cmvr_es::device::motor_system + cmvr_es::device::arm osqp - cmvr_es::device::humanoid_robot # cmvr_es::device::aubo_robot ) diff --git a/cmvr-es/manager/device_manager/include/device_factory.h b/cmvr-es/manager/device_manager/include/device_factory.h new file mode 100644 index 00000000..76d3479c --- /dev/null +++ b/cmvr-es/manager/device_manager/include/device_factory.h @@ -0,0 +1,41 @@ +// +// Created by xtkuang on 2025/5/13. +// + +#ifndef CMVR_ES_DEVICE_FACTORY_H +#define CMVR_ES_DEVICE_FACTORY_H +#pragma once + +#include +#include +#include +#include + +#include "../../devices/abstract_device.h" +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" + +namespace cmvr::device { + + struct DeviceRecord { + std::string id; + DeviceKind kind = DeviceKind::Unknown; + std::string type_name; + std::shared_ptr device; + }; + + class DeviceFactory { + public: + using Creator = std::function; + + DeviceFactory(); + + void registerCreator(config::DeviceConfigEntry::DeviceType type, Creator creator); + DeviceRecord create(const config::DeviceConfigEntry& entry) const; + + private: + std::unordered_map creators_; + }; +} + + +#endif //CMVR_ES_DEVICE_FACTORY_H diff --git a/cmvr-es/manager/device_manager/include/device_manager.h b/cmvr-es/manager/device_manager/include/device_manager.h new file mode 100644 index 00000000..f80bc179 --- /dev/null +++ b/cmvr-es/manager/device_manager/include/device_manager.h @@ -0,0 +1,57 @@ +// +// Created by xtkuang on 2025/5/6. +// + +#ifndef CMVR_ES_DEVICE_MANAGER_H +#define CMVR_ES_DEVICE_MANAGER_H + +#include +#include +#include +#include +#include +#include "device_factory.h" +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" + +namespace cmvr::device { + + class DeviceManager { + public: + DeviceManager(const DeviceManager&) = delete; + DeviceManager& operator=(const DeviceManager&) = delete; + + static DeviceManager& getInstance(const config::DeviceManagerConfig &cfg); + static DeviceManager& getInstance(); + static void destroyInstance(); + + void start(); + void restart(); + void stop(); + + void getDeviceList(std::list> &device_list); + void registerDevice(const std::shared_ptr& device); + void registerDevice(const std::string& device_id, const std::shared_ptr& device); + + std::string version() const; + std::string name() const; + std::string description() const; + + template + std::shared_ptr getDevice(const std::string &device_id); + + private: + static std::mutex init_mutex_; + static std::shared_ptr instance_; + + config::DeviceManagerConfig cfg_; + std::unordered_map devices_; + std::unique_ptr dev_factory_; + + explicit DeviceManager(const config::DeviceManagerConfig &cfg); + void log_device_plan_() const; + void pre_scan_robot_arm_dependencies_() const; + void init_devices_(); + }; +} // cmvr + +#endif //CMVR_ES_DEVICE_MANAGER_H diff --git a/cmvr-es/manager/device_manager/src/device_factory.cpp b/cmvr-es/manager/device_manager/src/device_factory.cpp new file mode 100644 index 00000000..ccb5b564 --- /dev/null +++ b/cmvr-es/manager/device_manager/src/device_factory.cpp @@ -0,0 +1,281 @@ +// +// Created by xtkuang on 2025/5/13. +// + +#include "manager/device_manager/include/device_factory.h" + +#include "devices/biohead/biohead_esp32/include/biohead_esp32.h" +#include "devices/agv/agv_factory.h" +#include "devices/arm/robot_arm_factory.h" +#include "devices/camera/camera_factory.h" +#include "devices/dexhand/dexhand_factory.h" +#include "devices/microphone/microphone_factory.h" +#include "devices/motor/motor_system/include/motor_system.h" +#include "devices/speaker/speaker_factory.h" + +#include "common/config/config_files.h" +#include "cmvr/config/biohead_config/biohead_config.pb.h" +#include "cmvr/config/agv_config/agv_config.pb.h" +#include "cmvr/config/arm_config/arm_config.pb.h" +#include "cmvr/config/camera_config/camera_config.pb.h" +#include "cmvr/config/dexhand_config/dexhand_config.pb.h" +#include "cmvr/config/microphone_config/microphone_config.pb.h" +#include "cmvr/config/motor_config/motor_config.pb.h" +#include "cmvr/config/speaker_config/speaker_config.pb.h" +#include "common/base/logging/logger.h" + +using namespace std; +using namespace cmvr::device; +namespace config = cmvr::config; + +namespace { + +template +std::string resolveDeviceId(const config::DeviceConfigEntry& entry, const ConfigType& cfg) +{ + return entry.id().empty() ? cfg.id() : entry.id(); +} + +template +DeviceRecord createDeviceRecordFromConfig(const config::DeviceConfigEntry& entry, const ConfigType& cfg) +{ + const auto id = resolveDeviceId(entry, cfg); + if (!entry.id().empty() && entry.id() != cfg.id()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Device entry id '" << entry.id() + << "' does not match config id '" << cfg.id() << "'"; + return {}; + } + auto device = std::make_shared(cfg); + + DeviceRecord record; + record.id = id; + record.kind = device->kind(); + record.type_name = device->typeName(); + record.device = std::move(device); + return record; +} + +DeviceRecord createRobotArmRecordFromConfig(const config::DeviceConfigEntry& entry, + const config::RobotArmConfig& cfg) +{ + const auto id = resolveDeviceId(entry, cfg); + if (!entry.id().empty() && entry.id() != cfg.id()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Device entry id '" << entry.id() + << "' does not match config id '" << cfg.id() << "'"; + return {}; + } + + auto device = RobotArmFactory::create(cfg); + if (!device) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Failed to create RobotArm: " << id; + return {}; + } + + DeviceRecord record; + record.id = id; + record.kind = device->kind(); + record.type_name = device->typeName(); + record.device = std::move(device); + return record; +} + +template +DeviceRecord createSingleDeviceRecord(const config::DeviceConfigEntry& entry, ConfigGetter get_config) +{ + if (entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for device ID: " << entry.id(); + return {}; + } + + RootConfigType root_cfg; + if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail"; + return {}; + } + const ConfigType& cfg = get_config(root_cfg); + CMVR_LOG(INFO) << "[DeviceFactory]: Read device config success"; + return createDeviceRecordFromConfig(entry, cfg); +} + +template +DeviceRecord createFactoryRecordFromCollection(const config::DeviceConfigEntry& entry, + CollectionGetter get_collection, + CreateDevice create_device) +{ + if (entry.id().empty()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Device entry id is required when loading from collection config"; + return {}; + } + if (entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for device ID: " << entry.id(); + return {}; + } + + RootConfigType root_cfg; + if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail"; + return {}; + } + + const auto& configs = get_collection(root_cfg); + for (const auto& cfg : configs) { + if (cfg.id() == entry.id()) { + CMVR_LOG(INFO) << "[DeviceFactory]: Read device config success"; + auto device = create_device(cfg); + if (!device) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Creator returned nullptr for device ID: " << entry.id(); + return {}; + } + DeviceRecord record; + record.id = cfg.id(); + record.kind = device->kind(); + record.type_name = device->typeName(); + record.device = std::move(device); + return record; + } + } + + CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail"; + return {}; +} + +DeviceRecord createRobotArmRecord(const config::DeviceConfigEntry& entry) +{ + if (entry.id().empty()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: RobotArm id is required"; + return {}; + } + if (entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for RobotArm ID: " << entry.id(); + return {}; + } + + config::ArmRootConfig root_cfg; + if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail"; + return {}; + } + + for (const auto& cfg : root_cfg.arm().robot_arms()) { + if (cfg.id() != entry.id()) { + continue; + } + CMVR_LOG(INFO) << "[DeviceFactory]: Read device config success"; + return createRobotArmRecordFromConfig(entry, cfg); + } + + CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail"; + return {}; +} + +} // namespace + +DeviceFactory::DeviceFactory() +{ + registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_CAMERA, + [](const auto& entry) { + return createFactoryRecordFromCollection( + entry, + [](const auto& root) -> decltype(auto) { + return (root.camera().cameras()); + }, + [](const auto& cfg) { return CameraFactory::create(cfg); }); + }); + + registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_AGV, + [](const auto& entry) { + return createFactoryRecordFromCollection( + entry, + [](const auto& root) -> decltype(auto) { + return (root.agv().agvs()); + }, + [](const auto& cfg) { return AGVFactory::create(cfg); }); + }); + + registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_DEXHAND, + [](const auto& entry) { + return createFactoryRecordFromCollection( + entry, + [](const auto& root) -> decltype(auto) { + return (root.dexhand().dexhands()); + }, + [](const auto& cfg) { return DexHandFactory::create(cfg); }); + }); + + registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MICROPHONE, + [](const auto& entry) { + return createFactoryRecordFromCollection( + entry, + [](const auto& root) -> decltype(auto) { + return (root.microphone().microphones()); + }, + [](const auto& cfg) { return MicrophoneFactory::create(cfg); }); + }); + + registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_SPEAKER, + [](const auto& entry) { + return createFactoryRecordFromCollection( + entry, + [](const auto& root) -> decltype(auto) { + return (root.speaker().speakers()); + }, + [](const auto& cfg) { return SpeakerFactory::create(cfg); }); + }); + + registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM, + [](const auto& entry) { + if (entry.id().empty()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: MotorSystem id is required"; + return DeviceRecord{}; + } + if (entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for MotorSystem ID: " << entry.id(); + return DeviceRecord{}; + } + config::MotorRootConfig root_cfg; + if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail"; + return DeviceRecord{}; + } + CMVR_LOG(INFO) << "[DeviceFactory]: Read device config success"; + auto device = std::make_shared(entry.id(), root_cfg.motor()); + DeviceRecord record; + record.id = entry.id(); + record.kind = device->kind(); + record.type_name = device->typeName(); + record.device = std::move(device); + return record; + }); + + registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM, createRobotArmRecord); + + registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_BIO_HEAD_ROBOT, + [](const auto& entry) { + return createSingleDeviceRecord( + entry, [](const auto& root) -> decltype(auto) { return (root.bio_head()); }); + }); +} + +void DeviceFactory::registerCreator(config::DeviceConfigEntry::DeviceType type, Creator creator) +{ + creators_[static_cast(type)] = std::move(creator); +} + +DeviceRecord DeviceFactory::create(const config::DeviceConfigEntry& entry) const +{ + const auto it = creators_.find(static_cast(entry.type())); + if (it == creators_.end()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Unsupported device type in config entry: " << entry.id(); + return {}; + } + + auto record = it->second(entry); + if (!record.device) { + return {}; + } + if (record.id.empty()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Empty " << record.type_name << " Device ID"; + return {}; + } + return record; +} diff --git a/cmvr-es/manager/device_manager/src/device_manager.cpp b/cmvr-es/manager/device_manager/src/device_manager.cpp new file mode 100644 index 00000000..ef889f6d --- /dev/null +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -0,0 +1,425 @@ +#include "common/base/logging/logger.h" +// +// Created by xtkuang on 2025/5/6. +// + +#include "../include/device_manager.h" + +#include "devices/agv/abstract_agv.h" +#include "devices/arm/robot_arm.h" +#include "devices/battery/abstract_battery.h" +#include "devices/biohead/abstract_biohead.h" +#include "devices/camera/abstract_camera.h" +#include "devices/dexhand/abstract_dexhand.h" +#include "devices/gripper/abstract_gripper.h" +#include "devices/microphone/abstract_microphone.h" +#include "devices/motor/motor_system/include/motor_system.h" +#include "devices/speaker/abstract_speaker.h" +#include "common/config/config_files.h" +#include "cmvr/config/arm_config/arm_config.pb.h" +#include "cmvr/config/motor_config/motor_config.pb.h" + +using namespace std; +using namespace cmvr::device; + +namespace { + +void logSection(const char* title) +{ + CMVR_LOG(INFO) << "---------------- " << title << " ----------------"; +} + +const char* deviceTypeToString(const cmvr::config::DeviceConfigEntry::DeviceType type) +{ + switch (type) { + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_BIO_HEAD_ROBOT: + return "DEVICE_TYPE_BIO_HEAD_ROBOT"; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM: + return "DEVICE_TYPE_MOTOR_SYSTEM"; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM: + return "DEVICE_TYPE_ROBOT_ARM"; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA: + return "DEVICE_TYPE_CAMERA"; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_DEXHAND: + return "DEVICE_TYPE_DEXHAND"; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MICROPHONE: + return "DEVICE_TYPE_MICROPHONE"; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_SPEAKER: + return "DEVICE_TYPE_SPEAKER"; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_AGV: + return "DEVICE_TYPE_AGV"; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN: + default: + return "DEVICE_TYPE_UNKNOWN"; + } +} + +} // namespace + +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); + + +std::shared_ptr DeviceManager::instance_ = nullptr; +std::mutex DeviceManager::init_mutex_; + + +DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) { + cfg_ = cfg; + + dev_factory_ = std::make_unique(); + logSection("Device Plan"); + log_device_plan_(); + pre_scan_robot_arm_dependencies_(); + logSection("Initialize Devices"); + init_devices_(); +} + +DeviceManager& DeviceManager::getInstance(const config::DeviceManagerConfig& cfg) { + std::lock_guard lock(init_mutex_); + if (!instance_) { + instance_.reset(new DeviceManager(cfg)); + } + return *instance_; +} + +DeviceManager& DeviceManager::getInstance() { + std::lock_guard lock(init_mutex_); + if (!instance_) { + CMVR_LOG(FATAL) << "[DeviceManager] (getInstance): DeviceManager not initialized. Call getInstance(const DeviceManagerConfig&) first."; + } + return *instance_; +} + +void DeviceManager::destroyInstance() { + std::lock_guard lock(init_mutex_); + instance_.reset(); + MotorSystem::clearActiveJoints(); +} + +void DeviceManager::start(){ + for (auto& [id, record] : devices_) { + if (!record.device) { + CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id; + continue; + } + if (record.device->start()) { + CMVR_LOG(INFO) << "[DeviceManager]: Start device " << id << " Success"; + } else { + CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id << " Failed"; + } + } +} + +void DeviceManager::restart() { + stop(); + start(); +} + +void DeviceManager::stop() { + for (auto& [id, record] : devices_) { + if (!record.device) { + CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id; + continue; + } + if (record.device->stop()) { + CMVR_LOG(INFO) << "[DeviceManager]: Stop device " << id << " Success"; + } else { + CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id << " Failed"; + } + } +} + +template +std::shared_ptr DeviceManager::getDevice(const std::string& device_id) +{ + auto it = devices_.find(device_id); + if (it == devices_.end()) { + CMVR_LOG(WARNING) << "[DeviceManager]: Device ID " << device_id << " not found."; + return nullptr; + } + + auto ptr = std::dynamic_pointer_cast(it->second.device); + if (!ptr) { + CMVR_LOG(WARNING) << "[DeviceManager]: Device ID " << device_id << " type mismatch."; + return nullptr; + } + return ptr; +} + +void DeviceManager::getDeviceList(std::list>& device_list){ + device_list.clear(); + for (const auto& [device_id, record] : devices_) { + device_list.emplace_back(device_id, record.type_name); + } +} + +void DeviceManager::registerDevice(const std::shared_ptr& device) +{ + if (!device) { + CMVR_LOG(ERROR) << "[DeviceManager]: Cannot register null device"; + return; + } + registerDevice(device->id(), device); +} + +void DeviceManager::registerDevice(const std::string& device_id, + const std::shared_ptr& device) +{ + if (!device) { + CMVR_LOG(ERROR) << "[DeviceManager]: Cannot register null device"; + return; + } + if (device_id.empty()) { + CMVR_LOG(ERROR) << "[DeviceManager]: Cannot register device with empty id"; + return; + } + if (devices_.count(device_id)) { + CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate device ID " << device_id; + return; + } + DeviceRecord record; + record.id = device_id; + record.kind = device->kind(); + record.type_name = device->typeName(); + record.device = device; + devices_.emplace(record.id, std::move(record)); + CMVR_LOG(INFO) << "[DeviceManager]: Register device success" + << ", id=" << device_id + << ", type=" << device->typeName() + << ", kind=" << toString(device->kind()); +} + +std::string DeviceManager::version() const { + return cfg_.version().empty() ? "1.0" : cfg_.version(); +} + +std::string DeviceManager::name() const { + return cfg_.name().empty() ? "cmvr_es" : cfg_.name(); +} + +std::string DeviceManager::description() const { + return cfg_.description().empty() ? "empty" : cfg_.description(); +} + +void DeviceManager::log_device_plan_() const +{ + CMVR_LOG(INFO) << "[DeviceManager]: Device plan begin" + << ", name=" << name() + << ", version=" << version() + << ", total_entries=" << cfg_.devices_size(); + + for (const auto& entry : cfg_.devices()) { + if (!entry.enable()) { + CMVR_LOG(INFO) << "[DeviceManager]: Skip disabled device" + << ", id=" << entry.id() + << ", type=" << deviceTypeToString(entry.type()) + << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file()); + continue; + } + + CMVR_LOG(INFO) << "[DeviceManager]: Will create device" + << ", id=" << entry.id() + << ", type=" << deviceTypeToString(entry.type()) + << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file()); + } + + CMVR_LOG(INFO) << "[DeviceManager]: Device plan end"; +} + +void DeviceManager::pre_scan_robot_arm_dependencies_() const +{ + using GroupJointSelection = std::unordered_map>; + std::unordered_map selections; + std::unordered_map motor_roots; + + for (const auto& entry : cfg_.devices()) { + if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM) { + continue; + } + if (entry.id().empty()) { + CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorSystem device id is empty"; + return; + } + if (entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorSystem config_file is empty: " << entry.id(); + return; + } + + config::MotorRootConfig root_cfg; + if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { + CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load motor config: " << entry.config_file(); + return; + } + if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) { + CMVR_LOG(ERROR) << "[DeviceManager]: MotorSystem entry id '" << entry.id() + << "' does not match config id '" << root_cfg.motor().id() << "'"; + return; + } + motor_roots.emplace(entry.id(), std::move(root_cfg)); + } + + for (const auto& entry : cfg_.devices()) { + if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM) { + continue; + } + if (entry.id().empty()) { + CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm device id is empty"; + return; + } + if (entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm config_file is empty: " << entry.id(); + return; + } + + config::ArmRootConfig root_cfg; + if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { + CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load arm config: " << entry.config_file(); + return; + } + + const config::RobotArmConfig* arm_cfg = nullptr; + for (const auto& candidate : root_cfg.arm().robot_arms()) { + if (candidate.id() == entry.id()) { + arm_cfg = &candidate; + break; + } + } + if (!arm_cfg) { + CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm ID '" << entry.id() + << "' not found in config: " << entry.config_file(); + return; + } + + if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor) { + continue; + } + if (arm_cfg->backend_case() != config::RobotArmConfig::kMotor) { + CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm backend is not configured: " << entry.id(); + return; + } + + const auto& motor_config = arm_cfg->motor(); + if (motor_config.motor_system_id().empty()) { + CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_system_id: " << entry.id(); + return; + } + if (motor_config.motor_group_ids_size() == 0) { + CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_group_ids: " << entry.id(); + return; + } + if (motor_config.joint_names_size() == 0) { + CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing joint_names: " << entry.id(); + return; + } + + const auto motor_root_it = motor_roots.find(motor_config.motor_system_id()); + if (motor_root_it == motor_roots.end()) { + CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id() + << "' depends on disabled or missing MotorSystem: " + << motor_config.motor_system_id(); + return; + } + + std::unordered_set allowed_groups; + allowed_groups.reserve(static_cast(motor_config.motor_group_ids_size())); + for (const auto& group_id : motor_config.motor_group_ids()) { + if (group_id.empty()) { + CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty motor_group_id: " << entry.id(); + return; + } + allowed_groups.insert(group_id); + } + + auto& group_selection = selections[motor_config.motor_system_id()]; + for (const auto& joint_name : motor_config.joint_names()) { + if (joint_name.empty()) { + CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty joint_name: " << entry.id(); + return; + } + + std::string matched_group; + for (const auto& motor_group : motor_root_it->second.motor().motor_groups()) { + const auto& group_id = motor_group.id(); + if (allowed_groups.count(group_id) == 0) { + continue; + } + for (const auto& motor : motor_group.motors()) { + if (motor.joint_name() == joint_name) { + matched_group = group_id; + break; + } + } + if (!matched_group.empty()) { + break; + } + } + + if (matched_group.empty()) { + CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id() + << "' joint '" << joint_name + << "' not found in configured motor_group_ids"; + return; + } + group_selection[matched_group].insert(joint_name); + } + } + + MotorSystem::clearActiveJoints(); + for (auto& [motor_system_id, group_selection] : selections) { + MotorSystem::setActiveJoints(motor_system_id, std::move(group_selection)); + } +} + +void DeviceManager::init_devices_() { + for (const auto& entry : cfg_.devices()) { + if (!entry.enable()) { + continue; + } + + CMVR_LOG(INFO) << "[DeviceManager]: Initialize device begin" + << ", id=" << entry.id() + << ", type=" << deviceTypeToString(entry.type()) + << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file()); + + DeviceRecord record = dev_factory_->create(entry); + if (!record.device || record.id.empty()) { + CMVR_LOG(ERROR) << "[DeviceManager]: Failed to create device for entry id=" << entry.id(); + continue; + } + CMVR_LOG(INFO) << "[DeviceManager]: Create device object success" + << ", id=" << record.id + << ", type=" << record.type_name + << ", kind=" << toString(record.kind); + if (devices_.count(record.id)) { + CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate " << record.type_name << " Device ID " << record.id; + continue; + } + CMVR_LOG(INFO) << "[DeviceManager]: Init device object begin" + << ", id=" << record.id + << ", type=" << record.type_name + << ", kind=" << toString(record.kind); + if (!record.device->init()) { + CMVR_LOG(ERROR) << "[DeviceManager]: Init device object failed" + << ", id=" << record.id + << ", type=" << record.type_name + << ", kind=" << toString(record.kind) + << ", config_file=" << entry.config_file(); + continue; + } + CMVR_LOG(INFO) << "[DeviceManager]: Init device object success" + << ", id=" << record.id + << ", type=" << record.type_name + << ", kind=" << toString(record.kind) + << ", config_file=" << entry.config_file(); + devices_.emplace(record.id, std::move(record)); + } +} diff --git a/cmvr-es/manager/task_manager/CMakeLists.txt b/cmvr-es/manager/task_manager/CMakeLists.txt new file mode 100644 index 00000000..40b1eb48 --- /dev/null +++ b/cmvr-es/manager/task_manager/CMakeLists.txt @@ -0,0 +1,17 @@ +add_library(task_manager STATIC + src/task_manager.cpp +) + +target_include_directories(task_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(task_manager + PUBLIC + cmvr_es::proto + cmvr_es::task + PRIVATE + cmvr_es::common + cmvr_es::device_manager +) + +add_library(cmvr_es::task_manager ALIAS task_manager) +install(TARGETS task_manager LIBRARY DESTINATION lib) diff --git a/cmvr-es/manager/task_manager/include/task_manager.h b/cmvr-es/manager/task_manager/include/task_manager.h new file mode 100644 index 00000000..b1d0609f --- /dev/null +++ b/cmvr-es/manager/task_manager/include/task_manager.h @@ -0,0 +1,58 @@ +#ifndef CMVR_ES_TASK_MANAGER_H +#define CMVR_ES_TASK_MANAGER_H + +#include +#include +#include +#include +#include +#include +#include + +#include "task/task.h" +#include "task/touch_screen_task/include/touch_screen_task.h" +#include "cmvr/config/task_manager_config/task_manager_config.pb.h" + +namespace cmvr::task { + + class TaskManager { + public: + TaskManager(const TaskManager&) = delete; + TaskManager& operator=(const TaskManager&) = delete; + + static TaskManager& getInstance(const config::TaskManagerConfig& cfg); + static TaskManager& getInstance(); + static void destroyInstance(); + + ~TaskManager(); + + void startRunTask(double control_period_s = 0.001); + void stopRunTask(); + bool running() const { return running_.load(); } + + std::shared_ptr getTask(const std::string& task_id) const; + std::shared_ptr getTouchScreenTask(const std::string& task_id = "touch_screen") const; + + private: + explicit TaskManager(const config::TaskManagerConfig& cfg); + + void logTaskPlan() const; + void initTasks(); + void runTaskLoop(double control_period_s); + static TaskRunMode toTaskRunMode(config::TaskConfigEntry::TaskRunMode run_mode); + + static std::mutex init_mutex_; + static std::shared_ptr instance_; + + config::TaskManagerConfig cfg_; + std::unordered_map> tasks_; + std::unordered_map task_period_s_; + std::unordered_map next_step_time_; + mutable std::mutex tasks_mutex_; + std::atomic running_{false}; + std::thread run_thread_; + }; + +} // namespace cmvr::task + +#endif // CMVR_ES_TASK_MANAGER_H diff --git a/cmvr-es/manager/task_manager/src/task_manager.cpp b/cmvr-es/manager/task_manager/src/task_manager.cpp new file mode 100644 index 00000000..05410231 --- /dev/null +++ b/cmvr-es/manager/task_manager/src/task_manager.cpp @@ -0,0 +1,369 @@ +#include "manager/task_manager/include/task_manager.h" + +#include +#include +#include +#include +#include + +#include "common/base/logging/logger.h" +#include "common/config/config_files.h" +#include "task/task_factory.h" + +using namespace cmvr; +using namespace cmvr::task; + +namespace { + +void logSection(const char* title) +{ + CMVR_LOG(INFO) << "---------------- " << title << " ----------------"; +} + +const char* taskRunModeToString(const TaskRunMode mode) +{ + switch (mode) { + case TaskRunMode::PERIODIC_STEP: return "PERIODIC_STEP"; + case TaskRunMode::BLOCKING_SERVICE: return "BLOCKING_SERVICE"; + } + return "UNKNOWN"; +} + +const char* taskConfigTypeToString(const config::TaskConfigEntry::TaskType type) +{ + switch (type) { + case config::TaskConfigEntry::TASK_TYPE_TOUCH_SCREEN: + return "TASK_TYPE_TOUCH_SCREEN"; + case config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER: + return "TASK_TYPE_GRPC_SERVER"; + case config::TaskConfigEntry::TASK_TYPE_UNKNOWN: + default: + return "TASK_TYPE_UNKNOWN"; + } +} + +const char* taskConfigRunModeToString(const config::TaskConfigEntry::TaskRunMode run_mode) +{ + switch (run_mode) { + case config::TaskConfigEntry::TASK_RUN_MODE_PERIODIC_STEP: + return "TASK_RUN_MODE_PERIODIC_STEP"; + case config::TaskConfigEntry::TASK_RUN_MODE_BLOCKING_SERVICE: + return "TASK_RUN_MODE_BLOCKING_SERVICE"; + case config::TaskConfigEntry::TASK_RUN_MODE_UNKNOWN: + default: + return "TASK_RUN_MODE_UNKNOWN"; + } +} + +} // namespace + +std::shared_ptr TaskManager::instance_ = nullptr; +std::mutex TaskManager::init_mutex_; + +TaskManager::TaskManager(const config::TaskManagerConfig& cfg) + : cfg_(cfg) +{ + logSection("Task Plan"); + logTaskPlan(); + logSection("Initialize Tasks"); + initTasks(); +} + +TaskManager::~TaskManager() +{ + stopRunTask(); +} + +TaskManager& TaskManager::getInstance(const config::TaskManagerConfig& cfg) +{ + std::lock_guard lock(init_mutex_); + if (!instance_) { + instance_.reset(new TaskManager(cfg)); + } + return *instance_; +} + +TaskManager& TaskManager::getInstance() +{ + std::lock_guard lock(init_mutex_); + if (!instance_) { + CMVR_LOG(FATAL) << "[TaskManager] not initialized. Call getInstance(const TaskManagerConfig&) first."; + } + return *instance_; +} + +void TaskManager::destroyInstance() +{ + std::lock_guard lock(init_mutex_); + if (instance_) { + instance_->stopRunTask(); + } + instance_.reset(); +} + +std::shared_ptr TaskManager::getTouchScreenTask(const std::string& task_id) const +{ + auto task = std::dynamic_pointer_cast(getTask(task_id)); + if (!task) { + CMVR_LOG(ERROR) << "[TaskManager] Task is not a TouchScreen task: " << task_id; + return nullptr; + } + return task; +} + +std::shared_ptr TaskManager::getTask(const std::string& task_id) const +{ + std::lock_guard lock(tasks_mutex_); + const auto it = tasks_.find(task_id); + if (it == tasks_.end()) { + CMVR_LOG(ERROR) << "[TaskManager] task not found: " << task_id; + return nullptr; + } + return it->second; +} + +void TaskManager::startRunTask(const double control_period_s) +{ + if (!std::isfinite(control_period_s) || control_period_s <= 0.0) { + CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s"; + return; + } + + bool expected = false; + if (!running_.compare_exchange_strong(expected, true)) { + return; + } + + std::vector> tasks; + { + std::lock_guard lock(tasks_mutex_); + tasks.reserve(tasks_.size()); + for (const auto& [id, task] : tasks_) { + if (task) { + tasks.push_back(task); + } + } + } + + std::vector> started_tasks; + for (const auto& task : tasks) { + if (!task->start()) { + CMVR_LOG(ERROR) << "[TaskManager] task start failed: " << task->id(); + for (const auto& started_task : started_tasks) { + try { + started_task->stop(); + } catch (...) { + } + } + running_.store(false); + return; + } + started_tasks.push_back(task); + } + + try { + run_thread_ = std::thread(&TaskManager::runTaskLoop, this, control_period_s); + } catch (const std::exception& e) { + CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread: " << e.what(); + running_.store(false); + for (const auto& task : started_tasks) { + try { + task->stop(); + } catch (...) { + } + } + return; + } +} + +void TaskManager::stopRunTask() +{ + bool expected = true; + if (!running_.compare_exchange_strong(expected, false)) { + return; + } + + if (run_thread_.joinable()) { + run_thread_.join(); + } + + std::vector> tasks; + { + std::lock_guard lock(tasks_mutex_); + tasks.reserve(tasks_.size()); + for (const auto& [id, task] : tasks_) { + if (task) { + tasks.push_back(task); + } + } + } + for (const auto& task : tasks) { + try { + task->stop(); + } catch (...) { + } + } +} + +void TaskManager::initTasks() +{ + for (const auto& entry : cfg_.tasks()) { + if (!entry.enable()) { + continue; + } + if (entry.id().empty()) { + CMVR_LOG(ERROR) << "[TaskManager] Task ID is empty"; + continue; + } + + CMVR_LOG(INFO) << "[TaskManager] Initialize task begin" + << ", id=" << entry.id() + << ", type=" << taskConfigTypeToString(entry.type()) + << ", run_mode=" << taskConfigRunModeToString(entry.run_mode()) + << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file()); + + auto task = TaskFactory::create(entry); + if (!task || task->id() != entry.id()) { + CMVR_LOG(ERROR) << "[TaskManager] Task ID mismatch: " << entry.id(); + continue; + } + const TaskRunMode configured_run_mode = toTaskRunMode(entry.run_mode()); + if (configured_run_mode != task->runMode()) { + CMVR_LOG(ERROR) << "[TaskManager] Task run_mode mismatch: id=" << entry.id() + << ", configured=" << taskRunModeToString(configured_run_mode) + << ", actual=" << taskRunModeToString(task->runMode()); + continue; + } + double control_period_s = entry.control_period_s(); + if (configured_run_mode == TaskRunMode::PERIODIC_STEP && + (!std::isfinite(control_period_s) || control_period_s <= 0.0)) { + CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s for task: " << entry.id(); + continue; + } + if (!task->init()) { + CMVR_LOG(ERROR) << "[TaskManager] Task init failed: " << entry.id() + << ", status=" << task->detailStatusString(); + continue; + } + { + std::lock_guard lock(tasks_mutex_); + if (tasks_.count(entry.id())) { + CMVR_LOG(ERROR) << "[TaskManager] Duplicate task ID: " << entry.id(); + continue; + } + if (configured_run_mode == TaskRunMode::PERIODIC_STEP) { + task_period_s_[entry.id()] = control_period_s; + } + tasks_.emplace(entry.id(), std::move(task)); + } + } +} + +void TaskManager::logTaskPlan() const +{ + CMVR_LOG(INFO) << "[TaskManager] Task plan begin" + << ", total_entries=" << cfg_.tasks_size(); + + for (const auto& entry : cfg_.tasks()) { + if (!entry.enable()) { + CMVR_LOG(INFO) << "[TaskManager] Skip disabled task" + << ", id=" << entry.id() + << ", type=" << taskConfigTypeToString(entry.type()) + << ", run_mode=" << taskConfigRunModeToString(entry.run_mode()) + << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file()); + continue; + } + + CMVR_LOG(INFO) << "[TaskManager] Will create task" + << ", id=" << entry.id() + << ", type=" << taskConfigTypeToString(entry.type()) + << ", run_mode=" << taskConfigRunModeToString(entry.run_mode()) + << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file()); + } + + CMVR_LOG(INFO) << "[TaskManager] Task plan end"; +} + +void TaskManager::runTaskLoop(const double control_period_s) +{ + while (running_.load()) { + std::vector, double>> periodic_tasks; + { + std::lock_guard lock(tasks_mutex_); + periodic_tasks.reserve(tasks_.size()); + for (const auto& [id, task] : tasks_) { + if (!task || task->runMode() != TaskRunMode::PERIODIC_STEP) { + continue; + } + const auto period_it = task_period_s_.find(id); + const double period_s = period_it == task_period_s_.end() + ? control_period_s + : period_it->second; + periodic_tasks.emplace_back(task, period_s); + } + } + + const auto now = std::chrono::steady_clock::now(); + auto sleep_until_time = now + std::chrono::duration_cast( + std::chrono::duration(control_period_s)); + + for (const auto& [task, period_s] : periodic_tasks) { + if (!running_.load()) { + break; + } + auto should_step = false; + auto task_next_time = now; + { + std::lock_guard lock(tasks_mutex_); + auto& next_time = next_step_time_[task->id()]; + if (next_time.time_since_epoch().count() == 0) { + next_time = now; + } + if (now >= next_time) { + should_step = true; + next_time += std::chrono::duration_cast( + std::chrono::duration(period_s)); + if (next_time < now) { + next_time = now + std::chrono::duration_cast( + std::chrono::duration(period_s)); + } + } + task_next_time = next_time; + } + + if (!should_step || task->state() != TaskState::RUNNING) { + if (task_next_time < sleep_until_time) { + sleep_until_time = task_next_time; + } + continue; + } + if (!task->step(period_s)) { + CMVR_LOG(WARNING) << "[TaskManager] task step returned false, status=" + << task->detailStatusString(); + } + if (task_next_time < sleep_until_time) { + sleep_until_time = task_next_time; + } + } + + if (sleep_until_time <= now) { + sleep_until_time = now + std::chrono::duration_cast( + std::chrono::duration(control_period_s)); + } + std::this_thread::sleep_until(sleep_until_time); + } +} + +TaskRunMode TaskManager::toTaskRunMode(const config::TaskConfigEntry::TaskRunMode run_mode) +{ + switch (run_mode) { + case config::TaskConfigEntry::TASK_RUN_MODE_PERIODIC_STEP: + return TaskRunMode::PERIODIC_STEP; + case config::TaskConfigEntry::TASK_RUN_MODE_BLOCKING_SERVICE: + return TaskRunMode::BLOCKING_SERVICE; + case config::TaskConfigEntry::TASK_RUN_MODE_UNKNOWN: + default: + CMVR_LOG(ERROR) << "[TaskManager] TASK_RUN_MODE_UNKNOWN is invalid"; + return TaskRunMode::PERIODIC_STEP; + } +} diff --git a/cmvr-es/monitor/CMakeLists.txt b/cmvr-es/monitor/CMakeLists.txt deleted file mode 100644 index 47820768..00000000 --- a/cmvr-es/monitor/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -# 首先添加子目录，确保disk_monitor目标被定义 -add_subdirectory(diskmonitor) \ No newline at end of file diff --git a/cmvr-es/monitor/abstract_monitor.h b/cmvr-es/monitor/abstract_monitor.h deleted file mode 100644 index 6b707e9c..00000000 --- a/cmvr-es/monitor/abstract_monitor.h +++ /dev/null @@ -1,42 +0,0 @@ -// -// Created by xtkuang on 2025/6/6. -// - -#ifndef ABSTRACT_MONITOR_H -#define ABSTRACT_MONITOR_H - -#include -#include -#include "rapidxml/xml_parser.h" -#include "../utils/base/include/timer.h" - -namespace cmvr::monitor{ - class AbstractMonitor { - public: - explicit AbstractMonitor(const XmlNode &cfg){ - cfg_ = cfg; - freq_ = cfg_.getAttrDefault("freq", 1); - timer_ = std::make_unique(); - timer_->start(std::chrono::milliseconds(1000/freq_), [this](){run();}); - } - - virtual ~AbstractMonitor() {timer_->stop();} - - void run() { - if (!check_()) { - execution_(); - } - } - - protected: - XmlNode cfg_; - int freq_; - std::unique_ptr timer_; - - virtual bool check_() = 0; - virtual bool execution_() = 0; - }; - -} - -#endif //ABSTRACT_MONITOR_H \ No newline at end of file diff --git a/cmvr-es/monitor/diskmonitor/CMakeLists.txt b/cmvr-es/monitor/diskmonitor/CMakeLists.txt deleted file mode 100644 index 34c44ff8..00000000 --- a/cmvr-es/monitor/diskmonitor/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -add_library(disk_monitor SHARED src/disk_monitor.cpp) - -target_include_directories(disk_monitor PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -target_link_libraries(disk_monitor PRIVATE cmvr_es::utils) - -add_library(cmvr_es::monitor::disk_monitor ALIAS disk_monitor) - -install(TARGETS disk_monitor LIBRARY DESTINATION lib) \ No newline at end of file diff --git a/cmvr-es/monitor/diskmonitor/include/disk_monitor.h b/cmvr-es/monitor/diskmonitor/include/disk_monitor.h deleted file mode 100644 index 89664362..00000000 --- a/cmvr-es/monitor/diskmonitor/include/disk_monitor.h +++ /dev/null @@ -1,32 +0,0 @@ -// -// Created by linbo on 2025/6/18. -// - -#ifndef FOLDER_MONITOR_H -#define FOLDER_MONITOR_H - -#include - -#include "../../abstract_monitor.h" - -namespace cmvr::monitor { - - class DiskMonitor final : public AbstractMonitor{ - public: - explicit DiskMonitor(const XmlNode& config); - ~DiskMonitor(); - protected: - bool check_() override; - bool execution_() override; - private: - std::unordered_map folders_; - std::vector dirsToClean_; // 需要清理的目录列表 - mutable std::mutex mutex_; // 保护共享资源的互斥锁 - - bool is_initialized_ = false; - }; -} - - - -#endif //FOLDER_MONITOR_H \ No newline at end of file diff --git a/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp b/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp deleted file mode 100644 index 20630615..00000000 --- a/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp +++ /dev/null @@ -1,188 +0,0 @@ -// -// Created by linbo on 2025/6/18. -// - -#include "../include/disk_monitor.h" -#include -#include -#include -#include -#include - -using namespace std; -using namespace cmvr::monitor; -namespace fs = std::filesystem; -using namespace std::chrono; - - -// 文件信息结构体 -struct FileInfo { - std::string path; - time_t creationTime; -}; - -// 比较函数，用于按创建时间排序 -bool compareByCreationTime(const FileInfo& a, const FileInfo& b) { - return a.creationTime < b.creationTime; -} - -// 删除指定目录下创建时间最早的文件 -bool deleteOldestFile(const std::string& directoryPath) { - DIR* dir = opendir(directoryPath.c_str()); - if (!dir) { - std::cerr << "Failed to open directory: " << directoryPath << std::endl; - return false; - } - - std::vector files; - struct dirent* entry; - struct stat fileStat; - - // 遍历目录中的每个条目 - while ((entry = readdir(dir)) != nullptr) { - // 跳过 "." 和 ".." - if (std::string(entry->d_name) == "." || std::string(entry->d_name) == "..") { - continue; - } - - // 构建完整路径 - std::string filePath = directoryPath + "/" + entry->d_name; - - // 获取文件状态信息 - if (stat(filePath.c_str(), &fileStat) == -1) { - std::cerr << "Failed to get file status: " << filePath << std::endl; - continue; - } - - // 只处理普通文件 - if (S_ISREG(fileStat.st_mode)) { - FileInfo fileInfo; - fileInfo.path = filePath; - - // 优先使用 st_birthtime (如果可用)，否则使用 st_ctime - #ifdef HAVE_STRUCT_STAT_ST_BIRTHTIME - fileInfo.creationTime = fileStat.st_birthtime; - #else - fileInfo.creationTime = fileStat.st_ctime; - #endif - - files.push_back(fileInfo); - } - } - - closedir(dir); - - // 如果没有找到文件，返回失败 - if (files.empty()) { - std::cout << "No files found in directory: " << directoryPath << std::endl; - return false; - } - - // 按创建时间排序 - std::sort(files.begin(), files.end(), compareByCreationTime); - - // 删除最早创建的文件 - std::string oldestFilePath = files[0].path; - if (unlink(oldestFilePath.c_str()) == -1) { - std::cerr << "Failed to delete file: " << oldestFilePath << std::endl; - return false; - } - - std::cout << "Deleted oldest file: " << oldestFilePath << std::endl; - return true; -} - - -bool pathExists(const std::string& path) { - struct stat buffer; - return (stat(path.c_str(), &buffer) == 0); -} -int countFilesInDirectory(const std::string& path) { - DIR* dir = opendir(path.c_str()); - if (!dir) { - std::cerr << "Failed to open directory: " << path << std::endl; - return -1; - } - - int count = 0; - struct dirent* entry; - - // 遍历目录中的每个条目 - while ((entry = readdir(dir)) != nullptr) { - // 跳过 "." 和 ".." - if (std::string(entry->d_name) != "." && std::string(entry->d_name) != "..") { - count++; - } - } - - closedir(dir); - return count; -} - -DiskMonitor::DiskMonitor(const XmlNode& config) : AbstractMonitor(config){ - - try { - for (auto &node: config.getChildren()){ - string fileDir = node.getAttrString("fileDir"); - if (!pathExists(fileDir)) { - throw runtime_error("[DiskMonitor] (DiskMonitor): path not exists: " + fileDir); - } - int maxVolume_ = node.getAttrDefault("maxVolume", 1000); - folders_[fileDir] = maxVolume_; - } - is_initialized_ = true; - LOG(INFO) << "[DiskMonitor]: Init DiskMonitor Success"; - } - catch (const exception& e) { - LOG(ERROR) <<"[DiskMonitor] (DiskMonitor): Failed " << e.what(); - throw runtime_error("[DiskMonitor] (DiskMonitor): Failed "+ string(e.what())); - } -} -DiskMonitor::~DiskMonitor() { - folders_.clear(); -} -bool DiskMonitor::check_() { - // 检查当前目录下的文件数量是否超过最大值 - bool allOk = true; - // 遍历所有监控的文件夹 - for (const auto& folder : folders_) { - const std::string& path = folder.first; - size_t maxFiles = folder.second; - try { - if (!pathExists(path)) { - std::cout << "Path not exists\n"; - continue; - } - int nFileCount = countFilesInDirectory(path); - if (nFileCount > maxFiles) - { - allOk = false; - dirsToClean_.push_back(path); - std::cout << "path need remove oldest file: " << path << std::endl; - } - - - } catch (const fs::filesystem_error& e) { - std::cerr << "Filesystem error: " << e.what() << '\n'; - } catch (const std::exception& e) { - std::cerr << "Unexpected error: " << e.what() << '\n'; - } - } - - return allOk; -} - - -bool DiskMonitor::execution_() { - - // 删除创建时间最早的文件 - std::lock_guard lock(mutex_); - bool success = true; - - for (const auto& path : dirsToClean_) { - deleteOldestFile(path); - } - - dirsToClean_.clear(); // 清理完成后清空列表 - return success; -} \ No newline at end of file diff --git a/cmvr-es/monitor_manager/CMakeLists.txt b/cmvr-es/monitor_manager/CMakeLists.txt deleted file mode 100644 index fdff2289..00000000 --- a/cmvr-es/monitor_manager/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -# 然后定义monitor静态库并链接依赖 -add_library(monitor_manager STATIC - src/monitor_factory.cpp - src/monitor_manager.cpp -) - -target_include_directories(monitor_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -target_link_libraries(monitor_manager PRIVATE - cmvr_es::monitor::disk_monitor -) - -add_library(cmvr_es::monitor_manager ALIAS monitor_manager) - -install(TARGETS monitor_manager LIBRARY DESTINATION lib) \ No newline at end of file diff --git a/cmvr-es/monitor_manager/include/monitor_factory.h b/cmvr-es/monitor_manager/include/monitor_factory.h deleted file mode 100644 index a0dbc900..00000000 --- a/cmvr-es/monitor_manager/include/monitor_factory.h +++ /dev/null @@ -1,27 +0,0 @@ -// -// Created by xtkuang on 2025/6/6. -// - -#ifndef MONITOR_FACTORY_H -#define MONITOR_FACTORY_H - - -#include "../../monitor/abstract_monitor.h" - -namespace cmvr::monitor -{ - class MonitorFactory - { - public: - MonitorFactory() = default; - template - std::shared_ptr create(const XmlNode& cfg); - std::shared_ptr create(const XmlNode& cfg); - }; - - -} - - - -#endif //MONITOR_FACTORY_H \ No newline at end of file diff --git a/cmvr-es/monitor_manager/include/monitor_manager.h b/cmvr-es/monitor_manager/include/monitor_manager.h deleted file mode 100644 index 8c53a401..00000000 --- a/cmvr-es/monitor_manager/include/monitor_manager.h +++ /dev/null @@ -1,37 +0,0 @@ -// -// Created by xtkuang on 2025/6/9. -// - -#ifndef MONITOR_MANAGER_H -#define MONITOR_MANAGER_H - -#include -#include "rapidxml/xml_parser.h" -#include "monitor_factory.h" - -namespace cmvr::monitor { - - class MonitorManager { - public: - MonitorManager(const MonitorManager&) = delete; - MonitorManager& operator=(const MonitorManager&) = delete; - static MonitorManager& getInstance(const XmlNode &cfg); - static MonitorManager& getInstance(); - static void destroyInstance(); - - private: - explicit MonitorManager (const XmlNode &cfg); - void init_monitors(); - private: - static std::once_flag init_flag_; - static std::shared_ptr instance_; - - XmlNode cfg_; - std::shared_ptr monitor_factory_; - std::unordered_map> monitors_; - }; -} - - - -#endif //MONITOR_MANAGER_H \ No newline at end of file diff --git a/cmvr-es/monitor_manager/src/monitor_factory.cpp b/cmvr-es/monitor_manager/src/monitor_factory.cpp deleted file mode 100644 index 821373e2..00000000 --- a/cmvr-es/monitor_manager/src/monitor_factory.cpp +++ /dev/null @@ -1,24 +0,0 @@ -// -// Created by xtkuang on 2025/6/6. -// - -#include "../include/monitor_factory.h" - -#include "../../monitor/diskmonitor/include/disk_monitor.h" - -using namespace std; -using namespace cmvr::monitor; - -template std::shared_ptr MonitorFactory::create(const XmlNode&); - -template -std::shared_ptr MonitorFactory::create(const XmlNode& cfg) { - if constexpr (std::is_same_v) { - return std::make_shared(cfg); - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type"; - throw runtime_error("[DeviceFactory]: Unsupported device type"); - } -} - diff --git a/cmvr-es/monitor_manager/src/monitor_manager.cpp b/cmvr-es/monitor_manager/src/monitor_manager.cpp deleted file mode 100644 index 2dd9e2cf..00000000 --- a/cmvr-es/monitor_manager/src/monitor_manager.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// Created by xtkuang on 2025/6/9. -// - -#include "../include/monitor_manager.h" - -#include "../../monitor/diskmonitor/include/disk_monitor.h" - -using namespace std; -using namespace cmvr::monitor; - -std::shared_ptr MonitorManager::instance_ = nullptr; -std::once_flag MonitorManager::init_flag_; - -MonitorManager::MonitorManager(const XmlNode& cfg) { - monitor_factory_ = make_shared(); - cfg_ = cfg; - init_monitors(); -} -MonitorManager& MonitorManager::getInstance(const XmlNode& cfg) { - std::call_once(init_flag_, [&cfg] { - instance_.reset(new MonitorManager(cfg)); - }); - return *instance_; -} - -MonitorManager& MonitorManager::getInstance() { - if (!instance_) { - throw std::runtime_error("[MonitorManager] (getInstance): MonitorManager not initialized. Call getInstance(const XmlNode&) first."); - } - return *instance_; -} - -void MonitorManager::destroyInstance() { - instance_.reset(); -} - -void MonitorManager::init_monitors() { - try{ - auto diskmonitor_node = cfg_.getChild("DiskMonitor"); - auto id = diskmonitor_node.getAttrString("id"); - auto monitor = monitor_factory_->create(diskmonitor_node); - if (monitor == nullptr) { - throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); - } - monitors_[id] = monitor; - } - catch (const exception& e) { - LOG(ERROR) << e.what(); - throw runtime_error(e.what()); - } - -} \ No newline at end of file diff --git a/cmvr-es/planner/CMakeLists.txt b/cmvr-es/planner/CMakeLists.txt deleted file mode 100644 index 0e6384b3..00000000 --- a/cmvr-es/planner/CMakeLists.txt +++ /dev/null @@ -1,68 +0,0 @@ - - -add_library(planner SHARED - joint_space_planner/src/joint_space_planner.cpp - joint_space_planner/src/joint_space_planner_creator.cpp - joint_space_planner/src/toppra_bspline.cpp - - s_curve_planner/src/s_curve_position_planner.cpp - s_curve_planner/src/s_curve_velocity_planner.cpp - cartesian_space_planner/src/cartesian_twist_limiter.cpp -) - -target_include_directories(planner PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -target_link_libraries(planner PUBLIC - OsqpEigen - tinyxml2 - fcl - toppra - cmvr_es::common -) - -add_library(cmvr_es::planner ALIAS planner) -install(TARGETS planner LIBRARY DESTINATION lib) - - -# -------------------------------------------------------- -# Unit test -# -------------------------------------------------------- - - - - -add_executable(joint_space_planner_test - ${CMAKE_CURRENT_SOURCE_DIR}/joint_space_planner/src/joint_space_planner_test.cpp -) - - -target_link_libraries(joint_space_planner_test - PRIVATE - cmvr_es::utils - cmvr_es::data_center - cmvr_es::planner - gtest - gtest_main - pthread - glog - cmvr_es::proto - ccd - fcl - OsqpEigen - -) - -add_executable(cartesian_twist_limiter_test - ${CMAKE_CURRENT_SOURCE_DIR}/cartesian_space_planner/src/cartesian_twist_limiter_test.cpp -) - -target_link_libraries(cartesian_twist_limiter_test - PRIVATE - cmvr_es::planner - gtest - gtest_main - pthread - glog - cmvr_es::proto - matplot -) diff --git a/cmvr-es/planner/joint_space_planner/include/joint_space_planner_creator.h b/cmvr-es/planner/joint_space_planner/include/joint_space_planner_creator.h deleted file mode 100644 index 17c0185a..00000000 --- a/cmvr-es/planner/joint_space_planner/include/joint_space_planner_creator.h +++ /dev/null @@ -1,32 +0,0 @@ -// -// Created by lgv on 11/7/25. -// - -#pragma once - -#include -#include -#include - -namespace cmvr -{ - enum JointSpacePlannerType - { - JOINT_SPACE_PLANNER_UNKNOWN = 0, - TOPPRA_BSPLINE = 1 - }; - class JointSpacePlannerCreator - { - protected: - static std::unordered_map> - registers_; - static std::shared_ptr createNew(const JointSpacePlannerType type); - - public: - static void clear() - { - registers_.clear(); - } - static std::shared_ptr create(const JointSpacePlannerType type); - }; -} // namespace parking \ No newline at end of file diff --git a/cmvr-es/planner/joint_space_planner/src/joint_space_planner.cpp b/cmvr-es/planner/joint_space_planner/src/joint_space_planner.cpp deleted file mode 100644 index c76041e2..00000000 --- a/cmvr-es/planner/joint_space_planner/src/joint_space_planner.cpp +++ /dev/null @@ -1,18 +0,0 @@ -// -// Created by lgv on 11/7/25. -// -#include "planner/joint_space_planner/include/joint_space_planner.h" -using namespace cmvr; - -void JointSpacePlanner::setSymmetricLimits(const std::vector &v_max, - const std::vector &a_max) { - v_max_ = v_max; - a_max_ = a_max; -} - -void JointSpacePlanner::setGridSizes(int N, int N_high) { - N_grid_ = N; - N_grid_high_ = N_high; -} - -void JointSpacePlanner::setPathType(PathType p) { path_type_ = p; } \ No newline at end of file diff --git a/cmvr-es/planner/joint_space_planner/src/joint_space_planner_creator.cpp b/cmvr-es/planner/joint_space_planner/src/joint_space_planner_creator.cpp deleted file mode 100644 index f03ea777..00000000 --- a/cmvr-es/planner/joint_space_planner/src/joint_space_planner_creator.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// -// Created by lgv on 11/7/25. -// - -#include "joint_space_planner/include/joint_space_planner_creator.h" -#include "planner/joint_space_planner/include/toppra_bspline.h" - -namespace cmvr -{ - std::unordered_map> - JointSpacePlannerCreator::registers_; - - std::shared_ptr JointSpacePlannerCreator::create(const JointSpacePlannerType type) - { - static std::unordered_map> - registered_planners; - auto planner_pair = registered_planners.find(type); - if (planner_pair == registered_planners.end()) - { - auto res = createNew(type); - registered_planners.emplace(type, res); - return res; - } - return planner_pair->second; - } - std::shared_ptr - JointSpacePlannerCreator::createNew(const JointSpacePlannerType type) - { - std::shared_ptr result; - switch (type) - { - case JOINT_SPACE_PLANNER_UNKNOWN: - return nullptr; - case TOPPRA_BSPLINE: - return std::make_shared(); - default: - return nullptr; - } - } -} \ No newline at end of file diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 4ef50063..a33da009 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -6,9 +6,9 @@ add_library(service grpc/src/grpc_microphone_service.cpp grpc/src/grpc_head_service.cpp grpc/src/grpc_dexhand_service.cpp - grpc/src/grpc_humanoid_robot_service.cpp + grpc/src/grpc_arm_service.cpp grpc/src/grpc_hlc_service.cpp - grpc/src/server_runner.cpp + ../task/grpc_server_task/src/grpc_server_task.cpp ) target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) @@ -17,8 +17,9 @@ target_link_libraries(service PRIVATE cmvr_es::proto osqp cmvr_es::device_manager - cmvr_es::controller - cmvr_es::applications + cmvr_es::task_manager + cmvr_es::algorithms::controller + cmvr_es::task protobuf::libprotobuf ) @@ -39,17 +40,16 @@ link_directories( ) -add_executable(grpc_humanoid_robot_client_test - grpc/src/grpc_humanoid_robot_client_test.cpp +add_executable(grpc_arm_client_test + grpc/src/grpc_arm_client_test.cpp ) -target_link_libraries(grpc_humanoid_robot_client_test +target_link_libraries(grpc_arm_client_test PRIVATE cmvr_es::device::canbus cmvr_es::device::ti5motor osqp - cmvr_es::device::humanoid_robot gtest gtest_main pthread @@ -72,7 +72,6 @@ target_link_libraries(grpc_hlc_client_test cmvr_es::device::canbus cmvr_es::device::ti5motor osqp - cmvr_es::device::humanoid_robot gtest gtest_main pthread diff --git a/cmvr-es/service/grpc/include/grpc_arm_service.h b/cmvr-es/service/grpc/include/grpc_arm_service.h new file mode 100644 index 00000000..9a39ab3a --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_arm_service.h @@ -0,0 +1,58 @@ +#pragma once + +#include "cmvr/api/arm_service.grpc.pb.h" +#include "devices/arm/robot_arm.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::service { + +class gRPCArmServiceImpl final : public api::ArmService::Service { +public: + gRPCArmServiceImpl(); + ~gRPCArmServiceImpl() override = default; + + grpc::Status torqueOff(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status torqueOn(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status moveJ(grpc::ServerContext* context, + const api::MoveJ_Request* request, + api::MoveJ_Response* response) override; + grpc::Status moveL(grpc::ServerContext* context, + const api::MoveL_Request* request, + api::MoveL_Response* response) override; + grpc::Status speedJ(grpc::ServerContext* context, + const api::SpeedJ_Request* request, + api::SpeedJ_Response* response) override; + grpc::Status speedL(grpc::ServerContext* context, + const api::SpeedL_Request* request, + api::SpeedL_Response* response) override; + grpc::Status servoJ(grpc::ServerContext* context, + const api::ServoJ_Request* request, + api::ServoJ_Response* response) override; + grpc::Status stopMotion(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status getJointState(grpc::ServerContext* context, + const api::JointRequest* request, + api::JointResponse* response) override; + grpc::Status getPose(grpc::ServerContext* context, + const api::GetPose_Request* request, + api::GetPose_Response* response) override; + grpc::Status calibrateZeroQ(grpc::ServerContext* context, + const api::CalibrateZeroQ_Request* request, + api::CalibrateZeroQ_Response* response) override; + grpc::Status getPoseMatrix(grpc::ServerContext* context, + const api::GetPoseMatrix_Request* request, + api::GetPoseMatrix_Response* response) override; + grpc::Status computeForwardKinematics(grpc::ServerContext* context, + const api::ComputeForwardKinematics_Request* request, + api::ComputeForwardKinematics_Response* response) override; + +private: + device::DeviceManager& dmgr_; +}; + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_camera_service.h b/cmvr-es/service/grpc/include/grpc_camera_service.h index dd10a634..d9bc1782 100644 --- a/cmvr-es/service/grpc/include/grpc_camera_service.h +++ b/cmvr-es/service/grpc/include/grpc_camera_service.h @@ -6,8 +6,9 @@ #define GRPC_CAMERA_SERVICE_H #include "cmvr/api/camera_service.grpc.pb.h" -#include "utils/base/include/grpc_utils.h" -#include "device_manager/include/device_manager.h" +#include "common/base/grpc_utils.h" +#include "manager/device_manager/include/device_manager.h" +#include "devices/camera/abstract_camera.h" namespace cmvr::service { diff --git a/cmvr-es/service/grpc/include/grpc_dexhand_service.h b/cmvr-es/service/grpc/include/grpc_dexhand_service.h index b9fcc758..865a9870 100644 --- a/cmvr-es/service/grpc/include/grpc_dexhand_service.h +++ b/cmvr-es/service/grpc/include/grpc_dexhand_service.h @@ -6,8 +6,9 @@ #define GRPC_DEXHAND_SERVICE_H #include "cmvr/api/dexhand_service.grpc.pb.h" -#include "utils/base/include/grpc_utils.h" -#include "device_manager/include/device_manager.h" +#include "common/base/grpc_utils.h" +#include "manager/device_manager/include/device_manager.h" +#include "devices/dexhand/abstract_dexhand.h" namespace cmvr::service { class gRPCDexHandServiceImpl final: public api::DexHandService::Service { public: diff --git a/cmvr-es/service/grpc/include/grpc_head_service.h b/cmvr-es/service/grpc/include/grpc_head_service.h index d7995a57..4b8b20c3 100644 --- a/cmvr-es/service/grpc/include/grpc_head_service.h +++ b/cmvr-es/service/grpc/include/grpc_head_service.h @@ -2,7 +2,8 @@ #define BIO_HEAD_SERVICE_H #include "cmvr/api/biohead_service.grpc.pb.h" -#include "device_manager/include/device_manager.h" +#include "manager/device_manager/include/device_manager.h" +#include "devices/biohead/abstract_biohead.h" namespace cmvr::service { diff --git a/cmvr-es/service/grpc/include/grpc_hlc_service.h b/cmvr-es/service/grpc/include/grpc_hlc_service.h index 14675e8f..9fc593cc 100644 --- a/cmvr-es/service/grpc/include/grpc_hlc_service.h +++ b/cmvr-es/service/grpc/include/grpc_hlc_service.h @@ -3,10 +3,6 @@ // #pragma once -#include -#include - -#include "applications/include/touch_screen_app.h" #include "cmvr/api/hlc_service.grpc.pb.h" namespace cmvr { @@ -16,13 +12,6 @@ namespace cmvr { gRPCHlcServiceImpl(); ~gRPCHlcServiceImpl() = default; grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override; - - private: - bool ensureTouchAppInitialized(std::string& error_message); - - std::mutex touch_mutex_; - app::TouchScreenApp touch_app_; - bool touch_app_initialized_{false}; }; diff --git a/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h b/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h deleted file mode 100644 index b9f0b9f9..00000000 --- a/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h +++ /dev/null @@ -1,53 +0,0 @@ -// -// Created by lgv on 2025/8/15. -// - -#pragma once - -#include "device_manager/include/device_manager.h" -#include "cmvr/api/humanoid_robot_service.grpc.pb.h" -#include "ik_solver/include/pinocchio_qp_ik_solver.h" -namespace cmvr { - namespace service { - class gRPCHumanoidRobotServiceImpl final : public api::HumanoidRobotService::Service { - public: - gRPCHumanoidRobotServiceImpl(); - ~gRPCHumanoidRobotServiceImpl() = default; - grpc::Status torqueOff(grpc::ServerContext *context, - const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) override; - grpc::Status torqueOn(grpc::ServerContext *context, - const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) override; - - grpc::Status moveJ(grpc::ServerContext *context, const cmvr::api::MoveJ_Request *request, cmvr::api::MoveJ_Response *response) override; - - grpc::Status moveL(grpc::ServerContext* context, const cmvr::api::MoveL_Request* request, cmvr::api::MoveL_Response* response) override; - - grpc::Status speedJ(grpc::ServerContext* context, const cmvr::api::SpeedJ_Request* request, cmvr::api::SpeedJ_Response* response) override; - - grpc::Status speedL(grpc::ServerContext* context, const cmvr::api::SpeedL_Request* request, cmvr::api::SpeedL_Response* response) override; - - grpc::Status getJointState(grpc::ServerContext *context, const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) override; - - grpc::Status getPose(grpc::ServerContext *context, const cmvr::api::GetPose_Request *request, cmvr::api::GetPose_Response *response) override; - - grpc::Status calibrateZeroQ(grpc::ServerContext* context, const cmvr::api::CalibrateZeroQ_Request* request, cmvr::api::CalibrateZeroQ_Response* response) override; - - grpc::Status servoJ(grpc::ServerContext* context, const cmvr::api::ServoJ_Request* request, cmvr::api::ServoJ_Response* response) override; - - grpc::Status getPoseMatrix(grpc::ServerContext* context, const cmvr::api::GetPoseMatrix_Request* request, cmvr::api::GetPoseMatrix_Response* response) override; - - grpc::Status computeForwardKinematics(grpc::ServerContext* context, const cmvr::api::ComputeForwardKinematics_Request* request, cmvr::api::ComputeForwardKinematics_Response* response) override; - private: - device::DeviceManager& dmgr_; - - - - //矩阵转换 - std::shared_ptr pinocchio_ik_{nullptr}; - }; - - - - } - -} diff --git a/cmvr-es/service/grpc/include/grpc_microphone_service.h b/cmvr-es/service/grpc/include/grpc_microphone_service.h index 6a7932f1..33420240 100644 --- a/cmvr-es/service/grpc/include/grpc_microphone_service.h +++ b/cmvr-es/service/grpc/include/grpc_microphone_service.h @@ -6,8 +6,9 @@ #ifndef GRPC_MICROPHONE_SERVICE_H #define GRPC_MICROPHONE_SERVICE_H #include "cmvr/api/microphone_service.grpc.pb.h" -#include "device_manager/include/device_manager.h" -#include "utils/base/include/grpc_utils.h" +#include "manager/device_manager/include/device_manager.h" +#include "devices/microphone/abstract_microphone.h" +#include "common/base/grpc_utils.h" namespace cmvr::service { diff --git a/cmvr-es/service/grpc/include/grpc_speaker_service.h b/cmvr-es/service/grpc/include/grpc_speaker_service.h index ac171af7..f2f61256 100644 --- a/cmvr-es/service/grpc/include/grpc_speaker_service.h +++ b/cmvr-es/service/grpc/include/grpc_speaker_service.h @@ -6,8 +6,9 @@ #define GRPC_SPEAKER_SERVICE_H #include "cmvr/api/speaker_service.grpc.pb.h" -#include "device_manager/include/device_manager.h" -#include "utils/base/include/grpc_utils.h" +#include "manager/device_manager/include/device_manager.h" +#include "devices/speaker/abstract_speaker.h" +#include "common/base/grpc_utils.h" namespace cmvr::service { class gRPCSpeakerServiceImpl: public api::SpeakerService::Service { diff --git a/cmvr-es/service/grpc/include/grpc_system_service.h b/cmvr-es/service/grpc/include/grpc_system_service.h index ec409f43..17028a92 100644 --- a/cmvr-es/service/grpc/include/grpc_system_service.h +++ b/cmvr-es/service/grpc/include/grpc_system_service.h @@ -6,8 +6,8 @@ #define GRPC_SYSTEM_SERVICE_H #include "cmvr/api/system_service.grpc.pb.h" -#include "utils/base/include/grpc_utils.h" -#include "device_manager/include/device_manager.h" +#include "common/base/grpc_utils.h" +#include "manager/device_manager/include/device_manager.h" namespace cmvr::service { diff --git a/cmvr-es/service/grpc/include/server_runner.h b/cmvr-es/service/grpc/include/server_runner.h deleted file mode 100644 index be8eadf8..00000000 --- a/cmvr-es/service/grpc/include/server_runner.h +++ /dev/null @@ -1,56 +0,0 @@ -// -// Created by lgv on 3/13/26. -// -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include "rapidxml/xml_parser.h" -namespace cmvr::service { - - - class ServerRunner { - public: - ServerRunner(); - explicit ServerRunner(XmlNode cfg); - ~ServerRunner(); - - ServerRunner(const ServerRunner&) = delete; - ServerRunner& operator=(const ServerRunner&) = delete; - ServerRunner(ServerRunner&&) = delete; - ServerRunner& operator=(ServerRunner&&) = delete; - - void start(XmlNode cfg); - void stop(); - void join(); - - bool isRunning() const; - std::string address() const; - - private: - void threadMain(); - - private: - mutable std::mutex mtx_; - std::condition_variable cv_; - std::thread worker_; - - XmlNode cfg_; - std::unique_ptr server_; - - std::string address_; - std::string last_error_; - - std::atomic running_{false}; - bool stop_requested_{false}; - bool started_{false}; - bool start_failed_{false}; - }; - - -} diff --git a/cmvr-es/service/grpc/src/grpc_arm_client_test.cpp b/cmvr-es/service/grpc/src/grpc_arm_client_test.cpp new file mode 100644 index 00000000..ff1af4d8 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_arm_client_test.cpp @@ -0,0 +1,84 @@ +#include +#include "common/base/logging/logger.h" +#include +#include + +#include "cmvr/api/arm_service.grpc.pb.h" + +using google::protobuf::util::TimeUtil; + +namespace { + +std::unique_ptr makeStub() +{ + auto channel = grpc::CreateChannel("0.0.0.0:50055", grpc::InsecureChannelCredentials()); + return cmvr::api::ArmService::NewStub(channel); +} + +void fillHeader(cmvr::api::CommandHeader_Request* header) +{ + header->set_device_id("right_arm"); + *header->mutable_timestamp() = TimeUtil::GetCurrentTime(); +} + +} // namespace + +TEST(GrpcArmClientTest, TorqueOn) +{ + auto stub = makeStub(); + cmvr::api::CommandHeader_Request request; + fillHeader(&request); + + cmvr::api::CommandHeader_Feedback response; + grpc::ClientContext context; + const grpc::Status status = stub->torqueOn(&context, request, &response); + + if (!status.ok()) { + CMVR_LOG(ERROR) << "RPC failed: " << status.error_message(); + } +} + +TEST(GrpcArmClientTest, MoveJ) +{ + auto stub = makeStub(); + cmvr::api::MoveJ_Request request; + fillHeader(request.mutable_header()); + + const std::vector q{ + 0.00203898, 1.34062, 0.0, 0.322261, 0.0, -0.000210733, -0.0942364 + }; + for (double value : q) { + request.mutable_target()->add_position(value); + } + request.mutable_options()->set_velocity(0.8); + request.mutable_options()->set_acceleration(0.8); + + cmvr::api::MoveJ_Response response; + grpc::ClientContext context; + const grpc::Status status = stub->moveJ(&context, request, &response); + + if (!status.ok()) { + CMVR_LOG(ERROR) << "RPC failed: " << status.error_message(); + } +} + +TEST(GrpcArmClientTest, GetJointState) +{ + auto stub = makeStub(); + cmvr::api::JointRequest request; + fillHeader(request.mutable_header()); + + cmvr::api::JointResponse response; + grpc::ClientContext context; + const grpc::Status status = stub->getJointState(&context, request, &response); + + if (status.ok()) { + const auto& state = response.state(); + for (int i = 0; i < state.name_size() && i < state.position_size(); ++i) { + CMVR_LOG(INFO) << "Joint: " << state.name(i) + << ", Position: " << state.position(i); + } + } else { + CMVR_LOG(ERROR) << "RPC failed: " << status.error_message(); + } +} diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp new file mode 100644 index 00000000..ac8e3c0b --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -0,0 +1,353 @@ +#include "service/grpc/include/grpc_arm_service.h" + +#include + +using google::protobuf::util::TimeUtil; + +namespace cmvr::service { + +namespace { + +void fillFeedback(api::CommandHeader_Feedback* feedback, + const bool success, + const std::string& message = {}) +{ + feedback->set_success(success); + feedback->set_error_message(message); + *feedback->mutable_timestamp() = TimeUtil::GetCurrentTime(); +} + +grpc::Status resultToStatus(const device::Result& result) +{ + if (result.ok()) { + return grpc::Status::OK; + } + return grpc::Status(grpc::StatusCode::INTERNAL, result.message); +} + +device::FrameType toFrameType(const api::ArmFrameType frame) +{ + switch (frame) { + case api::ARM_FRAME_TOOL: + return device::FrameType::Tool; + case api::ARM_FRAME_WORLD: + return device::FrameType::World; + case api::ARM_FRAME_USER: + return device::FrameType::User; + case api::ARM_FRAME_BASE: + default: + return device::FrameType::Base; + } +} + +device::JointPositionCommand toJointPositionCommand(const api::JointPositionCommand& src) +{ + device::JointPositionCommand dst; + dst.position.assign(src.position().begin(), src.position().end()); + return dst; +} + +device::JointVelocityCommand toJointVelocityCommand(const api::JointVelocityCommand& src) +{ + device::JointVelocityCommand dst; + dst.velocity.assign(src.velocity().begin(), src.velocity().end()); + return dst; +} + +device::MotionOptions toMotionOptions(const api::MotionOptions& src) +{ + device::MotionOptions dst; + dst.velocity = src.velocity(); + dst.acceleration = src.acceleration(); + dst.blend_radius = src.blend_radius(); + dst.jerk = src.jerk() > 0.0 ? src.jerk() : 5.0; + dst.joint_velocity_limits.assign(src.joint_velocity_limits().begin(), + src.joint_velocity_limits().end()); + dst.asynchronous = src.asynchronous(); + return dst; +} + +device::CartesianPose toCartesianPose(const api::CartesianPose& src) +{ + return {src.x(), src.y(), src.z(), src.rx(), src.ry(), src.rz()}; +} + +api::CartesianPose toApiCartesianPose(const device::CartesianPose& src) +{ + api::CartesianPose dst; + dst.set_x(src.x); + dst.set_y(src.y); + dst.set_z(src.z); + dst.set_rx(src.rx); + dst.set_ry(src.ry); + dst.set_rz(src.rz); + return dst; +} + +device::CartesianVelocity toCartesianVelocity(const api::CartesianVelocity& src) +{ + return {src.vx(), src.vy(), src.vz(), src.wx(), src.wy(), src.wz()}; +} + +template +grpc::Status setResponseResult(Response* response, const device::Result& result) +{ + fillFeedback(response->mutable_header(), result.ok(), result.ok() ? "" : result.message); + return resultToStatus(result); +} + +grpc::Status setDeviceNotFound(api::CommandHeader_Feedback* response, const std::string& device_id) +{ + const std::string message = "RobotArm device not found: " + device_id; + fillFeedback(response, false, message); + return grpc::Status(grpc::StatusCode::NOT_FOUND, message); +} + +template +grpc::Status setDeviceNotFound(Response* response, const std::string& device_id) +{ + const std::string message = "RobotArm device not found: " + device_id; + fillFeedback(response->mutable_header(), false, message); + return grpc::Status(grpc::StatusCode::NOT_FOUND, message); +} + +} // namespace + +gRPCArmServiceImpl::gRPCArmServiceImpl() + : dmgr_(device::DeviceManager::getInstance()) +{ +} + +grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + const std::string device_id = request->device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + const auto result = arm->torqueOff(); + fillFeedback(response, result.ok(), result.ok() ? "" : result.message); + return resultToStatus(result); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + const std::string device_id = request->device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + const auto result = arm->torqueOn(); + fillFeedback(response, result.ok(), result.ok() ? "" : result.message); + return resultToStatus(result); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*, + const api::MoveJ_Request* request, + api::MoveJ_Response* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + return setResponseResult(response, arm->moveJ(toJointPositionCommand(request->target()), + toMotionOptions(request->options()))); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*, + const api::MoveL_Request* request, + api::MoveL_Response* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + return setResponseResult(response, arm->moveL(toCartesianPose(request->target()), + toMotionOptions(request->options()), + toFrameType(request->frame()))); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*, + const api::SpeedJ_Request* request, + api::SpeedJ_Response* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + return setResponseResult(response, arm->speedJ(toJointVelocityCommand(request->velocity()), + request->acceleration(), + request->duration())); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*, + const api::SpeedL_Request* request, + api::SpeedL_Response* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + return setResponseResult(response, arm->speedL(toCartesianVelocity(request->velocity()), + request->acceleration(), + request->duration(), + toFrameType(request->frame()))); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*, + const api::ServoJ_Request* request, + api::ServoJ_Response* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + return setResponseResult(response, arm->servoJ(toJointPositionCommand(request->target()))); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + const std::string device_id = request->device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + const auto result = arm->stopMotion(); + fillFeedback(response, result.ok(), result.ok() ? "" : result.message); + return resultToStatus(result); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext*, + const api::JointRequest* request, + api::JointResponse* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + const auto model = arm->getRobotModel(); + const auto state = arm->getJointState(); + auto* msg = response->mutable_state(); + for (const auto& name : model.joint_names) { + msg->add_name(name); + } + for (double v : state.position) msg->add_position(v); + for (double v : state.velocity) msg->add_velocity(v); + for (double v : state.effort) msg->add_effort(v); + fillFeedback(response->mutable_header(), true); + return grpc::Status::OK; + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCArmServiceImpl::getPose(grpc::ServerContext*, + const api::GetPose_Request* request, + api::GetPose_Response* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + const auto pose = request->base_link().empty() || request->ee_link().empty() + ? arm->fk(true) + : arm->fk(request->base_link(), request->ee_link()); + *response->mutable_pose() = toApiCartesianPose(pose); + fillFeedback(response->mutable_header(), true); + return grpc::Status::OK; + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*, + const api::CalibrateZeroQ_Request* request, + api::CalibrateZeroQ_Response* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + return setResponseResult(response, arm->calibrateZeroQ(request->joint_name())); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCArmServiceImpl::getPoseMatrix(grpc::ServerContext*, + const api::GetPoseMatrix_Request*, + api::GetPoseMatrix_Response* response) +{ + fillFeedback(response->mutable_header(), false, "getPoseMatrix is not implemented"); + return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "getPoseMatrix is not implemented"); +} + +grpc::Status gRPCArmServiceImpl::computeForwardKinematics(grpc::ServerContext*, + const api::ComputeForwardKinematics_Request*, + api::ComputeForwardKinematics_Response* response) +{ + fillFeedback(response->mutable_header(), false, "computeForwardKinematics is not implemented"); + return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "computeForwardKinematics is not implemented"); +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index 18779b1a..8f15c8a7 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by xtkuang on 2025/6/1. // @@ -10,7 +11,16 @@ using namespace std; using namespace cmvr::service; using namespace cmvr::device; - +namespace { +template +grpc::Status failResponse(ResponseT* response, const std::string& message) { + CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] " << message; + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(message); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; +} +} gRPCCameraServiceImpl::gRPCCameraServiceImpl(): dmgr_(DeviceManager::getInstance()) {} @@ -19,8 +29,11 @@ grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context, { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCCameraServiceImpl] (GetStatus): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetStatus): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Camera device not found: " + dev_id); + } CameraState state{}; dev->getState(state); response->mutable_header()->set_success(true); @@ -49,9 +62,14 @@ grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context, { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCCameraServiceImpl] (StartCamera): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StartCamera): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); - dev->start(); + if (!dev) { + return failResponse(response, "Camera device not found: " + dev_id); + } + if (!dev->start()) { + return failResponse(response, "Failed to start camera: " + dev_id); + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; @@ -69,9 +87,14 @@ grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context, { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCCameraServiceImpl] (StopCamera): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StopCamera): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); - dev->stop(); + if (!dev) { + return failResponse(response, "Camera device not found: " + dev_id); + } + if (!dev->stop()) { + return failResponse(response, "Failed to stop camera: " + dev_id); + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; @@ -89,9 +112,12 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id; cv::Mat image; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Camera device not found: " + dev_id); + } Rs2Intrinsics intrinsics = {0}; dev->getRGBImage(image,intrinsics); response->mutable_header()->set_success(true); @@ -117,8 +143,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, response->mutable_color_frame()->set_type(api::FrameData::U16C3); } else { - LOG(ERROR) << "unsupported image type"; - throw std::runtime_error("unsupported image type"); + return failResponse(response, "unsupported image type"); } response->mutable_color_frame()->set_data( reinterpret_cast(image.data), image.total() * image.elemSize()); @@ -140,9 +165,12 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context, { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImage): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImage): id=" << dev_id; cv::Mat image; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Camera device not found: " + dev_id); + } Rs2Intrinsics intrinsics = {0}; dev->getDepthImage(image,intrinsics); response->mutable_header()->set_success(true); @@ -170,8 +198,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context, response->mutable_depth_frame()->set_type(api::FrameData::F32C1); } else { - LOG(ERROR) << "unsupported image type"; - throw std::runtime_error("unsupported image type"); + return failResponse(response, "unsupported image type"); } response->mutable_depth_frame()->set_data( @@ -194,9 +221,12 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImages): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImages): id=" << dev_id; cv::Mat color_image, depth_image; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Camera device not found: " + dev_id); + } Rs2Intrinsics intrinsics = {0}; dev->getRGBDImages(color_image,depth_image, intrinsics); response->mutable_header()->set_success(true); @@ -219,8 +249,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, response->mutable_color_frame()->set_type(api::FrameData::U16C3); } else { - LOG(ERROR) << "unsupported image type"; - throw std::runtime_error("unsupported image type"); + return failResponse(response, "unsupported image type"); } response->mutable_color_frame()->set_data( reinterpret_cast(color_image.data), color_image.total() * color_image.elemSize()); @@ -261,8 +290,11 @@ grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context, { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCCameraServiceImpl] (StartRecording): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StartRecording): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Camera device not found: " + dev_id); + } dev->startRecording(request->video_path()); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -281,8 +313,11 @@ grpc::Status gRPCCameraServiceImpl::StopRecording(grpc::ServerContext* context, { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCCameraServiceImpl] (StopRecording): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StopRecording): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Camera device not found: " + dev_id); + } dev->stopRecording(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -304,8 +339,16 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con api::GetDepthImageStreamCommand_Request request; stream->Read(&request); string dev_id = request.header().device_id(); - LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + api::GetDepthImageStreamCommand_Feedback response; + response.mutable_header()->set_success(false); + response.mutable_header()->set_error_message("Camera device not found: " + dev_id); + setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); + stream->Write(response); + return grpc::Status::OK; + } int nFrameCount = 0; dev->startStreaming(); size_t index = 0; @@ -313,7 +356,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con { if (context->IsCancelled()) { - LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id; break; } @@ -341,12 +384,12 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con grpc::WriteOptions options; options.set_last_message(); if (!stream->Write(response)) { - LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; break; } } } - LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id; dev->stopStreaming(); return grpc::Status::OK; } @@ -365,8 +408,16 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con api::GetRGBDImagesStreamCommand_Request request; stream->Read(&request); string dev_id = request.header().device_id(); - LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + api::GetRGBDImagesStreamCommand_Feedback response; + response.mutable_header()->set_success(false); + response.mutable_header()->set_error_message("Camera device not found: " + dev_id); + setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); + stream->Write(response); + return grpc::Status::OK; + } int nFrameCount = 0; dev->startStreaming(); size_t index = 0; @@ -374,7 +425,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con { if (context->IsCancelled()) { - LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBDImagesStream) context is cancelled,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBDImagesStream) context is cancelled,id=" << dev_id; break; } @@ -408,12 +459,12 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con grpc::WriteOptions options; options.set_last_message(); if (!stream->Write(response)) { - LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; break; } } } - LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id; dev->stopStreaming(); return grpc::Status::OK; } @@ -431,8 +482,16 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte api::GetRGBImageStreamCommand_Request request; stream->Read(&request); string dev_id = request.header().device_id(); - LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + api::GetRGBImageStreamCommand_Feedback response; + response.mutable_header()->set_success(false); + response.mutable_header()->set_error_message("Camera device not found: " + dev_id); + setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); + stream->Write(response); + return grpc::Status::OK; + } int nFrameCount = 0; dev->startStreaming(); size_t last_sent_index = std::numeric_limits::max(); @@ -440,7 +499,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte { if (context->IsCancelled()) { - LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id; break; } @@ -472,12 +531,12 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte response.set_seq_no(nFrameCount++); if (!stream->Write(response)) { - LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; break; } last_sent_index = next_index; } - LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id; dev->stopStreaming(); return grpc::Status::OK; } diff --git a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp index 6f08f5dc..5de05e55 100644 --- a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by linbo on 2025/7/3. // @@ -7,7 +8,6 @@ #include #include #include -#include #include #include @@ -90,18 +90,35 @@ void appendSensorData(const std::vector& tac } template -void applyFreedomValues(const FreedomCollection& freedoms, +bool applyFreedomValues(const FreedomCollection& freedoms, const int scale, - std::vector& targets) { + std::vector& targets, + std::string* error_message) { for (const auto& freedom : freedoms) { if (freedom.id() < 0 || freedom.id() >= static_cast(targets.size())) { - throw std::invalid_argument("Invalid dexhand DOF id: " + std::to_string(freedom.id())); + if (error_message) { + *error_message = "Invalid dexhand DOF id: " + std::to_string(freedom.id()); + } + return false; } if (!std::isfinite(freedom.value())) { - throw std::invalid_argument("Invalid dexhand command value: not finite."); + if (error_message) { + *error_message = "Invalid dexhand command value: not finite."; + } + return false; } targets[static_cast(freedom.id())] = static_cast(freedom.value() * scale); } + return true; +} + +template +grpc::Status failResponse(ResponseT* response, const std::string& message) { + CMVR_LOG(ERROR) << "[gRPCDexHandServiceImpl] " << message; + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(message); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; } std::vector readCurrentAngles(const std::shared_ptr& dev) { @@ -170,8 +187,11 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context, const api::GetDexHandStateCommand_Request* request, api::GetDexHandStateCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCDexHandServiceImpl] (GetStatus): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetStatus): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "DexHand device not found: " + dev_id); + } DexHandState state{}; dev->getState(state); response->mutable_state()->set_is_initialized(state.is_initialized); @@ -206,8 +226,11 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context , cmvr::api::SetDexHandPositionsCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPos): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPos): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "DexHand device not found: " + dev_id); + } if (respondUnsupportedForRh56(dev, "SetDexHandPos", "Use SetDexHandAngle for RH56 joint commands.", @@ -216,7 +239,10 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context } std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); - applyFreedomValues(request->values(), DEXHAND_MAX_POSITION, finger_joint_targets); + std::string error_message; + if (!applyFreedomValues(request->values(), DEXHAND_MAX_POSITION, finger_joint_targets, &error_message)) { + return failResponse(response, error_message); + } dev->setPositions(finger_joint_targets); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -235,16 +261,25 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex , cmvr::api::SetDexHandAnglesCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "DexHand device not found: " + dev_id); + } if (const auto rh56 = std::dynamic_pointer_cast(dev)) { std::vector finger_joint_targets = readCurrentAngles(dev); - applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets); + std::string error_message; + if (!applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets, &error_message)) { + return failResponse(response, error_message); + } rh56->setAngles(finger_joint_targets); } else { std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); - applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets); + std::string error_message; + if (!applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets, &error_message)) { + return failResponse(response, error_message); + } dev->setAngles(finger_joint_targets); } @@ -265,8 +300,11 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex , cmvr::api::SetDexHandForceCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandForce): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandForce): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "DexHand device not found: " + dev_id); + } if (respondUnsupportedForRh56(dev, "SetDexHandForce", "RH56DFTPDexhand currently exposes angle and tactile APIs only.", @@ -275,7 +313,10 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex } std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); - applyFreedomValues(request->values(), DEXHAND_MAX_FORCE, finger_joint_targets); + std::string error_message; + if (!applyFreedomValues(request->values(), DEXHAND_MAX_FORCE, finger_joint_targets, &error_message)) { + return failResponse(response, error_message); + } dev->setForce(finger_joint_targets); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -294,8 +335,11 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex , cmvr::api::SetDexHandSpeedCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "DexHand device not found: " + dev_id); + } if (respondUnsupportedForRh56(dev, "SetDexHandSpeed", "RH56DFTPDexhand currently exposes angle and tactile APIs only.", @@ -304,7 +348,10 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex } std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); - applyFreedomValues(request->values(), DEXHAND_MAX_SPEED, finger_joint_targets); + std::string error_message; + if (!applyFreedomValues(request->values(), DEXHAND_MAX_SPEED, finger_joint_targets, &error_message)) { + return failResponse(response, error_message); + } dev->setVelocities(finger_joint_targets); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -323,8 +370,11 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* co , cmvr::api::SetDexHandPresetActCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "DexHand device not found: " + dev_id); + } if (respondUnsupportedForRh56(dev, "SetDexHandPresetAct", "RH56DFTPDexhand currently exposes angle and tactile APIs only.", @@ -352,8 +402,11 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorData): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorData): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "DexHand device not found: " + dev_id); + } maybeConfigureRh56FullTactilePolling(dev); appendSensorData(dev->getSensorData(), response); response->mutable_header()->set_success(true); @@ -378,8 +431,16 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co } string dev_id = request.header().device_id(); - LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): start,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): start,id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + api::GetSensorDataStreamCommand_Feedback response; + response.mutable_header()->set_success(false); + response.mutable_header()->set_error_message("DexHand device not found: " + dev_id); + setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); + stream->Write(response); + return grpc::Status::OK; + } maybeConfigureRh56FullTactilePolling(dev); while (!context->IsCancelled()) @@ -390,7 +451,7 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); if (!stream->Write(response)) { - LOG(INFO) << "[gRPCDexHandServiceImpl] (stream->Write) failed,id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (stream->Write) failed,id=" << dev_id; break; } @@ -399,7 +460,7 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co return grpc::Status::OK; } catch (const std::exception& e) { - LOG(ERROR) << "[gRPCDexHandServiceImpl] (GetSensorDataStream) exception: " << e.what(); + CMVR_LOG(ERROR) << "[gRPCDexHandServiceImpl] (GetSensorDataStream) exception: " << e.what(); return grpc::Status::OK; } diff --git a/cmvr-es/service/grpc/src/grpc_head_service.cpp b/cmvr-es/service/grpc/src/grpc_head_service.cpp index 10b02810..118a55e6 100644 --- a/cmvr-es/service/grpc/src/grpc_head_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_head_service.cpp @@ -1,19 +1,30 @@ +#include "common/base/logging/logger.h" #include "../include/grpc_head_service.h" #include "cmvr/api/biohead_service.grpc.pb.h" -#include "device_manager/include/device_manager.h" -#include "utils/base/include/grpc_utils.h" +#include "manager/device_manager/include/device_manager.h" +#include "common/base/grpc_utils.h" #include "biohead/biohead_esp32/include/biohead_esp32.h" #include #include #include -#include using namespace std; using namespace cmvr::service; using namespace cmvr::device; using namespace cmvr::api; +namespace { +template +grpc::Status failResponse(ResponseT* response, const std::string& message) { + CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] " << message; + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(message); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; +} +} + gRPCMBioHeadServiceImpl::gRPCMBioHeadServiceImpl() : dmgr_(DeviceManager::getInstance()) {} @@ -28,7 +39,7 @@ grpc::Status gRPCMBioHeadServiceImpl::SetExpression( std::string dev_id = request->header().device_id(); auto robot = dmgr_.getDevice(dev_id); if (!robot) { - throw std::runtime_error("Biohead device not found"); + return failResponse(response, "Biohead device not found: " + dev_id); } FacialExpressionState& expression_state = robot->expression_state_; @@ -84,17 +95,28 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( const auto time_interval = std::chrono::milliseconds(static_cast(1000 / control_frequency)); auto last_control_time = std::chrono::steady_clock::now(); - LOG(INFO) << "StreamExpression started."; + CMVR_LOG(INFO) << "StreamExpression started."; while (stream->Read(&request_msg)) { if (first_message) { dev_id = request_msg.header().device_id(); if (dev_id.empty()) { - throw runtime_error("Device ID is empty in first message"); + CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] Device ID is empty in first message"; + feedback_msg.mutable_header()->set_success(false); + feedback_msg.mutable_header()->set_error_message("Device ID is empty in first message"); + setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp()); + stream->Write(feedback_msg); + return grpc::Status::OK; } robot = dmgr_.getDevice(dev_id); if (!robot) { - throw runtime_error("Biohead device not found: " + dev_id); + const std::string message = "Biohead device not found: " + dev_id; + CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] " << message; + feedback_msg.mutable_header()->set_success(false); + feedback_msg.mutable_header()->set_error_message(message); + setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp()); + stream->Write(feedback_msg); + return grpc::Status::OK; } // ✅ 重置紧急停止标志 @@ -105,7 +127,7 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( // ✅ 如果紧急停止触发，直接退出 if (robot->emergency_stop_requested) { - LOG(WARNING) << "[Stream] Emergency stop requested. Terminating stream for device: " << dev_id; + CMVR_LOG(WARNING) << "[Stream] Emergency stop requested. Terminating stream for device: " << dev_id; break; } @@ -161,10 +183,10 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( if (!stream->Write(feedback_msg)) break; } - LOG(INFO) << "StreamExpression finished for device: " << dev_id; + CMVR_LOG(INFO) << "StreamExpression finished for device: " << dev_id; return grpc::Status::OK; } catch (const std::exception& e) { - LOG(ERROR) << "StreamExpression error: " << e.what(); + CMVR_LOG(ERROR) << "StreamExpression error: " << e.what(); feedback_msg.mutable_header()->set_success(false); feedback_msg.mutable_header()->set_error_message(e.what()); setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp()); @@ -184,7 +206,7 @@ grpc::Status gRPCMBioHeadServiceImpl::GetSystemStatus( string dev_id = request->header().device_id(); auto robot = dmgr_.getDevice(dev_id); if (!robot) { - throw runtime_error("Biohead device not found"); + return failResponse(response, "Biohead device not found: " + dev_id); } response->mutable_header()->set_success(true); @@ -211,7 +233,7 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop( auto robot = dmgr_.getDevice(dev_id); if (!robot) { - throw runtime_error("Biohead device not found"); + return failResponse(response, "Biohead device not found: " + dev_id); } robot->eStop(); // 停止执行 @@ -243,9 +265,8 @@ grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, c auto robot = dmgr_.getDevice(dev_id); - std::vector cmds{}; if (!robot) { - throw runtime_error("Biohead device not found"); + return failResponse(response, "Biohead device not found: " + dev_id); } robot->speakstart(); // kaish开始 @@ -272,7 +293,7 @@ grpc::Status gRPCMBioHeadServiceImpl::SpeakStop(grpc::ServerContext* context, co string dev_id = request->header().device_id(); auto robot = dmgr_.getDevice(dev_id); if (!robot) { - throw runtime_error("Biohead device not found"); + return failResponse(response, "Biohead device not found: " + dev_id); } robot->speakstop(); // 停止执行 @@ -297,7 +318,7 @@ grpc::Status gRPCMBioHeadServiceImpl::Happy(grpc::ServerContext* context, const string dev_id = request->header().device_id(); auto robot = dmgr_.getDevice(dev_id); if (!robot) { - throw runtime_error("Biohead device not found"); + return failResponse(response, "Biohead device not found: " + dev_id); } robot->expressionHappy(); // 停止执行 @@ -320,7 +341,7 @@ grpc::Status gRPCMBioHeadServiceImpl::Surprise(grpc::ServerContext* context, con string dev_id = request->header().device_id(); auto robot = dmgr_.getDevice(dev_id); if (!robot) { - throw runtime_error("Biohead device not found"); + return failResponse(response, "Biohead device not found: " + dev_id); } robot->expressionSurprised(); // @@ -344,7 +365,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* conte string dev_id = request->header().device_id(); auto robot = dmgr_.getDevice(dev_id); if (!robot) { - throw runtime_error("Biohead device not found"); + return failResponse(response, "Biohead device not found: " + dev_id); } robot->expressionTired(); // 停止执行 @@ -369,7 +390,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionAngry(grpc::ServerContext* conte string dev_id = request->header().device_id(); auto robot = dmgr_.getDevice(dev_id); if (!robot) { - throw runtime_error("Biohead device not found"); + return failResponse(response, "Biohead device not found: " + dev_id); } robot->expressionAngry(); // 停止执行 @@ -394,7 +415,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* con string dev_id = request->header().device_id(); auto robot = dmgr_.getDevice(dev_id); if (!robot) { - throw runtime_error("Biohead device not found"); + return failResponse(response, "Biohead device not found: " + dev_id); } robot->expressionSadness(); // 停止执行 @@ -418,7 +439,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionYawn(grpc::ServerContext* contex string dev_id = request->header().device_id(); auto robot = dmgr_.getDevice(dev_id); if (!robot) { - throw runtime_error("Biohead device not found"); + return failResponse(response, "Biohead device not found: " + dev_id); } robot->expressionYawn(); // 停止执行 diff --git a/cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp b/cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp index 9fc65ef0..3c564f37 100644 --- a/cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp +++ b/cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp @@ -2,7 +2,7 @@ // Created by lgv on 2025/8/25. // #include "gtest/gtest.h" -#include +#include "common/base/logging/logger.h" #include #include "../include/grpc_hlc_service.h" #include "google/protobuf/timestamp.pb.h" @@ -36,11 +36,11 @@ TEST(GrpcHlcClientTest, MyTest) { grpc::Status status = stub->touch(&context, request, &response); if (status.ok()) { - LOG(INFO) << "Touch RPC succeeded." << std::endl; - LOG(INFO) << "Success: " << response.mutable_header()->success() << std::endl; - LOG(INFO) << "Error message: " << response.mutable_header()->error_message() << std::endl; - LOG(INFO) << "Timestamp: " << response.mutable_header()->timestamp().seconds() << std::endl; + CMVR_LOG(INFO) << "Touch RPC succeeded." << std::endl; + CMVR_LOG(INFO) << "Success: " << response.mutable_header()->success() << std::endl; + CMVR_LOG(INFO) << "Error message: " << response.mutable_header()->error_message() << std::endl; + CMVR_LOG(INFO) << "Timestamp: " << response.mutable_header()->timestamp().seconds() << std::endl; } else { - LOG(ERROR) << "Touch RPC failed: " << status.error_message() << std::endl; + CMVR_LOG(ERROR) << "Touch RPC failed: " << status.error_message() << std::endl; } } \ No newline at end of file diff --git a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp index 4c21ec43..edc20fac 100644 --- a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp @@ -5,14 +5,14 @@ #include "../include/grpc_hlc_service.h" -#include #include -#include +#include #include -#include #include -#include "applications/include/touch_screen_app.h" + +#include "manager/task_manager/include/task_manager.h" +#include "task/touch_screen_task/include/touch_screen_task.h" using namespace cmvr::service; @@ -21,12 +21,12 @@ using google::protobuf::util::TimeUtil; namespace { -std::string buildTouchFailureMessage(const cmvr::app::TouchScreenApp& app, +std::string buildTouchFailureMessage(const cmvr::task::TouchScreenTask& task, const std::string& prefix) { return prefix + ", phase=" + - cmvr::app::TouchScreenApp::phaseToString(app.phase()) + + cmvr::task::TouchScreenTask::phaseToString(task.phase()) + ", status=" + - cmvr::app::TouchScreenApp::statusToString(app.lastStatus()); + cmvr::task::TouchScreenTask::statusToString(task.lastStatus()); } void fillTouchResponse(Touch_Response* response, @@ -41,116 +41,38 @@ void fillTouchResponse(Touch_Response* response, gRPCHlcServiceImpl::gRPCHlcServiceImpl() = default; -bool gRPCHlcServiceImpl::ensureTouchAppInitialized(std::string& error_message) { - if (touch_app_initialized_) { - return true; - } - - LOG(INFO) << "[gRPCHlcServiceImpl] init TouchScreenApp from config"; - if (!touch_app_.init()) { - error_message = buildTouchFailureMessage(touch_app_, "TouchScreenApp init failed"); - touch_app_initialized_ = false; - return false; - } - - touch_app_initialized_ = true; - return true; -} - grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) { - std::lock_guard lock(touch_mutex_); - try { - const double forward_l = request->max_force(); - LOG(INFO) << "[gRPCHlcServiceImpl] touch: request.device_id=" - << request->header().device_id() - << " (ignored, using TouchScreenApp config)" - << ", u=" << request->u() - << ", v=" << request->v() - << ", forward_l(max_force)=" << forward_l; - - std::string init_error; - if (!ensureTouchAppInitialized(init_error)) { - fillTouchResponse(response, false, init_error); - return grpc::Status(grpc::StatusCode::INTERNAL, init_error); + auto touch_task = task::TaskManager::getInstance().getTouchScreenTask(); + if (!touch_task) { + const std::string error = "TouchScreenTask not found or not initialized"; + fillTouchResponse(response, false, error); + return grpc::Status(grpc::StatusCode::NOT_FOUND, error); } - if (forward_l != -1.0) { - if (forward_l < 0.0) { - throw std::runtime_error("invalid forward_l(max_force): must be -1 or >= 0"); - } - if (!touch_app_.setTouchSpeedlForwardL(forward_l)) { - throw std::runtime_error( - buildTouchFailureMessage(touch_app_, "TouchScreenApp set speedl forward L failed")); - } + if (!touch_task->touch(request->u(), request->v())) { + const std::string error = + buildTouchFailureMessage(*touch_task, "TouchScreenTask touch request rejected"); + fillTouchResponse(response, false, error); + return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); } - if (!touch_app_.startFromPixel(request->u(), request->v())) { - throw std::runtime_error( - buildTouchFailureMessage(touch_app_, "TouchScreenApp startFromPixel failed")); - } - - bool align_reached = false; - bool touch_triggered = false; - auto last_logged_status = cmvr::app::TouchScreenApp::Status::IDLE; - constexpr double kControlDt = 0.001; - auto next_step_time = std::chrono::steady_clock::now(); - while (touch_app_.isBusy()) { + while (touch_task->isBusy()) { if (context != nullptr && context->IsCancelled()) { - touch_app_.stop(); const std::string error = "touch request cancelled"; fillTouchResponse(response, false, error); return grpc::Status(grpc::StatusCode::CANCELLED, error); } - - if (!touch_app_.step(kControlDt)) { - throw std::runtime_error( - buildTouchFailureMessage(touch_app_, "touch flow failed")); - } - - if (touch_app_.lastStatus() == cmvr::app::TouchScreenApp::Status::ALIGN_REACHED) { - align_reached = true; - } - if (touch_app_.lastStatus() == cmvr::app::TouchScreenApp::Status::TOUCH_TRIGGERED) { - touch_triggered = true; - } - - if (touch_app_.lastStatus() != last_logged_status) { - LOG(INFO) << "[gRPCHlcServiceImpl] touch status=" - << cmvr::app::TouchScreenApp::statusToString(touch_app_.lastStatus()) - << ", phase=" - << cmvr::app::TouchScreenApp::phaseToString(touch_app_.phase()) - << ", active_tag=" << touch_app_.lastActiveTagId() - << ", nonzero_count=" << touch_app_.lastTouchNonzeroCount() - << ", pressure_sum=" << touch_app_.lastTouchPressureSum(); - last_logged_status = touch_app_.lastStatus(); - } - - next_step_time += std::chrono::duration_cast( - std::chrono::duration(kControlDt)); - const auto after_step = std::chrono::steady_clock::now(); - if (next_step_time < after_step) { - next_step_time = after_step; - } - std::this_thread::sleep_until(next_step_time); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - if (!align_reached) { - throw std::runtime_error( - buildTouchFailureMessage(touch_app_, "touch exited without ALIGN_REACHED")); - } - if (!touch_triggered) { - throw std::runtime_error( - buildTouchFailureMessage(touch_app_, "touch exited without TOUCH_TRIGGERED")); - } - if (!touch_app_.isFinished()) { - throw std::runtime_error( - buildTouchFailureMessage(touch_app_, "touch did not finish successfully")); + if (!touch_task->isFinished()) { + const std::string error = + buildTouchFailureMessage(*touch_task, "TouchScreenTask touch failed"); + fillTouchResponse(response, false, error); + return grpc::Status(grpc::StatusCode::INTERNAL, error); } - LOG(INFO) << "[gRPCHlcServiceImpl] touch done" - << ", final_status=" - << cmvr::app::TouchScreenApp::statusToString(touch_app_.lastStatus()); fillTouchResponse(response, true, ""); return grpc::Status::OK; } catch (const std::exception& e) { diff --git a/cmvr-es/service/grpc/src/grpc_humanoid_robot_client_test.cpp b/cmvr-es/service/grpc/src/grpc_humanoid_robot_client_test.cpp deleted file mode 100644 index 9c78c749..00000000 --- a/cmvr-es/service/grpc/src/grpc_humanoid_robot_client_test.cpp +++ /dev/null @@ -1,138 +0,0 @@ -// -// Created by lgv on 2025/8/15. -// - -#include "gtest/gtest.h" -#include -#include -#include "../include/grpc_humanoid_robot_service.h" -#include "google/protobuf/timestamp.pb.h" -#include -#include - -using namespace cmvr::api; -TEST(GrpcHumanoidRobotClientTest, MyTest) { - // 连接服务端 - auto channel = grpc::CreateChannel("0.0.0.0:50055", grpc::InsecureChannelCredentials()); - auto stub = cmvr::api::HumanoidRobotService::NewStub(channel); - - // 构造请求 - cmvr::api::CommandHeader_Request request; - request.set_device_id("hc01"); - *request.mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime(); - - cmvr::api::CommandHeader_Feedback response; - grpc::ClientContext context; - - // 调用 TorqueOn - grpc::Status status = stub->torqueOn(&context, request, &response); - - if (status.ok()) { - LOG(INFO) << "TorqueOn RPC succeeded." << std::endl; - LOG(INFO) << "Success: " << response.success() << std::endl; - LOG(INFO) << "Error message: " << response.error_message() << std::endl; - LOG(INFO) << "Timestamp: " << response.timestamp().seconds() << std::endl; - } else { - LOG(ERROR) << "RPC failed: " << status.error_message() << std::endl; - } -} - - -TEST(GrpcHumanoidRobotClientTest, MyMoveJTest) { - // 连接服务端 - auto channel = grpc::CreateChannel("0.0.0.0:50055", grpc::InsecureChannelCredentials()); - auto stub = cmvr::api::HumanoidRobotService::NewStub(channel); - - - // - std::vector cmds{}; - - - cmds = { - {"L_SHOULDER_P", 0.0}, - {"L_SHOULDER_R", -1.31873}, - {"L_SHOULDER_Y", 0.0}, - {"L_ELBOW_R", -0.337621}, - {"L_WRIST_P", 0.0}, - {"L_WRIST_Y", 0.000183204}, - {"L_WRIST_R", 0.0225797}, - - {"R_SHOULDER_P", 0.00203898}, - {"R_SHOULDER_R", 1.34062}, - {"R_SHOULDER_Y", 0.0}, - {"R_ELBOW_R", 0.322261}, - {"R_WRIST_P", 0.0}, - {"R_WRIST_Y", -0.000210733}, - {"R_WRIST_R", -0.0942364}, - {"WAIST_P" ,0.0}, - {"WAIST_Y" ,0.0}, - }; - - - - // 构造请求 - cmvr::api::MoveJ_Request request; - - request.mutable_header()->set_device_id("hc01"); - *request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime(); - - for (const auto& j : cmds) { - auto* jc = request.add_cmds(); - jc->set_joint_name(j.joint_name); - jc->set_rad(j.rad); - jc->set_vel(1.0); - } - request.set_vel(0.8); - request.set_acc(0.8); - - cmvr::api::MoveJ_Response response; - grpc::ClientContext context; - - // 调用 - grpc::Status status = stub->moveJ(&context, request, &response); - - if (status.ok()) { - LOG(INFO) << "TorqueOn RPC succeeded." << std::endl; - LOG(INFO) << "Success: " << response.mutable_header()->success() << std::endl; - LOG(INFO) << "Error message: " << response.mutable_header()->error_message() << std::endl; - LOG(INFO) << "Timestamp: " << response.mutable_header()->timestamp().seconds() << std::endl; - } else { - LOG(ERROR) << "RPC failed: " << status.error_message() << std::endl; - } -} - - -TEST(GrpcHumanoidRobotClientTest, GetJointStateTest) { - // 连接服务端 - auto channel = grpc::CreateChannel("0.0.0.0:50052", grpc::InsecureChannelCredentials()); - auto stub = cmvr::api::HumanoidRobotService::NewStub(channel); - - // 构造请求 - cmvr::api::JointRequest request; - request.mutable_header()->set_device_id("hc01"); - *request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime(); - - cmvr::api::JointResponse response; - grpc::ClientContext context; - - // 调用 getJointState - grpc::Status status = stub->getJointState(&context, request, &response); - - if (status.ok()) { - LOG(INFO) << "getJointState RPC succeeded."; - LOG(INFO) << "Success: " << response.header().success(); - LOG(INFO) << "Error message: " << response.header().error_message(); - LOG(INFO) << "Timestamp: " << response.header().timestamp().seconds(); - - // 输出每个关节状态 - for (const auto &js : response.state()) { - for (int i = 0; i < js.name_size(); ++i) { - LOG(INFO) << "Joint: " << js.name(i) - << ", Position: " << js.position(i) - << ", Velocity: " << js.velocity(i); - } - } - } else { - LOG(ERROR) << "RPC failed: " << status.error_message(); - } -} diff --git a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp deleted file mode 100644 index 487fe705..00000000 --- a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp +++ /dev/null @@ -1,377 +0,0 @@ -// -// Created by lgv on 2025/8/15. -// - - -#include "../include/grpc_humanoid_robot_service.h" -#include -#include "devices/robot/humanoid_robot/include/humanoid_robot.h" -#include "cmvr/msgs/geometry.pb.h" - -using namespace cmvr::service; -using namespace cmvr::device; -using namespace cmvr::api; -using google::protobuf::util::TimeUtil; - - -gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance()) -{ - - pinocchio_ik_ = std::make_shared("/home/linbo/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf" - ,"PELVIS_S" - ,"R_WRIST_R_S"); - pinocchio_ik_->init(); -} - - -grpc::Status gRPCHumanoidRobotServiceImpl::torqueOff(grpc::ServerContext *context, - const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) { - - grpc::Status ret = grpc::Status::OK; - - try { - auto robot = dmgr_.getDevice(request->device_id()); - robot->torqueOff(); - response->set_success(true); - response->set_error_message(""); - }catch (const std::exception& e) { - response->set_success(false); - response->set_error_message(e.what()); - ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } - *response->mutable_timestamp() = TimeUtil::GetCurrentTime(); - return ret; -} - - -grpc::Status gRPCHumanoidRobotServiceImpl::torqueOn(grpc::ServerContext *context, - const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) { - - grpc::Status ret = grpc::Status::OK; - - try { - auto robot = dmgr_.getDevice(request->device_id()); - robot->torqueOn(); - response->set_success(true); - response->set_error_message(""); - }catch (const std::exception& e) { - response->set_success(false); - response->set_error_message(e.what()); - ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } - *response->mutable_timestamp() = TimeUtil::GetCurrentTime(); - return ret; -} - - -grpc::Status gRPCHumanoidRobotServiceImpl::moveJ(grpc::ServerContext *context, - const cmvr::api::MoveJ_Request *request, cmvr::api::MoveJ_Response *response) { - grpc::Status ret = grpc::Status::OK; - - try { - auto robot = dmgr_.getDevice(request->header().device_id()); - std::vector cmds{}; - for (const auto& jc : request->cmds()) { - cmds.emplace_back(jc.joint_name(),jc.rad(),jc.vel()); - } - robot->moveJ(cmds,request->vel(),request->acc()); - - response->mutable_header()->set_success(true); - response->mutable_header()->set_error_message(""); - }catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } - *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); - return ret; -} - -grpc::Status gRPCHumanoidRobotServiceImpl::moveL(grpc::ServerContext* context, const cmvr::api::MoveL_Request* request, cmvr::api::MoveL_Response* response) -{ - - grpc::Status ret = grpc::Status::OK; - try { - auto robot = dmgr_.getDevice(request->header().device_id()); - auto ee_link = request->ee_link(); - msgs::Pose3d pose; - pose.mutable_position()->set_x(request->targetpose().x()); - pose.mutable_position()->set_y(request->targetpose().y()); - pose.mutable_position()->set_z(request->targetpose().z()); - pose.mutable_euler()->set_rx(request->targetpose().rx()); - pose.mutable_euler()->set_ry(request->targetpose().ry()); - pose.mutable_euler()->set_rz(request->targetpose().rz()); - - // robot->moveL("PELVIS_S",ee_link,pose,request->vel(),request->acc()); - response->mutable_header()->set_success(true); - response->mutable_header()->set_error_message(""); - }catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } - *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); - - return ret; - -} - - -grpc::Status gRPCHumanoidRobotServiceImpl::speedJ(grpc::ServerContext* context, const cmvr::api::SpeedJ_Request* request, cmvr::api::SpeedJ_Response* response) -{ - grpc::Status ret = grpc::Status::OK; - try - { - auto robot = dmgr_.getDevice(request->header().device_id()); - auto joint_name = request->joint_name(); - auto dir = static_cast(request->dir()); - auto vel = request->vel(); - auto acc = request->acc(); - robot->speedJ(joint_name,dir,vel,acc); - }catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } - *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); - return ret; -} - -grpc::Status gRPCHumanoidRobotServiceImpl::speedL(grpc::ServerContext* context, const cmvr::api::SpeedL_Request* request, cmvr::api::SpeedL_Response* response) -{ - grpc::Status ret = grpc::Status::OK; - try - { - auto robot = dmgr_.getDevice(request->header().device_id()); - auto ee_link = request->ee_link(); - auto dir = static_cast(request->dir()); - auto vel = request->vel(); - auto acc = request->acc(); - auto cart = static_cast(request->cart()); - - robot->setToolFrame(ee_link); - // robot->speedL(cart,dir,vel,acc); - }catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } - *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); - return ret; -} - -grpc::Status gRPCHumanoidRobotServiceImpl::getPose(grpc::ServerContext *context, - const cmvr::api::GetPose_Request *request, cmvr::api::GetPose_Response *response) { - - grpc::Status ret = grpc::Status::OK; - - try { - auto robot = dmgr_.getDevice(request->header().device_id()); - - std::vector states{}; - auto pose = robot->fk(request->base_link(),request->ee_link()); - - response->mutable_header()->set_success(true); - response->mutable_header()->set_error_message(""); - response->mutable_pose()->set_x(pose.position().x()); - response->mutable_pose()->set_y(pose.position().y()); - response->mutable_pose()->set_z(pose.position().z()); - response->mutable_pose()->set_rx(pose.euler().rx()); - response->mutable_pose()->set_ry(pose.euler().ry()); - response->mutable_pose()->set_rz(pose.euler().rz()); - - - - }catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } - *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); - return ret; - - -} - - -grpc::Status gRPCHumanoidRobotServiceImpl::getJointState(grpc::ServerContext *context, - const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) { - - grpc::Status ret = grpc::Status::OK; - - try { - auto robot = dmgr_.getDevice(request->header().device_id()); - - std::vector states{}; - robot->getJointsState(states); - - response->mutable_header()->set_success(true); - response->mutable_header()->set_error_message(""); - - for (const auto &js : states) { - - if (js.name != "WAIST_P" && js.name != "WAIST_Y") { - auto joint_msg = response->add_state(); - joint_msg->add_name(js.name); - joint_msg->add_position(js.position); - joint_msg->add_velocity(js.velocity); - } - - } - }catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } - *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); - return ret; -} - - -grpc::Status gRPCHumanoidRobotServiceImpl::calibrateZeroQ(grpc::ServerContext* context - , const cmvr::api::CalibrateZeroQ_Request* request - , cmvr::api::CalibrateZeroQ_Response* response) { - - grpc::Status ret = grpc::Status::OK; - - try { - auto robot = dmgr_.getDevice(request->header().device_id()); - - std::string joint_name = request->joint_name(); - - robot->calibrateZeroQ(joint_name); - - response->mutable_header()->set_success(true); - response->mutable_header()->set_error_message(""); - }catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } - *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); - return ret; -} - - -grpc::Status gRPCHumanoidRobotServiceImpl::servoJ(grpc::ServerContext* context - , const cmvr::api::ServoJ_Request* request - , cmvr::api::ServoJ_Response* response) -{ - grpc::Status ret = grpc::Status::OK; - try { - auto robot = dmgr_.getDevice(request->header().device_id()); - std::vector cmds{}; - for (const auto& jc : request->cmds()) { - cmds.emplace_back(jc.joint_name(),jc.rad(),jc.vel()); - } - robot->servoJ(cmds,request->vel(),0); - - response->mutable_header()->set_success(true); - response->mutable_header()->set_error_message(""); - }catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } - *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); - return ret; - -} - - -// 在 gRPC 服务实现文件中添加以下代码 - -grpc::Status gRPCHumanoidRobotServiceImpl::getPoseMatrix(grpc::ServerContext* context - , const cmvr::api::GetPoseMatrix_Request* request - , cmvr::api::GetPoseMatrix_Response* response) -{ - grpc::Status ret = grpc::Status::CANCELLED; - - return ret; -} - - -grpc::Status gRPCHumanoidRobotServiceImpl::computeForwardKinematics(grpc::ServerContext* context - , const cmvr::api::ComputeForwardKinematics_Request* request - , cmvr::api::ComputeForwardKinematics_Response* response) -{ - grpc::Status ret = grpc::Status::OK; - try { - // // 获取机械臂设备 - // auto robot = dmgr_.getDevice(request->header().device_id()); - - // 解析关节命令 - std::vector cmds{}; - for (const auto& jc : request->joints()) { - cmds.emplace_back(jc.joint_name(), jc.rad(), jc.vel()); - } - - // 如果没有关节命令，返回错误 - if (cmds.empty()) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message("No joint commands provided"); - *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); - return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, "No joint commands provided"); - } - - // 获取请求参数 - std::string base_link = request->base_link(); - std::string ee_link = request->ee_link(); - bool is_tcp = true; // 默认使用TCP - - // 从关节命令中提取关节角度 - std::vector joints_angle; - for (const auto& cmd : cmds) { - joints_angle.push_back(cmd.rad); // 使用 rad 作为关节角度 - } - - // 调用机械臂的 fk 方法计算正运动学 - Eigen::Matrix4d cur_pose; - bool success = pinocchio_ik_->fk(joints_angle, cur_pose, is_tcp); - - if (success) { - // 设置成功状态 - response->mutable_header()->set_success(true); - response->mutable_header()->set_error_message(""); - - // 填充4x4矩阵到响应 - auto* matrix = response->mutable_matrix(); - matrix->set_m00(cur_pose(0, 0)); - matrix->set_m01(cur_pose(0, 1)); - matrix->set_m02(cur_pose(0, 2)); - matrix->set_m03(cur_pose(0, 3)); - - matrix->set_m10(cur_pose(1, 0)); - matrix->set_m11(cur_pose(1, 1)); - matrix->set_m12(cur_pose(1, 2)); - matrix->set_m13(cur_pose(1, 3)); - - matrix->set_m20(cur_pose(2, 0)); - matrix->set_m21(cur_pose(2, 1)); - matrix->set_m22(cur_pose(2, 2)); - matrix->set_m23(cur_pose(2, 3)); - - matrix->set_m30(cur_pose(3, 0)); - matrix->set_m31(cur_pose(3, 1)); - matrix->set_m32(cur_pose(3, 2)); - matrix->set_m33(cur_pose(3, 3)); - - // 可选：记录日志 - // LOG(INFO) << "computeForwardKinematics success for device: " << request->header().device_id(); - } else { - // 正运动学计算失败 - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message("Forward kinematics calculation failed"); - ret = grpc::Status(grpc::StatusCode::INTERNAL, "Forward kinematics calculation failed"); - } - - } catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } - - // 设置时间戳 - *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); - return ret; -} \ No newline at end of file diff --git a/cmvr-es/service/grpc/src/grpc_microphone_service.cpp b/cmvr-es/service/grpc/src/grpc_microphone_service.cpp index fdb2e275..0d3b05a0 100644 --- a/cmvr-es/service/grpc/src/grpc_microphone_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_microphone_service.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by linbo on 2025/6/13. // Created by xtkuang on 2025/6/13. @@ -8,14 +9,28 @@ using namespace std; using namespace cmvr::device; using namespace cmvr::service; +namespace { +template +grpc::Status failResponse(ResponseT* response, const std::string& message) { + CMVR_LOG(ERROR) << "[gRPCMicroPhoneServiceImpl] " << message; + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(message); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; +} +} + gRPCMicroPhoneServiceImpl::gRPCMicroPhoneServiceImpl(): dmgr_(DeviceManager::getInstance()) {} grpc::Status gRPCMicroPhoneServiceImpl::GetStatus(grpc::ServerContext* context, const api::GetMicStateCommand_Request* request, api::GetMicStateCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (GetStatus): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (GetStatus): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Microphone device not found: " + dev_id); + } MicrophoneState state; dev->getState(state); response->mutable_state()->set_is_initialized(state.is_initialized); @@ -39,9 +54,14 @@ grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context const api::StartMicRecordingCommand_Request* request, api::StartMicRecordingCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StartRecord): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StartRecord): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); - dev->start(); + if (!dev) { + return failResponse(response, "Microphone device not found: " + dev_id); + } + if (!dev->start()) { + return failResponse(response, "Failed to start microphone: " + dev_id); + } dev->startRecording(request->file_path()); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -59,8 +79,11 @@ grpc::Status gRPCMicroPhoneServiceImpl::StopRecord(grpc::ServerContext* context, const api::StopMicRecordingCommand_Request* request, api::StopMicRecordingCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StopRecord): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StopRecord): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Microphone device not found: " + dev_id); + } dev->stopRecording(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -78,8 +101,11 @@ grpc::Status gRPCMicroPhoneServiceImpl::PauseRecord(grpc::ServerContext* context const api::PauseMicRecordingCommand_Request* request, api::PauseMicRecordingCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (PauseRecord): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (PauseRecord): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Microphone device not found: " + dev_id); + } dev->pause(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -97,8 +123,11 @@ grpc::Status gRPCMicroPhoneServiceImpl::ResumeRecord(grpc::ServerContext* contex const api::ResumeMicRecordingCommand_Request* request, api::ResumeMicRecordingCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Microphone device not found: " + dev_id); + } dev->resume(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -116,8 +145,11 @@ grpc::Status gRPCMicroPhoneServiceImpl::SetVolume(grpc::ServerContext* context, const api::SetMicPhoneVolumeCommand_Request* request, api::SetMicPhoneVolumeCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (SetVolume): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (SetVolume): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Microphone device not found: " + dev_id); + } dev->setVolume(request->volume()); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -135,8 +167,11 @@ grpc::Status gRPCMicroPhoneServiceImpl::GetVolume(grpc::ServerContext* context, const api::GetMicPhoneVolumeCommand_Request* request, api::GetMicPhoneVolumeCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (GetVolume): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (GetVolume): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Microphone device not found: " + dev_id); + } response->set_volume(dev->getVolume()); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); diff --git a/cmvr-es/service/grpc/src/grpc_speaker_service.cpp b/cmvr-es/service/grpc/src/grpc_speaker_service.cpp index e9ae8d95..abd5cf0a 100644 --- a/cmvr-es/service/grpc/src/grpc_speaker_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_speaker_service.cpp @@ -1,3 +1,4 @@ +#include "common/base/logging/logger.h" // // Created by xtkuang on 2025/6/10. // @@ -8,14 +9,28 @@ using namespace std; using namespace cmvr::device; using namespace cmvr::service; +namespace { +template +grpc::Status failResponse(ResponseT* response, const std::string& message) { + CMVR_LOG(ERROR) << "[gRPCSpeakerServiceImpl] " << message; + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(message); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; +} +} + gRPCSpeakerServiceImpl::gRPCSpeakerServiceImpl(): dmgr_(DeviceManager::getInstance()) {} grpc::Status gRPCSpeakerServiceImpl::GetStatus(grpc::ServerContext* context, const api::GetSpeakerStateCommand_Request* request, api::GetSpeakerStateCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - //LOG(INFO) << "[gRPCSpeakerServiceImpl] (GetStatus): id=" << dev_id; + //CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (GetStatus): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Speaker device not found: " + dev_id); + } SpeakerState state; dev->getState(state); response->mutable_state()->set_is_initialized(state.is_initialized); @@ -39,8 +54,11 @@ grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context, const api::PlayAudioCommand_Request* request, api::PlayAudioCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCSpeakerServiceImpl] (PlayAudio): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (PlayAudio): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Speaker device not found: " + dev_id); + } //dev->start(); dev->play(request->audio_path()); response->mutable_header()->set_success(true); @@ -59,9 +77,14 @@ grpc::Status gRPCSpeakerServiceImpl::StopPlayback(grpc::ServerContext* context, const api::StopSpeakerCommand_Request* request, api::StopSpeakerCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCSpeakerServiceImpl] (StopPlayback): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (StopPlayback): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); - dev->stop(); + if (!dev) { + return failResponse(response, "Speaker device not found: " + dev_id); + } + if (!dev->stop()) { + return failResponse(response, "Failed to stop speaker: " + dev_id); + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; @@ -78,8 +101,11 @@ grpc::Status gRPCSpeakerServiceImpl::PausePlayback(grpc::ServerContext* context, const api::PauseSpeakerCommand_Request* request, api::PauseSpeakerCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCSpeakerServiceImpl] (PausePlayback): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (PausePlayback): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Speaker device not found: " + dev_id); + } dev->pause(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -97,8 +123,11 @@ grpc::Status gRPCSpeakerServiceImpl::ResumePlayback(grpc::ServerContext* context const api::ResumeSpeakerCommand_Request* request, api::ResumeSpeakerCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCSpeakerServiceImpl] (ResumePlayback): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (ResumePlayback): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Speaker device not found: " + dev_id); + } dev->resume(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -116,8 +145,11 @@ grpc::Status gRPCSpeakerServiceImpl::SetVolume(grpc::ServerContext* context, const api::SetSpeakerVolumeCommand_Request* request, api::SetSpeakerVolumeCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCSpeakerServiceImpl] (SetVolume): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (SetVolume): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Speaker device not found: " + dev_id); + } dev->setVolume(request->volume()); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -135,8 +167,11 @@ grpc::Status gRPCSpeakerServiceImpl::GetVolume(grpc::ServerContext* context, const api::GetSpeakerVolumeCommand_Request* request, api::GetSpeakerVolumeCommand_Feedback* response) { try { string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCSpeakerServiceImpl] (GetVolume): id=" << dev_id; + CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (GetVolume): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); + if (!dev) { + return failResponse(response, "Speaker device not found: " + dev_id); + } response->set_volume(dev->getVolume()); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/src/grpc_system_service.cpp index bd8eb257..44ee5286 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_system_service.cpp @@ -14,13 +14,8 @@ grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) { try { - SystemInfo info; - dmgr_.getSystemInfo(info); - response->set_version(info.version); - response->set_system_name(info.name); - response->set_os(info.os); - response->set_kernel_version(info.kernel_version); - response->set_architecture(info.architecture); + response->set_version(dmgr_.version()); + response->set_system_name(dmgr_.name()); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; @@ -37,13 +32,6 @@ grpc::Status gRPCSystemServiceImpl::GetSystemStatus(grpc::ServerContext* context const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) { try { - SystemStatus status{}; - dmgr_.getSystemStatus(status); - response->set_cpu_usage(status.cpu_usage); - response->set_mem_total_mb(status.total_memory); - response->set_mem_used_mb(status.used_memory); - response->set_disk_total_gb(status.total_disk); - response->set_disk_used_gb(status.used_disk); std::list> dev_list; dmgr_.getDeviceList(dev_list); for (auto &pair: dev_list) { @@ -91,76 +79,10 @@ grpc::Status gRPCSystemServiceImpl::GetSystemStatus(grpc::ServerContext* context grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCSystemServiceImpl] (UpdateParams): id=" << dev_id; - auto params = request->params(); - for (auto& param : params) { - //遍历拿到需要配置的自由度,传入的是百分比0-1 - std::string param_name = param.param_name(); - std::string param_value_str; // 存储转换后的字符串值 - - // 根据 oneof 类型分支转换 - switch (param.param_value_case()) { - case cmvr::api::ConfigParam::kIntValue: { - // 整数类型：用 std::to_string 转换 - int32_t int_val = param.int_value(); - param_value_str = std::to_string(int_val); - break; - } - case cmvr::api::ConfigParam::kDoubleValue: { - // 浮点数类型：用 std::to_string 转换（也可按需用 printf 控制精度） - double double_val = param.double_value(); - param_value_str = std::to_string(double_val); - // （可选）若需控制浮点数精度（如保留2位小数），可替换为： - // char buf[64]; - // snprintf(buf, sizeof(buf), "%.2f", double_val); - // param_value_str = buf; - break; - } - case cmvr::api::ConfigParam::kStringValue: { - // 字符串类型：直接赋值（本身就是 string） - param_value_str = param.string_value(); - break; - } - case cmvr::api::ConfigParam::kBoolValue: { - // 布尔类型：转为 "true" 或 "false" - bool bool_val = param.bool_value(); - param_value_str = bool_val ? "true" : "false"; - break; - } - case cmvr::api::ConfigParam::kBytesValue: { - // 二进制类型：可选方案（二选一，根据你的需求） - // 方案1：转为十六进制字符串（推荐，二进制数据可视化） - const auto& bytes_val = param.bytes_value(); - static const char hex_chars[] = "0123456789ABCDEF"; - std::string hex_str; - hex_str.reserve(bytes_val.size() * 2); // 预分配空间，提升效率 - for (unsigned char c : bytes_val) { - hex_str.push_back(hex_chars[c >> 4]); - hex_str.push_back(hex_chars[c & 0x0F]); - } - param_value_str = hex_str; // 结果如 "A3B2C1" - break; - } - case cmvr::api::ConfigParam::PARAM_VALUE_NOT_SET: { - // 未设置值的异常场景：赋值为空字符串或标记 - param_value_str = "[UNSET]"; - LOG(WARNING) << "[UpdateParams] Param '" << param_name << "' has no value"; - break; - } - } - - dmgr_.updateParam(dev_id,std::pair(param_name,param_value_str)); - } - return grpc::Status::OK; - } - catch (std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message("UpdateParams is no longer supported. Use typed device commands or reload configuration."); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; } grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, diff --git a/cmvr-es/service/grpc/src/server_runner.cpp b/cmvr-es/service/grpc/src/server_runner.cpp deleted file mode 100644 index dc689aff..00000000 --- a/cmvr-es/service/grpc/src/server_runner.cpp +++ /dev/null @@ -1,186 +0,0 @@ -// -// Created by lgv on 3/13/26. -// - -#include "service/grpc/include/server_runner.h" - -#include -#include -#include -#include -#include -#include "device_manager/include/device_manager.h" -#include "monitor_manager/include/monitor_manager.h" -#include "service/grpc/include/grpc_camera_service.h" -#include "service/grpc/include/grpc_system_service.h" -#include "service/grpc/include/grpc_speaker_service.h" -#include "service/grpc/include/grpc_microphone_service.h" -#include "service/grpc/include/grpc_dexhand_service.h" -#include "utils/base/include/logger.h" -#include "service/grpc/include/grpc_head_service.h" -#include "service/grpc/include/grpc_humanoid_robot_service.h" -#include "service/grpc/include/grpc_hlc_service.h" -#include "json/json.h" -#include -using namespace cmvr::service; -ServerRunner::ServerRunner() = default; - -ServerRunner::ServerRunner(XmlNode cfg) { - start(std::move(cfg)); -} - -ServerRunner::~ServerRunner() { - stop(); - join(); -} - -void ServerRunner::start(XmlNode cfg) { - std::unique_lock lk(mtx_); - - if (worker_.joinable()) { - throw std::runtime_error("ServerRunner already started"); - } - - cfg_ = std::move(cfg); - stop_requested_ = false; - started_ = false; - start_failed_ = false; - running_ = false; - last_error_.clear(); - address_.clear(); - server_.reset(); - - worker_ = std::thread(&ServerRunner::threadMain, this); - - cv_.wait(lk, [this]() { - return started_ || start_failed_; - }); - - if (start_failed_) { - lk.unlock(); - if (worker_.joinable()) { - worker_.join(); - } - throw std::runtime_error(last_error_); - } -} - -void ServerRunner::stop() { - std::unique_lock lk(mtx_); - stop_requested_ = true; - - if (server_) { - auto* s = server_.get(); - lk.unlock(); - s->Shutdown(); - } -} - -void ServerRunner::join() { - if (worker_.joinable()) { - worker_.join(); - } -} - -bool ServerRunner::isRunning() const { - return running_.load(); -} - -std::string ServerRunner::address() const { - std::lock_guard lk(mtx_); - return address_; -} - -void ServerRunner::threadMain() { - using namespace cmvr::device; - using namespace cmvr::service; - using namespace cmvr::monitor; - - try { - if (!cfg_.hasChild("DeviceManager")) { - throw std::runtime_error("Device Manager node not found"); - } - - static std::once_flag reflection_once; - std::call_once(reflection_once, []() { - grpc::reflection::InitProtoReflectionServerBuilderPlugin(); - }); - - auto dmgr_cfg = cfg_.getChild("DeviceManager"); - DeviceManager::getInstance(dmgr_cfg); - - if (cfg_.hasChild("MonitorManager")) { - auto mmgr_cfg = cfg_.getChild("MonitorManager"); - MonitorManager::getInstance(mmgr_cfg); - } - - auto grpc_cfg = cfg_.getChild("gRPCServer"); - std::string port = grpc_cfg.getAttrDefault("port", "50051"); - std::string local_address = "0.0.0.0:" + port; - - auto camera_service = std::make_unique(); - auto system_service = std::make_unique(); - auto speaker_service = std::make_unique(); - auto microphone_service = std::make_unique(); - auto dexhand_service = std::make_unique(); - auto biohand_service = std::make_unique(); - auto humanoid_robot_service = std::make_unique(); - auto hlc_service = std::make_unique(); - - grpc::ServerBuilder builder; - builder.AddListeningPort(local_address, grpc::InsecureServerCredentials()); - builder.RegisterService(camera_service.get()); - builder.RegisterService(system_service.get()); - builder.RegisterService(speaker_service.get()); - builder.RegisterService(microphone_service.get()); - builder.RegisterService(dexhand_service.get()); - builder.RegisterService(biohand_service.get()); - builder.RegisterService(humanoid_robot_service.get()); - builder.RegisterService(hlc_service.get()); - - auto local_server = builder.BuildAndStart(); - if (!local_server) { - throw std::runtime_error("Failed to build and start gRPC server"); - } - - { - std::lock_guard lk(mtx_); - address_ = local_address; - server_ = std::move(local_server); - started_ = true; - running_ = true; - } - cv_.notify_all(); - - std::cout << "Server listening on " << local_address << std::endl; - - bool need_stop = false; - { - std::lock_guard lk(mtx_); - need_stop = stop_requested_; - } - if (need_stop && server_) { - server_->Shutdown(); - } - - server_->Wait(); - - { - std::lock_guard lk(mtx_); - server_.reset(); - running_ = false; - } - } - catch (const std::exception& e) { - { - std::lock_guard lk(mtx_); - last_error_ = e.what(); - start_failed_ = true; - started_ = false; - running_ = false; - server_.reset(); - } - cv_.notify_all(); - LOG(ERROR) << e.what(); - } -} diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt b/cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt index f13f7c10..962d6abc 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt +++ b/cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt @@ -40,10 +40,9 @@ install(TARGETS mujoco_viewer LIBRARY DESTINATION lib) # pthread # glog # cmvr_es::mujoco_viewer -# cmvr_es::controller +# cmvr_es::algorithms::controller # Python3::Python # Python3::NumPy #) - diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h b/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h index 7ad37d93..e505ecb8 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h +++ b/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h @@ -12,7 +12,7 @@ #include "mujoco/mujoco.h" #include "simulate/mujoco/mujoco_viewer/include/simulate.h" -#include "common/consts/constant.h" +#include "common/base/constants.h" namespace cmvr { class PiPGlfwAdapter; @@ -29,11 +29,14 @@ namespace cmvr { // 阻塞运行 void run(); + void setRunning(bool running); + void requestStop(); mjModel *model() const { return m_; } mjData *data() const { return d_; } // 同一窗口画中画：显示模型内固定相机视角（像素坐标） + void enablePiPCamera(const char *camera_name); void enablePiPCamera(const char *camera_name, int left, int bottom, diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp index 40ccf705..ed088aa1 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp +++ b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp @@ -181,6 +181,15 @@ namespace cmvr { cam_.elevation = elevation; // 俯仰角（度） } + void MuJocoViewer::enablePiPCamera(const char *camera_name) { + pip_enabled_ = true; + pip_camera_name_ = camera_name ? camera_name : ""; + pip_camera_id_ = -1; + pip_width_ = 320; + pip_height_ = 240; + pip_custom_pos_ = false; + } + void MuJocoViewer::enablePiPCamera(const char *camera_name, int left, int bottom, @@ -448,6 +457,20 @@ namespace cmvr { } } + void MuJocoViewer::setRunning(const bool running) { + if (!sim_) { + return; + } + const std::unique_lock lock(sim_->mtx); + sim_->run = running; + } + + void MuJocoViewer::requestStop() { + if (sim_) { + sim_->exitrequest.store(1); + } + } + uint64_t MuJocoViewer::getPiPCameraFrameId() const { std::lock_guard lock(pip_rgb_mtx_); diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp index 0378361a..09ced4f0 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp +++ b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp @@ -2,7 +2,7 @@ // Created by lgv on 11/25/25. // -#include "controller/include/pid_controller.h" +#include "algorithms/controllers/pid/include/pid_controller.h" #include "gtest/gtest.h" #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" @@ -12,7 +12,7 @@ #include #include -#include "common/utils/visualization/matplotlibcpp.h" +#include "common/vision/matplotlibcpp.h" namespace plt = matplotlibcpp; @@ -268,4 +268,3 @@ TEST(mujoco_viewer_test, test_view_and_pid) viewer.plotJointResponse("right_arm_joint_response.png"); } - diff --git a/cmvr-es/task/CMakeLists.txt b/cmvr-es/task/CMakeLists.txt new file mode 100644 index 00000000..9483367e --- /dev/null +++ b/cmvr-es/task/CMakeLists.txt @@ -0,0 +1,38 @@ +add_library(task + touch_screen_task/src/touch_screen_task.cpp +) + +target_include_directories(task PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(task + PUBLIC + cmvr_es::algorithms::controller + cmvr_es::common + cmvr_es::ik_solver + cmvr_es::base_motion + PRIVATE + cmvr_es::device_manager +) + +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 +) diff --git a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h new file mode 100644 index 00000000..b865e8f1 --- /dev/null +++ b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h @@ -0,0 +1,65 @@ +#ifndef CMVR_ES_GRPC_SERVER_TASK_H +#define CMVR_ES_GRPC_SERVER_TASK_H + +#include +#include +#include +#include + +#include + +#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" +#include "task/task.h" + +namespace cmvr::task { + +class GrpcServerTask final : public Task { +public: + explicit GrpcServerTask(const config::GRPCServerConfig& cfg); + ~GrpcServerTask() override; + + const std::string& id() const override { return id_; } + TaskRunMode runMode() const override { return TaskRunMode::BLOCKING_SERVICE; } + bool init() override; + bool start() override; + bool step(double dt) override; + void stop() override; + + TaskState state() const override; + bool isBusy() const override; + bool isFinished() const override; + bool isFailed() const override; + std::string stateString() const override; + std::string detailStatusString() const override; + + std::string address() const; + +private: + void waitLoop(); + void clearServices(); + + config::GRPCServerConfig cfg_; + std::string id_; + + mutable std::mutex mutex_; + TaskState state_{TaskState::UNINITIALIZED}; + std::string address_; + std::string last_error_; + std::unique_ptr server_; + std::thread wait_thread_; + + std::unique_ptr camera_service_; + std::unique_ptr system_service_; + std::unique_ptr speaker_service_; + std::unique_ptr microphone_service_; + std::unique_ptr dexhand_service_; + std::unique_ptr biohand_service_; + std::unique_ptr arm_service_; + std::unique_ptr hlc_service_; +}; + +void registerGrpcServerTaskFactory(); + +} // namespace cmvr::task + +#endif // CMVR_ES_GRPC_SERVER_TASK_H diff --git a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp new file mode 100644 index 00000000..4bab32b9 --- /dev/null +++ b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp @@ -0,0 +1,266 @@ +#include "task/grpc_server_task/include/grpc_server_task.h" + +#include + +#include + +#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" +#include "cmvr/config/task_manager_config/task_manager_config.pb.h" +#include "common/base/logging/logger.h" +#include "common/config/config_files.h" +#include "service/grpc/include/grpc_arm_service.h" +#include "service/grpc/include/grpc_camera_service.h" +#include "service/grpc/include/grpc_dexhand_service.h" +#include "service/grpc/include/grpc_head_service.h" +#include "service/grpc/include/grpc_hlc_service.h" +#include "service/grpc/include/grpc_microphone_service.h" +#include "service/grpc/include/grpc_speaker_service.h" +#include "service/grpc/include/grpc_system_service.h" +#include "task/task_factory.h" + +namespace cmvr::task { + +namespace { + +std::shared_ptr createGrpcServerTask(const config::TaskConfigEntry& entry) +{ + if (entry.id().empty()) { + CMVR_LOG(ERROR) << "[GrpcServerTask] Empty gRPC task ID"; + return nullptr; + } + if (entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[GrpcServerTask] Empty gRPC config_file for task ID: " << entry.id(); + return nullptr; + } + + config::GRPCServerRootConfig root_cfg; + if (!ConfigHelper::loadConfigFile(entry.config_file(), root_cfg)) { + CMVR_LOG(ERROR) << "[GrpcServerTask] Failed to load gRPC config: " << entry.config_file(); + return nullptr; + } + + const auto& cfg = root_cfg.grpc_server(); + if (cfg.id().empty()) { + CMVR_LOG(ERROR) << "[GrpcServerTask] gRPC config missing id: " << entry.config_file(); + return nullptr; + } + if (cfg.id() != entry.id()) { + CMVR_LOG(ERROR) << "[GrpcServerTask] gRPC task ID mismatch: manager id=" << entry.id() + << ", config id=" << cfg.id(); + return nullptr; + } + return std::make_shared(cfg); +} + +} // namespace + +GrpcServerTask::GrpcServerTask(const config::GRPCServerConfig& cfg) + : cfg_(cfg), + id_(cfg.id()) +{ + state_ = TaskState::UNINITIALIZED; +} + +GrpcServerTask::~GrpcServerTask() +{ + stop(); +} + +bool GrpcServerTask::start() +{ + std::lock_guard lock(mutex_); + if (state_ == TaskState::RUNNING) { + return true; + } + if (state_ != TaskState::IDLE && state_ != TaskState::STOPPED) { + last_error_ = "gRPC task is not initialized"; + state_ = TaskState::FAILED; + return false; + } + if (id_.empty()) { + last_error_ = "gRPC task id is empty"; + state_ = TaskState::FAILED; + return false; + } + + if (cfg_.enable_reflection()) { + static std::once_flag reflection_once; + std::call_once(reflection_once, []() { + grpc::reflection::InitProtoReflectionServerBuilderPlugin(); + }); + } + + const std::string host = cfg_.host().empty() ? "0.0.0.0" : cfg_.host(); + const std::string port = cfg_.port().empty() ? "50051" : cfg_.port(); + const std::string local_address = host + ":" + port; + + camera_service_ = std::make_unique(); + system_service_ = std::make_unique(); + speaker_service_ = std::make_unique(); + microphone_service_ = std::make_unique(); + dexhand_service_ = std::make_unique(); + biohand_service_ = std::make_unique(); + arm_service_ = std::make_unique(); + hlc_service_ = std::make_unique(); + + grpc::ServerBuilder builder; + builder.AddListeningPort(local_address, grpc::InsecureServerCredentials()); + builder.RegisterService(camera_service_.get()); + builder.RegisterService(system_service_.get()); + builder.RegisterService(speaker_service_.get()); + builder.RegisterService(microphone_service_.get()); + builder.RegisterService(dexhand_service_.get()); + builder.RegisterService(biohand_service_.get()); + builder.RegisterService(arm_service_.get()); + builder.RegisterService(hlc_service_.get()); + + server_ = builder.BuildAndStart(); + if (!server_) { + clearServices(); + last_error_ = "Failed to build and start gRPC server at " + local_address; + CMVR_LOG(ERROR) << "[GrpcServerTask] " << last_error_; + state_ = TaskState::FAILED; + return false; + } + + address_ = local_address; + state_ = TaskState::RUNNING; + CMVR_LOG(INFO) << "[GrpcServerTask] gRPC server started, address=" << address_; + wait_thread_ = std::thread(&GrpcServerTask::waitLoop, this); + return true; +} + +bool GrpcServerTask::init() +{ + std::lock_guard lock(mutex_); + if (id_.empty()) { + last_error_ = "gRPC task id is empty"; + state_ = TaskState::FAILED; + return false; + } + if (cfg_.port().empty()) { + last_error_ = "gRPC task port is empty"; + state_ = TaskState::FAILED; + return false; + } + last_error_.clear(); + state_ = TaskState::IDLE; + return true; +} + +bool GrpcServerTask::step(const double dt) +{ + (void)dt; + return true; +} + +void GrpcServerTask::stop() +{ + { + std::lock_guard lock(mutex_); + if (server_) { + server_->Shutdown(); + } + } + + if (wait_thread_.joinable()) { + wait_thread_.join(); + } + + std::lock_guard lock(mutex_); + server_.reset(); + clearServices(); + if (state_ == TaskState::RUNNING || + state_ == TaskState::IDLE || + state_ == TaskState::UNINITIALIZED) { + state_ = TaskState::STOPPED; + } +} + +TaskState GrpcServerTask::state() const +{ + std::lock_guard lock(mutex_); + return state_; +} + +bool GrpcServerTask::isBusy() const +{ + return state() == TaskState::RUNNING; +} + +bool GrpcServerTask::isFinished() const +{ + return state() == TaskState::STOPPED; +} + +bool GrpcServerTask::isFailed() const +{ + return state() == TaskState::FAILED; +} + +std::string GrpcServerTask::stateString() const +{ + return taskStateToString(state()); +} + +std::string GrpcServerTask::detailStatusString() const +{ + std::lock_guard lock(mutex_); + if (last_error_.empty()) { + return std::string(taskStateToString(state_)) + " " + address_; + } + return std::string(taskStateToString(state_)) + " " + last_error_; +} + +std::string GrpcServerTask::address() const +{ + std::lock_guard lock(mutex_); + return address_; +} + +void GrpcServerTask::waitLoop() +{ + try { + grpc::Server* server = nullptr; + { + std::lock_guard lock(mutex_); + server = server_.get(); + } + if (server) { + server->Wait(); + } + std::lock_guard lock(mutex_); + if (state_ == TaskState::RUNNING) { + state_ = TaskState::STOPPED; + } + } catch (const std::exception& e) { + std::lock_guard lock(mutex_); + last_error_ = e.what(); + state_ = TaskState::FAILED; + } catch (...) { + std::lock_guard lock(mutex_); + last_error_ = "Unknown gRPC server error"; + state_ = TaskState::FAILED; + } +} + +void GrpcServerTask::clearServices() +{ + hlc_service_.reset(); + arm_service_.reset(); + biohand_service_.reset(); + dexhand_service_.reset(); + microphone_service_.reset(); + speaker_service_.reset(); + system_service_.reset(); + camera_service_.reset(); +} + +void registerGrpcServerTaskFactory() +{ + TaskFactory::registerCreator( + config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER, + createGrpcServerTask); +} + +} // namespace cmvr::task diff --git a/cmvr-es/task/task.h b/cmvr-es/task/task.h new file mode 100644 index 00000000..a6786f4e --- /dev/null +++ b/cmvr-es/task/task.h @@ -0,0 +1,57 @@ +#ifndef CMVR_ES_TASK_H +#define CMVR_ES_TASK_H + +#include + +namespace cmvr::task { + +enum class TaskState { + UNINITIALIZED = 0, + IDLE, + RUNNING, + SUCCEEDED, + FAILED, + STOPPED +}; + +enum class TaskRunMode { + PERIODIC_STEP = 0, + BLOCKING_SERVICE +}; + +class Task { +public: + virtual ~Task() = default; + + virtual const std::string& id() const = 0; + virtual TaskRunMode runMode() const { return TaskRunMode::PERIODIC_STEP; } + virtual bool init() = 0; + virtual bool start() { return true; } + virtual bool step(double dt) = 0; + virtual void stop() = 0; + + virtual TaskState state() const = 0; + virtual bool isBusy() const = 0; + virtual bool isFinished() const = 0; + virtual bool isFailed() const = 0; + + virtual std::string stateString() const = 0; + virtual std::string detailStatusString() const = 0; +}; + +inline const char* taskStateToString(const TaskState state) +{ + switch (state) { + case TaskState::UNINITIALIZED: return "UNINITIALIZED"; + case TaskState::IDLE: return "IDLE"; + case TaskState::RUNNING: return "RUNNING"; + case TaskState::SUCCEEDED: return "SUCCEEDED"; + case TaskState::FAILED: return "FAILED"; + case TaskState::STOPPED: return "STOPPED"; + } + return "UNKNOWN"; +} + +} // namespace cmvr::task + +#endif // CMVR_ES_TASK_H diff --git a/cmvr-es/task/task_factory.h b/cmvr-es/task/task_factory.h new file mode 100644 index 00000000..e3efe733 --- /dev/null +++ b/cmvr-es/task/task_factory.h @@ -0,0 +1,102 @@ +#ifndef CMVR_ES_TASK_FACTORY_H +#define CMVR_ES_TASK_FACTORY_H + +#include +#include +#include +#include +#include +#include + +#include "cmvr/config/task_manager_config/task_manager_config.pb.h" +#include "cmvr/config/touch_screen_task_config/touch_screen_task_config.pb.h" +#include "common/base/logging/logger.h" +#include "common/config/config_files.h" +#include "task/task.h" +#include "task/touch_screen_task/include/touch_screen_task.h" + +namespace cmvr::task { + +namespace task_factory_detail { + +inline std::shared_ptr createTouchScreenTask(const config::TaskConfigEntry& entry) +{ + if (entry.id().empty()) { + CMVR_LOG(ERROR) << "[TaskFactory] Empty TouchScreen task ID"; + return nullptr; + } + if (entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[TaskFactory] Empty TouchScreen config_file for task ID: " << entry.id(); + return nullptr; + } + + config::TouchScreenTaskRootConfig root_cfg; + if (!ConfigHelper::loadConfigFile(entry.config_file(), root_cfg)) { + CMVR_LOG(ERROR) << "[TaskFactory] Failed to load TouchScreen config: " << entry.config_file(); + return nullptr; + } + CMVR_LOG(INFO) << "[TaskFactory] Load TouchScreenTask config_file=" << entry.config_file() + << ", manager_id=" << entry.id(); + + const auto& cfg = root_cfg.touch_screen_task(); + if (!cfg.has_id() || cfg.id().empty()) { + CMVR_LOG(ERROR) << "[TaskFactory] TouchScreen config missing id: " << entry.config_file(); + return nullptr; + } + if (cfg.id() != entry.id()) { + CMVR_LOG(ERROR) << "[TaskFactory] TouchScreen task ID mismatch: manager id=" << entry.id() + << ", config id=" << cfg.id(); + return nullptr; + } + + return std::make_shared(cfg); +} + +} // namespace task_factory_detail + +class TaskFactory { +public: + using Creator = std::function(const config::TaskConfigEntry& entry)>; + + static inline void registerCreator(const config::TaskConfigEntry::TaskType type, + Creator creator) + { + std::lock_guard lock(registryMutex()); + registry()[type] = std::move(creator); + } + + static inline std::shared_ptr create(const config::TaskConfigEntry& entry) + { + switch (entry.type()) { + case config::TaskConfigEntry::TASK_TYPE_TOUCH_SCREEN: + return task_factory_detail::createTouchScreenTask(entry); + default: + break; + } + + std::lock_guard lock(registryMutex()); + const auto it = registry().find(entry.type()); + if (it != registry().end()) { + return it->second(entry); + } + CMVR_LOG(ERROR) << "[TaskFactory] Unsupported task type for config_file: " << entry.config_file(); + return nullptr; + } + +private: + static inline std::unordered_map& registry() + { + static std::unordered_map creators; + return creators; + } + + static inline std::mutex& registryMutex() + { + static std::mutex mutex; + return mutex; + } +}; + +} // namespace cmvr::task + +#endif // CMVR_ES_TASK_FACTORY_H diff --git a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h new file mode 100644 index 00000000..8319bd1e --- /dev/null +++ b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h @@ -0,0 +1,180 @@ +#pragma once + +#ifndef CMVR_ES_TOUCH_SCREEN_TASK_H +#define CMVR_ES_TOUCH_SCREEN_TASK_H + +#include +#include +#include +#include +#include +#include + +#include + +#include "cmvr/config/touch_screen_task_config/touch_screen_task_config.pb.h" +#include "algorithms/controllers/ibvs/include/ibvs_controller.h" +#include "devices/camera/abstract_camera.h" +#include "devices/dexhand/abstract_dexhand.h" +#include "devices/arm/robot_arm.h" +#include "task/task.h" +#include "algorithms/perception/apriltag/include/apriltag_perception.h" +#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h" + +namespace cmvr::task { + +class TouchScreenTask : public Task { +public: + enum class Phase { + IDLE = 0, // 空闲，尚未开始任务。 + ALIGNING, // 视觉对准阶段：持续 IBVS 对齐目标点。 + ALIGN_REACHED, // 视觉对准已达到阈值，等待进入下一阶段。 + TOUCHING, // 前进触控阶段：沿设定方向向屏幕推进。 + DWELLING, // 已检测到接触，保持当前位置短暂停留。 + RETRACTING, // 回退阶段：沿设定回退方向离开屏幕。 + DONE, // 整个流程成功完成。 + FAILED // 流程失败并已停止。 + }; + + enum class Status { + IDLE = 0, // 空闲状态。 + NOT_INITIALIZED, // 尚未调用 init() 完成初始化。 + INVALID_CONFIG, // 配置非法，无法启动或应用参数。 + CONTROL_JOINT_MISMATCH, // 控制关节顺序与 IK 链不一致。 + ALIGN_WAITING_PERCEPTION, // 对准阶段等待相机/AprilTag 感知结果。 + ALIGN_WAITING_TRACK, // 对准阶段等待目标点跟踪恢复成功。 + ALIGN_TARGET_SETUP_FAILED,// 视觉目标设置失败，setTargetFromPointInTag 失败。 + ALIGN_COMPUTE_FAILED, // 对准阶段 IBVS 或 IK 计算失败。 + ALIGN_TIMEOUT, // 对准阶段超时仍未收敛。 + ALIGNING, // 正在执行视觉对准。 + ALIGN_REACHED, // 视觉对准完成。 + TOUCHING, // 正在向前触控。 + TACTILE_UNAVAILABLE, // 触觉数据不可用。 + TOUCH_TRIGGERED, // 已检测到接触触发。 + TOUCH_FORWARD_TIMEOUT, // 前进触控时间到，但未触发接触。 + RETRACTING, // 正在回退离开屏幕。 + DONE, // 流程成功完成。 + STOPPED, // 被外部 stop() 主动停止。 + ROBOT_STATE_FAILED, // 读取机器人状态失败。 + ROBOT_COMMAND_FAILED, // 向机器人下发控制命令失败。 + TASK_BUSY // 已有触屏流程正在运行，新的 touch 请求被拒绝。 + }; + + explicit TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg); + ~TouchScreenTask() = default; + + bool init() override; + bool init(const std::shared_ptr& arm, + const std::shared_ptr& dexhand, + const std::shared_ptr& camera); + + const std::string& id() const override { return id_; } + + bool touch(int u, int v); + bool startFromPixel(int u, int v); + bool step(double dt) override; + void stop() override; + + Phase phase() const; + Status lastStatus() const; + static const char* phaseToString(Phase phase); + static const char* statusToString(Status status); + + TaskState state() const override; + bool isBusy() const override; + bool isFinished() const override; + bool isFailed() const override; + std::string stateString() const override; + std::string detailStatusString() const override; + + int targetU() const; + int targetV() const; + double lastTouchPressureSum() const; + int lastTouchNonzeroCount() const; + int lastActiveTagId() const; + Eigen::Vector3d lastAlignErrorCamera() const; + + const std::shared_ptr& perception() const { return perception_; } + const perception::TagRelativeTarget3D& tracker() const { return tracker_; } + const IbvsController& ibvs() const { return ibvs_; } + +private: + using Clock = std::chrono::steady_clock; + + static bool validateConfig(const cmvr::config::TouchScreenTaskConfig& config); + bool isBusyUnlocked() const; + bool startFromPixelUnlocked(int u, int v); + void stopUnlocked(); + bool applyConfig(); + bool validateControlJointNames() const; + bool stepAligning(double dt); + bool stepTouching(); + bool stepDwelling(); + bool stepRetracting(); + + bool readControlledJointPositions(std::vector& q_out) const; + bool sendJointVelocity(const std::vector& qdot) const; + bool sendZeroJointVelocity() const; + void hardStopIbvsMotion(); + bool holdCurrentControlledPosition() const; + bool buildInitJointPositions(std::vector& positions_out) const; + bool moveToInitPositionIfEnabled() const; + bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const; + void logTouchingSpeedLState() const; + + bool startTouchPhase(); + bool handleTouchTriggered(bool stop_forward_motion); + bool startRetractPhase(Phase next_phase_after_retract, Status final_status_after_retract); + void enterFailed(Status status); + + bool updateTouchPressure(); + +private: + mutable std::mutex mutex_; + std::string id_; + std::shared_ptr arm_{nullptr}; + std::shared_ptr dexhand_{nullptr}; + std::shared_ptr camera_{nullptr}; + + std::shared_ptr perception_{nullptr}; + perception::TagRelativeTarget3D tracker_; + IbvsController ibvs_; + cmvr::config::TouchScreenTaskConfig config_{}; + bool config_valid_{false}; + + Phase phase_{Phase::IDLE}; + Phase phase_after_retract_{Phase::DONE}; + Status last_status_{Status::NOT_INITIALIZED}; + + bool initialized_{false}; + bool target_locked_{false}; + bool ibvs_target_initialized_{false}; + bool touch_command_started_{false}; + bool retract_command_started_{false}; + + int target_u_{-1}; + int target_v_{-1}; + int align_stable_count_{0}; + int align_debug_count_{0}; + int last_active_tag_id_{-1}; + + double last_touch_pressure_sum_{0.0}; + int last_touch_nonzero_count_{0}; + Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()}; + bool locked_target_rotation_valid_{false}; + Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()}; + bool touch_start_position_valid_{false}; + Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()}; + bool retract_start_position_valid_{false}; + Eigen::Vector3d retract_start_position_base_{Eigen::Vector3d::Zero()}; + + Clock::time_point phase_start_time_{}; + Clock::time_point last_retract_log_time_{}; + Status final_status_after_retract_{Status::DONE}; +}; + +using touch_screen_task = TouchScreenTask; + +} // namespace cmvr::task + +#endif // CMVR_ES_TOUCH_SCREEN_TASK_H 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 new file mode 100644 index 00000000..f0a4f419 --- /dev/null +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp @@ -0,0 +1,1716 @@ +#include "task/touch_screen_task/include/touch_screen_task.h" + +#include +#include +#include +#include +#include +#include + +#include "common/base/logging/logger.h" +#include "common/math/proto_geometry.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" +#include "cmvr/config/touch_screen_algorithm_config.pb.h" +#include "manager/device_manager/include/device_manager.h" +#include + +namespace cmvr::task { + +namespace { + +device::CartesianVelocity toCartesianVelocity(const Eigen::Matrix& twist) { + return {twist[0], twist[1], twist[2], twist[3], twist[4], twist[5]}; +} + +Eigen::Matrix toEigen6(const device::CartesianVelocity& twist) { + Eigen::Matrix out; + out << twist.vx, twist.vy, twist.vz, twist.wx, twist.wy, twist.wz; + return out; +} + +bool computeLinearMoveDeltaTool(const Eigen::Matrix& twist_base, + const double move_length, + Eigen::Vector3d& delta_out) { + if (!std::isfinite(move_length) || move_length <= 0.0) { + return false; + } + + const Eigen::Vector3d linear = twist_base.head<3>(); + if (!linear.allFinite()) { + return false; + } + + const double linear_norm = linear.norm(); + if (!std::isfinite(linear_norm) || linear_norm <= 1e-9) { + return false; + } + + delta_out = linear / linear_norm * move_length; + return delta_out.allFinite(); +} + +using TouchScreenTaskConfig = cmvr::config::TouchScreenTaskConfig; + +std::array toArray6(const Eigen::Matrix& value) { + return {{value[0], value[1], value[2], value[3], value[4], value[5]}}; +} + + + +std::vector controlJointNames(const TouchScreenTaskConfig& config) { + const auto& names = config.alignment().ibvs().control_joint_names(); + return {names.begin(), names.end()}; +} + +bool buildInitJointPositionsFromConfig(const TouchScreenTaskConfig& config, + std::vector& positions_out) { + std::unordered_map q_map; + q_map.reserve(static_cast(config.initialization().joint_positions_size())); + for (const auto& joint : config.initialization().joint_positions()) { + if (!joint.has_joint_name() || joint.joint_name().empty() || !joint.has_rad() || + !std::isfinite(joint.rad())) { + return false; + } + q_map[joint.joint_name()] = joint.rad(); + } + + positions_out.clear(); + positions_out.reserve(static_cast( + config.alignment().ibvs().control_joint_names_size())); + for (const auto& name : config.alignment().ibvs().control_joint_names()) { + const auto it = q_map.find(name); + if (it == q_map.end()) { + return false; + } + positions_out.push_back(it->second); + } + return !positions_out.empty(); +} + +bool isTouchTriggered(const TouchScreenTaskConfig& config, + const double resultant_force_value) { + return resultant_force_value >= config.touch().tactile().force_threshold(); +} + +double tactileForceValue(const device::AbstractDexHand::TactilePoint& point, + const cmvr::config::TouchScreenTactileCriterion criterion) { + switch (criterion) { + case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_FZ: + return static_cast(point.fz); + case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_MAGNITUDE: + return point.magnitude(); + } + return static_cast(point.fz); +} + +Eigen::Matrix3d rotationFromTargetRotvec(double rx, double ry, double rz) { + vpRotationMatrix R_visp; + R_visp.buildFrom(rx, ry, rz); + + Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + R(r, c) = R_visp[r][c]; + } + } + return R; +} + +double rotationErrorRad(const Eigen::Matrix3d& R_current, + const Eigen::Matrix3d& R_target) { + if (!R_current.allFinite() || !R_target.allFinite()) { + return std::numeric_limits::infinity(); + } + + const Eigen::Matrix3d R_err = R_current * R_target.transpose(); + const double cos_angle = std::clamp(0.5 * (R_err.trace() - 1.0), -1.0, 1.0); + return std::acos(cos_angle); +} + +Eigen::Vector3d rotvecFromRotationMatrix(const Eigen::Matrix3d& R) { + const Eigen::AngleAxisd aa(R); + if (!std::isfinite(aa.angle()) || !aa.axis().allFinite() || std::abs(aa.angle()) <= 1e-12) { + return Eigen::Vector3d::Zero(); + } + return aa.axis() * aa.angle(); +} + +bool extractProjectedYawAboutTargetNormal(const Eigen::Matrix3d& R_target, + const Eigen::Matrix3d& R_current, + double& yaw_rad_out) { + if (!R_target.allFinite() || !R_current.allFinite()) { + return false; + } + + const Eigen::Vector3d z_ref = R_target.col(2); + const Eigen::Vector3d x_ref = R_target.col(0); + const Eigen::Vector3d y_ref = R_target.col(1); + + Eigen::Vector3d in_plane = R_current.col(0) - z_ref * z_ref.dot(R_current.col(0)); + if (in_plane.norm() <= 1e-9) { + in_plane = R_current.col(1) - z_ref * z_ref.dot(R_current.col(1)); + } + const double in_plane_norm = in_plane.norm(); + if (!std::isfinite(in_plane_norm) || in_plane_norm <= 1e-9) { + return false; + } + + in_plane /= in_plane_norm; + yaw_rad_out = std::atan2(y_ref.dot(in_plane), x_ref.dot(in_plane)); + return std::isfinite(yaw_rad_out); +} + +bool appendRequestedTactileRegions( + const device::AbstractDexHand::FingerType finger, + const device::AbstractDexHand::TactileRegion region, + std::vector& regions_out) { + using DeviceTactileRegion = device::AbstractDexHand::TactileRegion; + + switch (region) { + case device::AbstractDexHand::TactileRegion::TIP: + regions_out.emplace_back(finger, DeviceTactileRegion::TIP); + return true; + case device::AbstractDexHand::TactileRegion::FINGER: + regions_out.emplace_back(finger, DeviceTactileRegion::FINGER); + return true; + case device::AbstractDexHand::TactileRegion::PAD: + regions_out.emplace_back(finger, DeviceTactileRegion::PAD); + return true; + case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE: + if (finger != device::AbstractDexHand::FingerType::THUMB) { + return false; + } + regions_out.emplace_back(finger, DeviceTactileRegion::THUMB_MIDDLE); + return true; + } + return false; +} + +perception::AprilTagPerception::DepthPolicy toDepthPolicy( + const cmvr::config::TouchScreenDepthPolicy policy) { + switch (policy) { + case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_NONE: + return perception::AprilTagPerception::DepthPolicy::NONE; + case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_PREFER: + return perception::AprilTagPerception::DepthPolicy::PREFER; + case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_REQUIRE: + return perception::AprilTagPerception::DepthPolicy::REQUIRE; + } + return perception::AprilTagPerception::DepthPolicy::NONE; +} + +perception::TagRelativeTarget3D::TargetPointMethod toTargetPointMethod( + const cmvr::config::TouchScreenTargetPointMethod method) { + switch (method) { + case cmvr::config::TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE: + return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE; + case cmvr::config::TOUCH_SCREEN_TARGET_POINT_METHOD_DEPTH_IMAGE: + return perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE; + } + return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE; +} + +device::AbstractDexHand::FingerType toFingerType(const cmvr::config::TouchScreenFingerType finger) { + switch (finger) { + case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_PINKY: + return device::AbstractDexHand::FingerType::PINKY; + case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_RING: + return device::AbstractDexHand::FingerType::RING; + case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_MIDDLE: + return device::AbstractDexHand::FingerType::MIDDLE; + case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_INDEX: + return device::AbstractDexHand::FingerType::INDEX; + case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_THUMB: + return device::AbstractDexHand::FingerType::THUMB; + } + return device::AbstractDexHand::FingerType::INDEX; +} + +const char* fingerTypeToString(const device::AbstractDexHand::FingerType finger) { + switch (finger) { + case device::AbstractDexHand::FingerType::PINKY: return "PINKY"; + case device::AbstractDexHand::FingerType::RING: return "RING"; + case device::AbstractDexHand::FingerType::MIDDLE: return "MIDDLE"; + case device::AbstractDexHand::FingerType::INDEX: return "INDEX"; + case device::AbstractDexHand::FingerType::THUMB: return "THUMB"; + case device::AbstractDexHand::FingerType::PALM: return "PALM"; + } + return "UNKNOWN"; +} + +const char* tactileRegionToString(const device::AbstractDexHand::TactileRegion region) { + switch (region) { + case device::AbstractDexHand::TactileRegion::TIP: return "TIP"; + case device::AbstractDexHand::TactileRegion::FINGER: return "FINGER"; + case device::AbstractDexHand::TactileRegion::PAD: return "PAD"; + case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE: return "THUMB_MIDDLE"; + case device::AbstractDexHand::TactileRegion::PALM_PAD: return "PALM_PAD"; + } + return "UNKNOWN"; +} + +device::AbstractDexHand::TactileRegion toTactileRegion( + const cmvr::config::TouchScreenTactileRegion region) { + switch (region) { + case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP: + return device::AbstractDexHand::TactileRegion::TIP; + case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_FINGER: + return device::AbstractDexHand::TactileRegion::FINGER; + case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_PAD: + return device::AbstractDexHand::TactileRegion::PAD; + case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE: + return device::AbstractDexHand::TactileRegion::THUMB_MIDDLE; + } + return device::AbstractDexHand::TactileRegion::TIP; +} + +} // namespace + +TouchScreenTask::TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg) + : id_(cfg.id()), + tracker_(nullptr), + config_(cfg) { + config_valid_ = validateConfig(config_); + if (!config_valid_) { + last_status_ = Status::INVALID_CONFIG; + } +} + +bool TouchScreenTask::init() { + if (!config_valid_) { + last_status_ = Status::INVALID_CONFIG; + return false; + } + + const auto& devices = config_.devices(); + if (!devices.has_arm_id() || devices.arm_id().empty() || + !devices.has_camera_id() || devices.camera_id().empty() || + !devices.has_dexhand_id() || devices.dexhand_id().empty()) { + last_status_ = Status::INVALID_CONFIG; + return false; + } + + auto& dm = device::DeviceManager::getInstance(); + auto arm = dm.getDevice(devices.arm_id()); + auto dexhand = dm.getDevice(devices.dexhand_id()); + auto camera = dm.getDevice(devices.camera_id()); + if (!camera || !camera->start()) { + CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start camera: " << devices.camera_id(); + last_status_ = Status::NOT_INITIALIZED; + return false; + } + return init(arm, dexhand, camera); +} + +bool TouchScreenTask::init(const std::shared_ptr& arm, + const std::shared_ptr& dexhand, + const std::shared_ptr& camera) { + std::lock_guard lock(mutex_); + arm_ = arm; + dexhand_ = dexhand; + camera_ = camera; + + if (!config_valid_ || !arm_ || !camera_) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } + + if ((config_.initialization().before_start() || config_.initialization().after_finish()) && + config_.initialization().joint_positions().empty()) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } + + if (config_.initialization().before_start()) { + std::vector init_positions; + if (!buildInitJointPositions(init_positions)) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } + device::JointPositionCommand init_cmd{init_positions}; + device::MotionOptions options; + options.velocity = config_.initialization().velocity(); + options.acceleration = config_.initialization().acceleration(); + const auto result = arm_->moveJ(init_cmd, options); + if (!result.ok()) { + initialized_ = false; + last_status_ = Status::ROBOT_COMMAND_FAILED; + return false; + } + } + + if (config_.alignment().ibvs().camera_link().empty() || + config_.alignment().ibvs().control_joint_names().empty()) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } + + perception_ = std::make_shared(camera_); + perception_->setTagSize(config_.perception().apriltag().tag_size_m()); + + tracker_.setPerception(perception_); + tracker_.setTargetPointMethod(toTargetPointMethod( + config_.perception().apriltag().target_point_method())); + + auto pinocchio_solver = + std::dynamic_pointer_cast(arm_->kinematicsSolver()); + if (!pinocchio_solver || + !ibvs_.init(pinocchio_solver, + config_.alignment().ibvs().camera_link())) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } + + ibvs_.setPerception(perception_); + if (!validateControlJointNames()) { + initialized_ = false; + last_status_ = Status::CONTROL_JOINT_MISMATCH; + return false; + } + + initialized_ = applyConfig(); + if (initialized_) { + tracker_.clear(); + tracker_.resetActiveTagTracking(); + ibvs_.reset(); + phase_ = Phase::IDLE; + phase_after_retract_ = Phase::DONE; + final_status_after_retract_ = Status::DONE; + target_locked_ = false; + ibvs_target_initialized_ = false; + touch_command_started_ = false; + retract_command_started_ = false; + align_stable_count_ = 0; + last_active_tag_id_ = -1; + last_touch_pressure_sum_ = 0.0; + last_touch_nonzero_count_ = 0; + last_align_error_camera_.setZero(); + touch_start_position_valid_ = false; + touch_start_position_base_.setZero(); + retract_start_position_valid_ = false; + retract_start_position_base_.setZero(); + last_status_ = Status::IDLE; + } else { + last_status_ = Status::INVALID_CONFIG; + } + return initialized_; +} + +bool TouchScreenTask::touch(const int u, const int v) { + std::lock_guard lock(mutex_); + if (isBusyUnlocked()) { + return false; + } + return startFromPixelUnlocked(u, v); +} + +bool TouchScreenTask::startFromPixel(const int u, const int v) { + std::lock_guard lock(mutex_); + return startFromPixelUnlocked(u, v); +} + +bool TouchScreenTask::startFromPixelUnlocked(int u, int v) { + if (!initialized_) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + if (u < 0 || v < 0) { + last_status_ = Status::INVALID_CONFIG; + return false; + } + + stopUnlocked(); + tracker_.clear(); + tracker_.resetActiveTagTracking(); + ibvs_.reset(); + + target_u_ = u; + target_v_ = v; + target_locked_ = false; + ibvs_target_initialized_ = false; + touch_command_started_ = false; + retract_command_started_ = false; + align_stable_count_ = 0; + align_debug_count_ = 0; + last_touch_pressure_sum_ = 0.0; + last_touch_nonzero_count_ = 0; + last_active_tag_id_ = -1; + last_align_error_camera_.setZero(); + locked_target_rotation_valid_ = false; + locked_target_rotation_.setIdentity(); + touch_start_position_valid_ = false; + touch_start_position_base_.setZero(); + retract_start_position_valid_ = false; + retract_start_position_base_.setZero(); + phase_ = Phase::ALIGNING; + phase_after_retract_ = Phase::DONE; + final_status_after_retract_ = Status::DONE; + phase_start_time_ = Clock::now(); + last_status_ = Status::ALIGN_WAITING_TRACK; + return true; +} + +bool TouchScreenTask::step(const double dt) { + std::lock_guard lock(mutex_); + if (!initialized_) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + if (!std::isfinite(dt) || dt <= 0.0) { + last_status_ = Status::INVALID_CONFIG; + return false; + } + + if (dexhand_) { + const bool tactile_ok = updateTouchPressure(); + // std::cout << "[TouchScreenTask][TACTILE] finger=" + // << fingerTypeToString(toFingerType(config_.touch().tactile().finger())) + // << ", region=" << tactileRegionToString(toTactileRegion(config_.touch().tactile().region())) + // << ", ok=" << (tactile_ok ? 1 : 0) + // << ", nonzero_count=" << last_touch_nonzero_count_ + // << ", pressure_sum=" << last_touch_pressure_sum_ + // << std::endl; + } + + switch (phase_) { + case Phase::IDLE: + last_status_ = Status::IDLE; + return true; + case Phase::ALIGNING: + return stepAligning(dt); + case Phase::ALIGN_REACHED: + last_status_ = Status::ALIGN_REACHED; + if (config_.alignment().pause_when_reached()) { + return true; + } + if (!startTouchPhase()) { + enterFailed(last_status_ == Status::TACTILE_UNAVAILABLE || + last_status_ == Status::INVALID_CONFIG || + last_status_ == Status::ROBOT_STATE_FAILED + ? last_status_ + : Status::ROBOT_COMMAND_FAILED); + return false; + } + return true; + case Phase::TOUCHING: + return stepTouching(); + case Phase::DWELLING: + return stepDwelling(); + case Phase::RETRACTING: + return stepRetracting(); + case Phase::DONE: + last_status_ = Status::DONE; + return true; + case Phase::FAILED: + return false; + } + last_status_ = Status::INVALID_CONFIG; + return false; +} + +void TouchScreenTask::stop() { + std::lock_guard lock(mutex_); + stopUnlocked(); +} + +void TouchScreenTask::stopUnlocked() { + if (arm_) { + try { + arm_->stopL(0.0); + } catch (...) { + } + } + + sendZeroJointVelocity(); + ibvs_.resetTwistCommandState(); + holdCurrentControlledPosition(); + + phase_ = Phase::IDLE; + phase_after_retract_ = Phase::DONE; + final_status_after_retract_ = Status::DONE; + target_locked_ = false; + ibvs_target_initialized_ = false; + touch_command_started_ = false; + retract_command_started_ = false; + align_stable_count_ = 0; + last_active_tag_id_ = -1; + last_touch_pressure_sum_ = 0.0; + last_touch_nonzero_count_ = 0; + last_align_error_camera_.setZero(); + locked_target_rotation_valid_ = false; + locked_target_rotation_.setIdentity(); + touch_start_position_valid_ = false; + touch_start_position_base_.setZero(); + retract_start_position_valid_ = false; + retract_start_position_base_.setZero(); + last_status_ = Status::STOPPED; +} + +TouchScreenTask::Phase TouchScreenTask::phase() const { + std::lock_guard lock(mutex_); + return phase_; +} + +TouchScreenTask::Status TouchScreenTask::lastStatus() const { + std::lock_guard lock(mutex_); + return last_status_; +} + +bool TouchScreenTask::isBusyUnlocked() const { + return phase_ == Phase::ALIGNING || phase_ == Phase::ALIGN_REACHED || + phase_ == Phase::TOUCHING || phase_ == Phase::DWELLING || + phase_ == Phase::RETRACTING; +} + +bool TouchScreenTask::isBusy() const { + std::lock_guard lock(mutex_); + return isBusyUnlocked(); +} + +TaskState TouchScreenTask::state() const { + std::lock_guard lock(mutex_); + if (!initialized_) { + return TaskState::UNINITIALIZED; + } + if (isBusyUnlocked()) { + return TaskState::RUNNING; + } + if (phase_ == Phase::DONE) { + return TaskState::SUCCEEDED; + } + if (phase_ == Phase::FAILED) { + return TaskState::FAILED; + } + if (last_status_ == Status::STOPPED) { + return TaskState::STOPPED; + } + return TaskState::IDLE; +} + +bool TouchScreenTask::isFinished() const { + std::lock_guard lock(mutex_); + return phase_ == Phase::DONE; +} + +bool TouchScreenTask::isFailed() const { + std::lock_guard lock(mutex_); + return phase_ == Phase::FAILED; +} + +int TouchScreenTask::targetU() const { + std::lock_guard lock(mutex_); + return target_u_; +} + +int TouchScreenTask::targetV() const { + std::lock_guard lock(mutex_); + return target_v_; +} + +double TouchScreenTask::lastTouchPressureSum() const { + std::lock_guard lock(mutex_); + return last_touch_pressure_sum_; +} + +int TouchScreenTask::lastTouchNonzeroCount() const { + std::lock_guard lock(mutex_); + return last_touch_nonzero_count_; +} + +int TouchScreenTask::lastActiveTagId() const { + std::lock_guard lock(mutex_); + return last_active_tag_id_; +} + +Eigen::Vector3d TouchScreenTask::lastAlignErrorCamera() const { + std::lock_guard lock(mutex_); + return last_align_error_camera_; +} + +std::string TouchScreenTask::stateString() const { + return taskStateToString(state()); +} + +std::string TouchScreenTask::detailStatusString() const { + std::lock_guard lock(mutex_); + return std::string(phaseToString(phase_)) + "/" + statusToString(last_status_); +} + +const char* TouchScreenTask::phaseToString(const Phase phase) { + switch (phase) { + case Phase::IDLE: return "IDLE"; + case Phase::ALIGNING: return "ALIGNING"; + case Phase::ALIGN_REACHED: return "ALIGN_REACHED"; + case Phase::TOUCHING: return "TOUCHING"; + case Phase::DWELLING: return "DWELLING"; + case Phase::RETRACTING: return "RETRACTING"; + case Phase::DONE: return "DONE"; + case Phase::FAILED: return "FAILED"; + } + return "UNKNOWN"; +} + +const char* TouchScreenTask::statusToString(const Status status) { + switch (status) { + case Status::IDLE: return "IDLE"; + case Status::NOT_INITIALIZED: return "NOT_INITIALIZED"; + case Status::INVALID_CONFIG: return "INVALID_CONFIG"; + case Status::CONTROL_JOINT_MISMATCH: return "CONTROL_JOINT_MISMATCH"; + case Status::ALIGN_WAITING_PERCEPTION: return "ALIGN_WAITING_PERCEPTION"; + case Status::ALIGN_WAITING_TRACK: return "ALIGN_WAITING_TRACK"; + case Status::ALIGN_TARGET_SETUP_FAILED: return "ALIGN_TARGET_SETUP_FAILED"; + case Status::ALIGN_COMPUTE_FAILED: return "ALIGN_COMPUTE_FAILED"; + case Status::ALIGN_TIMEOUT: return "ALIGN_TIMEOUT"; + case Status::ALIGNING: return "ALIGNING"; + case Status::ALIGN_REACHED: return "ALIGN_REACHED"; + case Status::TOUCHING: return "TOUCHING"; + case Status::TACTILE_UNAVAILABLE: return "TACTILE_UNAVAILABLE"; + case Status::TOUCH_TRIGGERED: return "TOUCH_TRIGGERED"; + case Status::TOUCH_FORWARD_TIMEOUT: return "TOUCH_FORWARD_TIMEOUT"; + case Status::RETRACTING: return "RETRACTING"; + case Status::DONE: return "DONE"; + case Status::STOPPED: return "STOPPED"; + case Status::ROBOT_STATE_FAILED: return "ROBOT_STATE_FAILED"; + case Status::ROBOT_COMMAND_FAILED: return "ROBOT_COMMAND_FAILED"; + case Status::TASK_BUSY: return "TASK_BUSY"; + } + return "UNKNOWN"; +} + +bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& config) { + if (!config.has_id() || config.id().empty() || + !config.has_devices() || + !config.devices().has_arm_id() || config.devices().arm_id().empty() || + !config.devices().has_dexhand_id() || config.devices().dexhand_id().empty() || + !config.devices().has_camera_id() || config.devices().camera_id().empty() || + !config.has_initialization() || + !config.initialization().has_before_start() || + !config.initialization().has_after_finish() || + !config.initialization().has_velocity() || + !config.initialization().has_acceleration() || + !config.has_perception() || + !config.perception().has_apriltag() || + !config.has_alignment() || + !config.alignment().has_ibvs() || + !config.alignment().has_target() || + !config.alignment().has_error_threshold() || + !config.alignment().has_stable_frames() || + !config.alignment().has_timeout_s() || + !config.alignment().has_pause_when_reached() || + !config.has_touch() || + !config.touch().has_tactile() || + !config.touch().has_dwell_time_s() || + !config.has_retract() || + !config.retract().has_twist_tool() || + !config.retract().has_acceleration() || + !config.retract().has_duration_s()) { + return false; + } + + const auto& apriltag = config.perception().apriltag(); + const auto& alignment = config.alignment(); + const auto& ibvs = alignment.ibvs(); + const auto& target = alignment.target(); + const auto& touch = config.touch(); + const auto& tactile = touch.tactile(); + const auto& retract = config.retract(); + + if (!apriltag.has_tag_size_m() || + !apriltag.has_depth_policy() || + !apriltag.has_target_point_method() || + !target.has_position_in_camera() || + !hasVec3(target.position_in_camera()) || + !target.has_rotation_vector() || + !hasVec3(target.rotation_vector()) || + !target.has_mode() || + !hasVec6(alignment.error_threshold()) || + !ibvs.has_camera_link() || + !ibvs.has_lambda() || + !ibvs.has_mu() || + !ibvs.has_qdot_max() || + !ibvs.has_vmax6() || + !hasVec6(ibvs.vmax6()) || + !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() || + !hasMat3(ibvs.r_camera_to_urdf()) || + !tactile.has_finger() || + !tactile.has_region() || + !tactile.has_criterion() || + !tactile.has_force_threshold() || + !hasVec6(retract.twist_tool())) { + return false; + } + + if (ibvs.camera_link().empty() || + ibvs.control_joint_names().empty()) { + return false; + } + + const Eigen::Vector3d target_position = + cmvr::common::math::toEigenVec3(alignment.target().position_in_camera()); + const Eigen::Vector3d target_rotation = + cmvr::common::math::toEigenVec3(alignment.target().rotation_vector()); + const Eigen::Matrix error_threshold = + cmvr::common::math::toEigenVec6(alignment.error_threshold()); + const Eigen::Matrix3d camera_to_visp = + cmvr::common::math::toEigenMat3(ibvs.r_camera_to_visp()); + const Eigen::Matrix3d camera_to_urdf = + cmvr::common::math::toEigenMat3(ibvs.r_camera_to_urdf()); + + if (!std::isfinite(apriltag.tag_size_m()) || apriltag.tag_size_m() <= 0.0 || + !target_position.allFinite() || + !target_rotation.allFinite() || + alignment.stable_frames() <= 0 || + !std::isfinite(alignment.timeout_s()) || alignment.timeout_s() <= 0.0 || + !std::isfinite(tactile.force_threshold()) || tactile.force_threshold() < 0.0 || + !std::isfinite(touch.dwell_time_s()) || touch.dwell_time_s() < 0.0 || + !camera_to_visp.allFinite() || !camera_to_urdf.allFinite() || + !cmvr::common::math::toEigenVec6(retract.twist_tool()).allFinite() || + !std::isfinite(retract.acceleration()) || retract.acceleration() <= 0.0 || + !std::isfinite(retract.duration_s()) || retract.duration_s() < 0.0) { + return false; + } + for (int i = 0; i < error_threshold.size(); ++i) { + if (!std::isfinite(error_threshold[i]) || error_threshold[i] < 0.0) { + return false; + } + } + + if (!std::isfinite(config.initialization().velocity()) || + config.initialization().velocity() <= 0.0 || + !std::isfinite(config.initialization().acceleration()) || + config.initialization().acceleration() <= 0.0) { + return false; + } + + if (config.initialization().before_start() || config.initialization().after_finish()) { + std::vector init_positions; + if (!buildInitJointPositionsFromConfig(config, init_positions)) { + return false; + } + } + + std::vector tactile_regions; + if (!appendRequestedTactileRegions(toFingerType(tactile.finger()), + toTactileRegion(tactile.region()), + tactile_regions)) { + return false; + } + + switch (touch.motion_case()) { + case cmvr::config::TouchScreenTaskTouchConfig::kSpeedL: { + const auto& speed_l = touch.speed_l(); + if (!speed_l.has_twist_tool() || + !hasVec6(speed_l.twist_tool()) || + !speed_l.has_acceleration() || + !speed_l.has_max_distance_m()) { + return false; + } + const Eigen::Matrix twist = + cmvr::common::math::toEigenVec6(speed_l.twist_tool()); + return twist.allFinite() && + std::isfinite(speed_l.acceleration()) && + speed_l.acceleration() > 0.0 && + std::isfinite(speed_l.max_distance_m()) && + speed_l.max_distance_m() >= 0.0; + } + case cmvr::config::TouchScreenTaskTouchConfig::kMoveL: { + const auto& move_l = touch.move_l(); + if (!move_l.has_direction_tool() || + !hasVec6(move_l.direction_tool()) || + !move_l.has_distance_m() || + !move_l.has_velocity() || + !move_l.has_acceleration() || + !move_l.has_jerk()) { + return false; + } + const Eigen::Matrix direction = + cmvr::common::math::toEigenVec6(move_l.direction_tool()); + Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero(); + if (!computeLinearMoveDeltaTool(direction, move_l.distance_m(), touch_forward_delta) || + !std::isfinite(move_l.velocity()) || move_l.velocity() <= 0.0 || + !std::isfinite(move_l.acceleration()) || move_l.acceleration() <= 0.0 || + !std::isfinite(move_l.jerk()) || move_l.jerk() <= 0.0 || + move_l.joint_velocity_limits_size() != ibvs.control_joint_names_size()) { + return false; + } + for (const double qd_max_i : move_l.joint_velocity_limits()) { + if (!std::isfinite(qd_max_i) || qd_max_i <= 0.0) { + return false; + } + } + return true; + } + case cmvr::config::TouchScreenTaskTouchConfig::MOTION_NOT_SET: + default: + return false; + } +} + +bool TouchScreenTask::applyConfig() { + if (!perception_) { + return false; + } + if (!validateConfig(config_)) { + return false; + } + + if (dexhand_) { + const auto& tactile = config_.touch().tactile(); + std::vector tactile_regions; + if (!appendRequestedTactileRegions(toFingerType(tactile.finger()), + toTactileRegion(tactile.region()), + tactile_regions)) { + return false; + } + try { + dexhand_->setTactilePollingRegions(tactile_regions); + } catch (...) { + return false; + } + } + + const auto& apriltag = config_.perception().apriltag(); + const auto& ibvs = config_.alignment().ibvs(); + const auto target_position_in_camera = + cmvr::common::math::toEigenVec3(config_.alignment().target().position_in_camera()); + const Eigen::Matrix3d r_camera_to_visp = + cmvr::common::math::toEigenMat3(ibvs.r_camera_to_visp()); + const Eigen::Matrix3d r_camera_to_urdf = + cmvr::common::math::toEigenMat3(ibvs.r_camera_to_urdf()); + perception_->setTagSize(apriltag.tag_size_m()); + tracker_.setTargetPointMethod(toTargetPointMethod(apriltag.target_point_method())); + ibvs_.setLambda(ibvs.lambda()); + ibvs_.setMu(ibvs.mu()); + ibvs_.setQdotMax(ibvs.qdot_max()); + 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() + << ", arm_id=" << config_.devices().arm_id() + << ", camera_id=" << config_.devices().camera_id() + << ", tag_size_m=" << apriltag.tag_size_m() + << ", target_position_in_camera=[" << target_position_in_camera.x() + << ", " << target_position_in_camera.y() + << ", " << target_position_in_camera.z() << "]" + << ", r_camera_to_visp=[" << r_camera_to_visp(0, 0) + << ", " << r_camera_to_visp(0, 1) + << ", " << r_camera_to_visp(0, 2) + << "; " << r_camera_to_visp(1, 0) + << ", " << r_camera_to_visp(1, 1) + << ", " << r_camera_to_visp(1, 2) + << "; " << r_camera_to_visp(2, 0) + << ", " << r_camera_to_visp(2, 1) + << ", " << r_camera_to_visp(2, 2) << "]" + << ", r_camera_to_urdf=[" << r_camera_to_urdf(0, 0) + << ", " << r_camera_to_urdf(0, 1) + << ", " << r_camera_to_urdf(0, 2) + << "; " << r_camera_to_urdf(1, 0) + << ", " << r_camera_to_urdf(1, 1) + << ", " << r_camera_to_urdf(1, 2) + << "; " << r_camera_to_urdf(2, 0) + << ", " << r_camera_to_urdf(2, 1) + << ", " << r_camera_to_urdf(2, 2) << "]"; + return true; +} + +bool TouchScreenTask::validateControlJointNames() const { + std::vector solver_joint_names; + if (!ibvs_.getChainJointNames(solver_joint_names)) { + return false; + } + const std::vector task_joint_names = controlJointNames(config_); + if (solver_joint_names == task_joint_names) { + return true; + } + + std::ostringstream mismatch; + mismatch << "[TouchScreenTask] control_joint_names mismatch with IbvsController IK chain" + << ", task joints=["; + for (const auto& name : task_joint_names) { + mismatch << name << ' '; + } + mismatch << "], solver joints=["; + for (const auto& name : solver_joint_names) { + mismatch << name << ' '; + } + mismatch << ']'; + CMVR_LOG(ERROR) << mismatch.str(); + return false; +} + +bool TouchScreenTask::stepAligning(const double dt) { + const auto& alignment = config_.alignment(); + const auto target_position_in_camera = + cmvr::common::math::toEigenVec3(alignment.target().position_in_camera()); + const auto target_rotation_vector = + cmvr::common::math::toEigenVec3(alignment.target().rotation_vector()); + const auto error_threshold = + cmvr::common::math::toEigenVec6(alignment.error_threshold()); + + const auto now = Clock::now(); + const double elapsed = std::chrono::duration(now - phase_start_time_).count(); + if (elapsed > alignment.timeout_s()) { + enterFailed(Status::ALIGN_TIMEOUT); + return false; + } + const double ibvs_dt = std::clamp(dt, 0.005, 0.05); + + perception::AprilTagPerception::Options perception_options; + perception_options.depth_policy = + toDepthPolicy(config_.perception().apriltag().depth_policy()); + perception_options.detect_tags = true; + perception_options.fetch_encoded = false; + if (!perception_->update(perception_options)) { + hardStopIbvsMotion(); + last_status_ = Status::ALIGN_WAITING_PERCEPTION; + return true; + } + + bool tracking_ok = false; + if (!target_locked_) { + tracking_ok = tracker_.startTrackingFromPixel(target_u_, target_v_); + if (tracking_ok) { + target_locked_ = true; + ibvs_target_initialized_ = false; + align_stable_count_ = 0; + } + } else { + tracking_ok = tracker_.track(); + } + + if (!tracking_ok) { + hardStopIbvsMotion(); + align_stable_count_ = 0; + last_status_ = Status::ALIGN_WAITING_TRACK; + return true; + } + + const int tag_id = tracker_.activeTagId(); + last_active_tag_id_ = tag_id; + if (tag_id < 0) { + hardStopIbvsMotion(); + align_stable_count_ = 0; + last_status_ = Status::ALIGN_WAITING_TRACK; + return true; + } + + Eigen::Vector3d p_t_target = Eigen::Vector3d::Zero(); + if (!tracker_.getAnchorInTag(tag_id, p_t_target)) { + hardStopIbvsMotion(); + align_stable_count_ = 0; + last_status_ = Status::ALIGN_WAITING_TRACK; + return true; + } + + const auto* current_tag = perception_->findTag(tag_id); + if (!current_tag) { + hardStopIbvsMotion(); + align_stable_count_ = 0; + last_status_ = Status::ALIGN_WAITING_TRACK; + return true; + } + + Eigen::Matrix3d R_target = rotationFromTargetRotvec(target_rotation_vector.x(), + target_rotation_vector.y(), + target_rotation_vector.z()); + const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0); + switch (alignment.target().mode()) { + case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION: + break; + case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION: + if (!locked_target_rotation_valid_ || tracker_.lastSwitched()) { + double locked_yaw_rad = 0.0; + if (!extractProjectedYawAboutTargetNormal(R_target, R_current, locked_yaw_rad)) { + enterFailed(Status::ALIGN_TARGET_SETUP_FAILED); + return false; + } + const Eigen::Matrix3d Rz_locked = + Eigen::AngleAxisd(locked_yaw_rad, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + locked_target_rotation_ = R_target * Rz_locked; + locked_target_rotation_valid_ = true; + } + R_target = locked_target_rotation_; + break; + case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY: + // True position-only mode: keep the desired orientation equal to the + // current tag orientation every frame so IBVS does not actively try to + // correct rotational error. + R_target = R_current; + break; + } + const Eigen::Vector3d target_rotvec = rotvecFromRotationMatrix(R_target); + + ibvs_.setTrackedTagId(tag_id); + const bool refresh_target = !ibvs_target_initialized_ || tracker_.lastSwitched() || + alignment.target().mode() == + cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY; + if (refresh_target) { + if (!ibvs_.setTargetFromPointInTag(p_t_target, + target_position_in_camera, + target_rotvec.x(), + target_rotvec.y(), + target_rotvec.z())) { + enterFailed(Status::ALIGN_TARGET_SETUP_FAILED); + return false; + } + ibvs_target_initialized_ = true; + } + + std::vector q_now; + if (!readControlledJointPositions(q_now)) { + enterFailed(Status::ROBOT_STATE_FAILED); + return false; + } + + std::vector qdot_cmd; + if (!ibvs_.computeQdot(q_now, ibvs_dt, qdot_cmd)) { + switch (ibvs_.lastComputeStatus()) { + case IbvsController::ComputeStatus::NO_NEW_FRAME: + case IbvsController::ComputeStatus::NO_TAG: + case IbvsController::ComputeStatus::TAG_MISMATCH: + case IbvsController::ComputeStatus::NO_DEPTH: + hardStopIbvsMotion(); + align_stable_count_ = 0; + last_status_ = Status::ALIGN_WAITING_TRACK; + return true; + case IbvsController::ComputeStatus::OK: + case IbvsController::ComputeStatus::NOT_READY: + case IbvsController::ComputeStatus::BAD_IMAGE: + case IbvsController::ComputeStatus::INVALID_INPUT: + case IbvsController::ComputeStatus::IK_FAILED: + default: + enterFailed(Status::ALIGN_COMPUTE_FAILED); + return false; + } + } + + if ((align_debug_count_++ % 20) == 0) { + Eigen::Matrix achieved_twist_base = + Eigen::Matrix::Zero(); + bool achieved_ok = false; + auto pinocchio_solver = + arm_ ? std::dynamic_pointer_cast(arm_->kinematicsSolver()) : nullptr; + if (pinocchio_solver) { + achieved_ok = pinocchio_solver->computeTwistBaseAtQ( + q_now, + qdot_cmd, + config_.alignment().ibvs().camera_link(), + achieved_twist_base); + } + + const auto& target_c = tracker_.lastTargetInCamera(); + const Eigen::Vector3d err_c = target_c - target_position_in_camera; + const auto& v_visp = ibvs_.lastCameraTwistVisp(); + const double qdot_norm = + qdot_cmd.empty() + ? 0.0 + : Eigen::Map( + qdot_cmd.data(), + static_cast(qdot_cmd.size())).norm(); + CMVR_LOG(DEBUG) << "[TouchScreenTask][ALIGN_DEBUG]" + << " target_c=[" << target_c.x() << ", " << target_c.y() + << ", " << target_c.z() << "]" + << ", err_c=[" << err_c.x() << ", " << err_c.y() + << ", " << err_c.z() << "]" + << ", v_visp=[" << v_visp[0] << ", " << v_visp[1] + << ", " << v_visp[2] << ", " << v_visp[3] + << ", " << v_visp[4] << ", " << v_visp[5] << "]" + << ", qdot0=" << (qdot_cmd.empty() ? 0.0 : qdot_cmd.front()) + << ", qdot_norm=" << qdot_norm + << ", achieved_ok=" << achieved_ok + << ", achieved_twist_base=[" << achieved_twist_base[0] + << ", " << achieved_twist_base[1] + << ", " << achieved_twist_base[2] + << ", " << achieved_twist_base[3] + << ", " << achieved_twist_base[4] + << ", " << achieved_twist_base[5] << "]"; + } + + if (!sendJointVelocity(qdot_cmd)) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + + last_align_error_camera_ = tracker_.lastTargetInCamera() - target_position_in_camera; + const Eigen::Vector3d rot_error_vec = + rotvecFromRotationMatrix(R_target.transpose() * R_current); + bool align_ok = + std::abs(last_align_error_camera_.x()) <= error_threshold[0] && + std::abs(last_align_error_camera_.y()) <= error_threshold[1] && + std::abs(last_align_error_camera_.z()) <= error_threshold[2]; + switch (alignment.target().mode()) { + case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION: + align_ok = align_ok && + std::abs(rot_error_vec.x()) <= error_threshold[3] && + std::abs(rot_error_vec.y()) <= error_threshold[4] && + std::abs(rot_error_vec.z()) <= error_threshold[5]; + break; + case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION: + align_ok = align_ok && + std::abs(rot_error_vec.x()) <= error_threshold[3] && + std::abs(rot_error_vec.y()) <= error_threshold[4]; + break; + case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY: + break; + } + if (align_ok) { + ++align_stable_count_; + } else { + align_stable_count_ = 0; + } + + if (align_stable_count_ >= alignment.stable_frames()) { + CMVR_LOG(INFO) << "[TouchScreenTask][ALIGN_REACHED] tag_id=" << tag_id + << ", err_xyz=[" << last_align_error_camera_.x() << ", " + << last_align_error_camera_.y() << ", " + << last_align_error_camera_.z() << "]" + << ", err_rxyz=[" << rot_error_vec.x() << ", " + << rot_error_vec.y() << ", " + << rot_error_vec.z() << "]"; + hardStopIbvsMotion(); + phase_ = Phase::ALIGN_REACHED; + phase_start_time_ = Clock::now(); + touch_command_started_ = false; + last_status_ = Status::ALIGN_REACHED; + return true; + } + + last_status_ = Status::ALIGNING; + return true; +} + +bool TouchScreenTask::stepTouching() { + if (!touch_command_started_) { + if (!startTouchPhase()) { + enterFailed(last_status_ == Status::TACTILE_UNAVAILABLE || + last_status_ == Status::INVALID_CONFIG || + last_status_ == Status::ROBOT_STATE_FAILED + ? last_status_ + : Status::ROBOT_COMMAND_FAILED); + return false; + } + } + + if (!dexhand_) { + enterFailed(Status::TACTILE_UNAVAILABLE); + return false; + } + + if (isTouchTriggered(config_, last_touch_pressure_sum_)) { + if (config_.touch().motion_case() == cmvr::config::TouchScreenTaskTouchConfig::kSpeedL) { + logTouchingSpeedLState(); + } + if (!handleTouchTriggered(true)) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + return true; + } + + if (config_.touch().motion_case() == cmvr::config::TouchScreenTaskTouchConfig::kMoveL) { + last_status_ = Status::TOUCHING; + return true; + } + + const double speed_l_max_distance_m = config_.touch().speed_l().max_distance_m(); + if (speed_l_max_distance_m > 0.0) { + if (!touch_start_position_valid_) { + enterFailed(Status::ROBOT_STATE_FAILED); + return false; + } + + Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero(); + if (!readCurrentTouchPointPositionBase(current_position_base)) { + enterFailed(Status::ROBOT_STATE_FAILED); + return false; + } + + const double traveled_distance = + (current_position_base - touch_start_position_base_).norm(); + if (traveled_distance >= speed_l_max_distance_m) { + logTouchingSpeedLState(); + if (!updateTouchPressure()) { + enterFailed(Status::TACTILE_UNAVAILABLE); + return false; + } + if (isTouchTriggered(config_, last_touch_pressure_sum_)) { + if (!handleTouchTriggered(true)) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + return true; + } + if (!startRetractPhase(Phase::FAILED, Status::TOUCH_FORWARD_TIMEOUT)) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + return true; + } + } + + last_status_ = Status::TOUCHING; + return true; +} + +bool TouchScreenTask::stepDwelling() { + const double elapsed = std::chrono::duration(Clock::now() - phase_start_time_).count(); + if (elapsed < config_.touch().dwell_time_s()) { + last_status_ = Status::TOUCH_TRIGGERED; + return true; + } + + if (!startRetractPhase(Phase::DONE, Status::DONE)) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + return true; +} + +bool TouchScreenTask::stepRetracting() { + if (!retract_command_started_) { + if (!startRetractPhase(phase_after_retract_, final_status_after_retract_)) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + } + + const auto now = Clock::now(); + const double elapsed = std::chrono::duration(now - phase_start_time_).count(); + const double retract_duration_s = config_.retract().duration_s(); + if (elapsed < retract_duration_s) { + if (std::chrono::duration(now - last_retract_log_time_).count() >= 0.2) { + last_retract_log_time_ = now; + const auto cmd_base = arm_ ? arm_->getSpeedLCommandTwistBase() : device::CartesianVelocity{}; + Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero(); + const bool have_current = readCurrentTouchPointPositionBase(current_position_base); + const bool have_delta = retract_start_position_valid_ && have_current; + Eigen::Vector3d delta_base = Eigen::Vector3d::Zero(); + if (have_delta) { + delta_base = current_position_base - retract_start_position_base_; + } + if (have_delta) { + CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed + << "/" << retract_duration_s + << ", cmd_base=[" << cmd_base.vx << ", " + << cmd_base.vy << ", " << cmd_base.vz << ", " + << cmd_base.wx << ", " << cmd_base.wy << ", " + << cmd_base.wz << "]" + << ", tcp_delta_base=[" << delta_base.x() << ", " + << delta_base.y() << ", " << delta_base.z() + << "], tcp_dist=" << delta_base.norm(); + } else { + CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed + << "/" << retract_duration_s + << ", cmd_base=[" << cmd_base.vx << ", " + << cmd_base.vy << ", " << cmd_base.vz << ", " + << cmd_base.wx << ", " << cmd_base.wy << ", " + << cmd_base.wz << "]" + << ", tcp_delta_base=unavailable"; + } + } + last_status_ = Status::RETRACTING; + return true; + } + + Eigen::Vector3d final_position_base = Eigen::Vector3d::Zero(); + const bool have_final_position = readCurrentTouchPointPositionBase(final_position_base); + Eigen::Vector3d final_delta_base = Eigen::Vector3d::Zero(); + const bool have_final_delta = retract_start_position_valid_ && have_final_position; + if (have_final_delta) { + final_delta_base = final_position_base - retract_start_position_base_; + } + if (have_final_delta) { + CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed + << ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0) + << ", final_tcp_delta_base=[" << final_delta_base.x() << ", " + << final_delta_base.y() << ", " << final_delta_base.z() + << "], final_tcp_dist=" << final_delta_base.norm(); + } else { + CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed + << ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0) + << ", final_tcp_delta_base=unavailable"; + } + + try { + arm_->stopL(0.0); + } catch (...) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + holdCurrentControlledPosition(); + if ((phase_after_retract_ == Phase::DONE || phase_after_retract_ == Phase::FAILED) && + !moveToInitPositionIfEnabled()) { + phase_ = Phase::FAILED; + last_status_ = Status::ROBOT_COMMAND_FAILED; + return false; + } + phase_ = phase_after_retract_; + last_status_ = final_status_after_retract_; + return phase_ != Phase::FAILED; +} + +bool TouchScreenTask::readControlledJointPositions(std::vector& q_out) const { + if (!arm_) { + return false; + } + + const auto state = arm_->getJointState(); + const auto model = arm_->getRobotModel(); + std::unordered_map q_map; + q_map.reserve(model.joint_names.size()); + for (size_t i = 0; i < model.joint_names.size() && i < state.position.size(); ++i) { + q_map[model.joint_names[i]] = state.position[i]; + } + + const auto& control_joint_names = config_.alignment().ibvs().control_joint_names(); + q_out.resize(static_cast(control_joint_names.size())); + for (int i = 0; i < control_joint_names.size(); ++i) { + const auto it = q_map.find(control_joint_names[i]); + if (it == q_map.end()) { + return false; + } + q_out[static_cast(i)] = it->second; + } + return true; +} + +bool TouchScreenTask::sendJointVelocity(const std::vector& qdot) const { + if (!arm_ || qdot.size() != static_cast( + config_.alignment().ibvs().control_joint_names_size())) { + return false; + } + + device::JointVelocityCommand cmd; + cmd.velocity = qdot; + const auto result = arm_->speedJ(cmd, 0.0, 0.0); + if (!result.ok()) { + return false; + } + return true; +} + +bool TouchScreenTask::sendZeroJointVelocity() const { + std::vector zero( + static_cast(config_.alignment().ibvs().control_joint_names_size()), 0.0); + return sendJointVelocity(zero); +} + +void TouchScreenTask::hardStopIbvsMotion() { + sendZeroJointVelocity(); + ibvs_.resetTwistCommandState(); +} + +bool TouchScreenTask::readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const { + if (!arm_) { + return false; + } + + try { + const auto pose = arm_->fk(true); + p_out << pose.x, pose.y, pose.z; + return p_out.allFinite(); + } catch (...) { + return false; + } +} + +void TouchScreenTask::logTouchingSpeedLState() const { + if (!arm_ || config_.touch().motion_case() != cmvr::config::TouchScreenTaskTouchConfig::kSpeedL || + phase_ != Phase::TOUCHING) { + return; + } + + const Eigen::Matrix cmd_twist_base = toEigen6(arm_->getSpeedLCommandTwistBase()); + Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero(); + const bool have_current_position = readCurrentTouchPointPositionBase(current_position_base); + const bool have_delta = touch_start_position_valid_ && have_current_position; + Eigen::Vector3d cumulative_delta_base = Eigen::Vector3d::Zero(); + if (have_delta) { + cumulative_delta_base = current_position_base - touch_start_position_base_; + } + + std::ostringstream state_log; + state_log << "[TouchScreenTask][TOUCHING] speedl_cmd_base=[" + << cmd_twist_base[0] << ", " + << cmd_twist_base[1] << ", " + << cmd_twist_base[2] << ", " + << cmd_twist_base[3] << ", " + << cmd_twist_base[4] << ", " + << cmd_twist_base[5] << "]"; + if (have_delta) { + state_log << ", cum_tcp_delta_base=[" + << cumulative_delta_base.x() << ", " + << cumulative_delta_base.y() << ", " + << cumulative_delta_base.z() << "]" + << ", cum_tcp_dist=" << cumulative_delta_base.norm(); + } else { + state_log << ", cum_tcp_delta_base=[unavailable]"; + } + CMVR_LOG(DEBUG) << state_log.str(); +} + +bool TouchScreenTask::holdCurrentControlledPosition() const { + if (!arm_) { + return false; + } + + const auto state = arm_->getJointState(); + const auto model = arm_->getRobotModel(); + std::unordered_map q_map; + q_map.reserve(model.joint_names.size()); + for (size_t i = 0; i < model.joint_names.size() && i < state.position.size(); ++i) { + q_map[model.joint_names[i]] = state.position[i]; + } + + device::JointPositionCommand joints; + joints.position.reserve(static_cast( + config_.alignment().ibvs().control_joint_names_size())); + for (const auto& name : config_.alignment().ibvs().control_joint_names()) { + const auto it = q_map.find(name); + if (it == q_map.end()) { + return false; + } + joints.position.push_back(it->second); + } + + const auto result = arm_->servoJ(joints); + if (!result.ok()) { + return false; + } + return true; +} + +bool TouchScreenTask::buildInitJointPositions(std::vector& positions_out) const { + return buildInitJointPositionsFromConfig(config_, positions_out); +} + +bool TouchScreenTask::moveToInitPositionIfEnabled() const { + if (!config_.initialization().after_finish()) { + return true; + } + std::vector init_positions; + if (!arm_ || !buildInitJointPositions(init_positions)) { + return false; + } + + device::JointPositionCommand init_cmd{init_positions}; + device::MotionOptions options; + options.velocity = config_.initialization().velocity(); + options.acceleration = config_.initialization().acceleration(); + const auto result = arm_->moveJ(init_cmd, options); + if (!result.ok()) { + return false; + } + return true; +} + +bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) { + if (stop_forward_motion) { + const auto result = arm_->stopL(0.0); + if (!result.ok()) { + return false; + } + } + + phase_ = Phase::DWELLING; + phase_start_time_ = Clock::now(); + last_status_ = Status::TOUCH_TRIGGERED; + return true; +} + +bool TouchScreenTask::startTouchPhase() { + if (!arm_) { + last_status_ = Status::ROBOT_COMMAND_FAILED; + return false; + } + + phase_ = Phase::TOUCHING; + phase_start_time_ = Clock::now(); + touch_command_started_ = true; + retract_command_started_ = false; + retract_start_position_valid_ = false; + retract_start_position_base_.setZero(); + last_status_ = Status::TOUCHING; + + if (config_.touch().motion_case() == cmvr::config::TouchScreenTaskTouchConfig::kSpeedL) { + touch_start_position_valid_ = readCurrentTouchPointPositionBase(touch_start_position_base_); + const auto& speed_l = config_.touch().speed_l(); + if (speed_l.max_distance_m() > 0.0 && !touch_start_position_valid_) { + CMVR_LOG(ERROR) << "[TouchScreenTask] startTouchPhase failed: cannot read touch start pose " + << "for speedL distance-based touching"; + last_status_ = Status::ROBOT_STATE_FAILED; + return false; + } + const auto result = arm_->speedL(toCartesianVelocity( + cmvr::common::math::toEigenVec6(speed_l.twist_tool())), + speed_l.acceleration(), + 0.0, + device::FrameType::Tool); + if (!result.ok()) { + last_status_ = Status::ROBOT_COMMAND_FAILED; + return false; + } + return true; + } + + Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero(); + const auto& move_l = config_.touch().move_l(); + if (!computeLinearMoveDeltaTool(cmvr::common::math::toEigenVec6(move_l.direction_tool()), + move_l.distance_m(), + touch_forward_delta)) { + last_status_ = Status::INVALID_CONFIG; + return false; + } + + device::CartesianPose pose_cmd; + pose_cmd.x = touch_forward_delta.x(); + pose_cmd.y = touch_forward_delta.y(); + pose_cmd.z = touch_forward_delta.z(); + device::MotionOptions options; + options.velocity = move_l.velocity(); + options.acceleration = move_l.acceleration(); + options.jerk = move_l.jerk(); + options.joint_velocity_limits.assign(move_l.joint_velocity_limits().begin(), + move_l.joint_velocity_limits().end()); + const auto result = arm_->moveL(pose_cmd, options, device::FrameType::Tool); + if (!result.ok()) { + last_status_ = Status::ROBOT_COMMAND_FAILED; + return false; + } + + if (!moveToInitPositionIfEnabled()) { + last_status_ = Status::ROBOT_COMMAND_FAILED; + return false; + } + + phase_ = Phase::DONE; + touch_command_started_ = false; + retract_command_started_ = false; + retract_start_position_valid_ = false; + retract_start_position_base_.setZero(); + last_status_ = Status::DONE; + return true; +} + +bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract, + const Status final_status_after_retract) { + if (!arm_) { + return false; + } + + const auto& retract = config_.retract(); + retract_start_position_valid_ = readCurrentTouchPointPositionBase(retract_start_position_base_); + const auto retract_cmd = toCartesianVelocity( + cmvr::common::math::toEigenVec6(retract.twist_tool())); + if (retract_start_position_valid_) { + CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=[" + << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz + << ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz + << "], acceleration=" << retract.acceleration() + << ", duration_s=" << retract.duration_s() + << ", start_tcp_base=[" << retract_start_position_base_.x() << ", " + << retract_start_position_base_.y() << ", " + << retract_start_position_base_.z() << "]"; + } else { + CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=[" + << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz + << ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz + << "], acceleration=" << retract.acceleration() + << ", duration_s=" << retract.duration_s() + << ", start_tcp_base=unavailable"; + } + + const auto result = arm_->speedL(retract_cmd, + retract.acceleration(), + 0.0, + device::FrameType::Tool); + if (!result.ok()) { + CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] speedL failed: " + << result.message; + return false; + } + + phase_ = Phase::RETRACTING; + phase_after_retract_ = next_phase_after_retract; + final_status_after_retract_ = final_status_after_retract; + phase_start_time_ = Clock::now(); + last_retract_log_time_ = phase_start_time_; + retract_command_started_ = true; + last_status_ = Status::RETRACTING; + return true; +} + +void TouchScreenTask::enterFailed(const Status status) { + try { + if (arm_) { + arm_->stopL(0.0); + } + } catch (...) { + } + hardStopIbvsMotion(); + holdCurrentControlledPosition(); + phase_ = Phase::FAILED; + touch_command_started_ = false; + retract_command_started_ = false; + last_status_ = moveToInitPositionIfEnabled() ? status : Status::ROBOT_COMMAND_FAILED; +} + +bool TouchScreenTask::updateTouchPressure() { + last_touch_pressure_sum_ = 0.0; + last_touch_nonzero_count_ = 0; + if (!dexhand_) { + return false; + } + + const auto& tactile = config_.touch().tactile(); + std::vector tactile_regions; + if (!appendRequestedTactileRegions(toFingerType(tactile.finger()), + toTactileRegion(tactile.region()), + tactile_regions)) { + return false; + } + + double resultant_value = 0.0; + double resultant_fz = 0.0; + try { + for (const auto& tactile_region : tactile_regions) { + const auto resultant_force = dexhand_->getResultantForce(tactile_region.first, tactile_region.second); + resultant_value += tactileForceValue(resultant_force, tactile.criterion()); + resultant_fz += static_cast(resultant_force.fz); + } + } catch (...) { + return false; + } + + last_touch_nonzero_count_ = std::abs(resultant_value) > 1e-9 ? 1 : 0; + last_touch_pressure_sum_ = resultant_value; + if (phase_ == Phase::TOUCHING) { + CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz=" << resultant_fz + << ", criterion_value=" << resultant_value + << ", threshold=" << tactile.force_threshold() + ; + } + return true; +} + +} // namespace cmvr::task 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 new file mode 100644 index 00000000..8e9b5db4 --- /dev/null +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp @@ -0,0 +1,646 @@ +#include "gtest/gtest.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "task/touch_screen_task/include/touch_screen_task.h" + +#include "common/config/config_files.h" +#include "common/vision/image_projection.h" +#include "common/io/proto_file_io.h" +#include "cmvr/config/arm_config/arm_config.pb.h" +#include "cmvr/config/motor_config/motor_config.pb.h" +#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 "manager/device_manager/include/device_manager.h" +#include "manager/task_manager/include/task_manager.h" +#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" + +namespace { + +constexpr int kRealTargetU = 1280 / 2.0; +constexpr int kRealTargetV = 720 / 2.0; + +constexpr std::size_t kDof = 7; +constexpr std::array kJointNames = { + "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", + "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R" +}; + +std::filesystem::path findProjectRoot() +{ + const std::filesystem::path marker = "model/xiaoyan_description/dual_arm.xml"; + const auto search = [&](std::filesystem::path current) { + while (!current.empty()) { + if (std::filesystem::exists(current / marker)) { + return current; + } + const auto parent = current.parent_path(); + if (parent == current) { + break; + } + current = parent; + } + return std::filesystem::path{}; + }; + + auto root = search(std::filesystem::current_path()); + if (!root.empty()) { + return root; + } + return search(std::filesystem::path(__FILE__).parent_path()); +} + +class TouchMujocoViewer final : public cmvr::MuJocoViewer { +public: + TouchMujocoViewer(const std::string& model_path, + std::shared_ptr bridge) + : MuJocoViewer(model_path.c_str()), bridge_(std::move(bridge)) + { + position_actuator_ids_.fill(-1); + qpos_ids_.fill(-1); + qvel_ids_.fill(-1); + } + + std::shared_ptr waitCameraReady( + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(camera_mutex_); + if (!camera_cv_.wait_for(lock, timeout, [this] { return camera_ != nullptr; })) { + return nullptr; + } + return camera_; + } + +protected: + void initOnce(mjModel* model, mjData* data) override + { + setupCamera(2.5, -160.0, -25.0); + enablePiPCamera("hand_cam"); + + bool valid = true; + for (std::size_t i = 0; i < kDof; ++i) { + const std::string actuator_name = std::string(kJointNames[i]) + "_pos"; + position_actuator_ids_[i] = mj_name2id(model, mjOBJ_ACTUATOR, actuator_name.c_str()); + const int joint_id = mj_name2id(model, mjOBJ_JOINT, kJointNames[i]); + if (position_actuator_ids_[i] < 0 || joint_id < 0) { + valid = false; + continue; + } + qpos_ids_[i] = model->jnt_qposadr[joint_id]; + qvel_ids_[i] = model->jnt_dofadr[joint_id]; + position_reference_[i] = data->qpos[qpos_ids_[i]]; + } + + const int hand_cam_id = mj_name2id(model, mjOBJ_CAMERA, "hand_cam"); + if (hand_cam_id < 0) { + valid = false; + } + + auto camera = std::make_shared( + [this](std::vector& rgb, + std::vector& depth, + int& width, + int& height, + std::uint64_t& frame_id) { + return getPiPCameraRGBD(rgb, depth, width, height, frame_id); + }); + if (hand_cam_id >= 0) { + camera->setFovyDeg(model->cam_fovy[hand_cam_id]); + } + camera->setConsumeNewFrameOnly(false); + { + std::lock_guard lock(camera_mutex_); + camera_ = std::move(camera); + } + camera_cv_.notify_all(); + bridge_->markReady(valid); + } + + void controlCallback(mjModel* model, mjData* data) override + { + std::vector measured_position(kDof, 0.0); + std::vector measured_velocity(kDof, 0.0); + for (std::size_t i = 0; i < kDof; ++i) { + if (qpos_ids_[i] >= 0) { + measured_position[i] = data->qpos[qpos_ids_[i]]; + } + if (qvel_ids_[i] >= 0) { + measured_velocity[i] = data->qvel[qvel_ids_[i]]; + } + } + bridge_->publishMeasured(measured_position, measured_velocity); + + const auto commands = bridge_->commands(); + for (std::size_t i = 0; i < kDof; ++i) { + const int actuator_id = position_actuator_ids_[i]; + if (actuator_id < 0) { + continue; + } + + if (commands.mode[i] == cmvr::msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { + if (last_mode_[i] != cmvr::msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY && qpos_ids_[i] >= 0) { + position_reference_[i] = data->qpos[qpos_ids_[i]]; + } + position_reference_[i] += commands.velocity[i] * model->opt.timestep; + } else { + position_reference_[i] = commands.position[i]; + } + + const double lower = model->actuator_ctrlrange[2 * actuator_id]; + const double upper = model->actuator_ctrlrange[2 * actuator_id + 1]; + position_reference_[i] = std::clamp(position_reference_[i], lower, upper); + data->ctrl[actuator_id] = position_reference_[i]; + last_mode_[i] = commands.mode[i]; + } + } + +private: + std::shared_ptr bridge_; + std::array position_actuator_ids_{}; + std::array qpos_ids_{}; + std::array qvel_ids_{}; + std::array position_reference_{}; + std::array last_mode_{}; + std::mutex camera_mutex_; + std::condition_variable camera_cv_; + std::shared_ptr camera_{nullptr}; +}; + +class ZeroTouchDexHand final : public cmvr::device::AbstractDexHand { +public: + ZeroTouchDexHand() + { + id_ = "mujoco_zero_touch_dexhand"; + } + + std::string typeName() const override { return "ZeroTouchDexHand"; } + Status state() const override { return Status::STREAMING; } + std::string lastError() const override { return {}; } + + void setAngles(const std::vector& finger_joint_angles) override + { + (void)finger_joint_angles; + } + + void setTactilePollingRegions(const std::vector& regions) override + { + polling_regions_ = regions; + } + + std::vector getSensorData() override + { + std::vector data; + if (polling_regions_.empty()) { + polling_regions_.push_back({FingerType::INDEX, TactileRegion::TIP}); + } + data.reserve(polling_regions_.size()); + for (const auto& region : polling_regions_) { + data.push_back(getSensorData(region.first, region.second)); + } + return data; + } + + TactileRegionData getSensorData(const FingerType finger, const TactileRegion region) override + { + tactile_points_[0] = getResultantForce(finger, region); + TactileMatrixView view; + view.data = tactile_points_.data(); + view.rows = 1; + view.cols = 1; + return {finger, region, view, "mujoco_touch_tip"}; + } + + ResultantForce getResultantForce(const FingerType finger, const TactileRegion region) override + { + (void)finger; + (void)region; + return TactilePoint::fromFz(0); + } + +private: + std::vector polling_regions_; + std::array tactile_points_{}; +}; + +struct TagCenterPixel { + int tag_id{-1}; + int u{0}; + int v{0}; +}; + +bool detectTagCenterPixel(const std::shared_ptr& camera, + const double tag_size_m, + const std::chrono::milliseconds timeout, + TagCenterPixel& pixel_out) +{ + cmvr::perception::AprilTagPerception perception(camera); + perception.setTagSize(tag_size_m); + + cmvr::perception::AprilTagPerception::Options options; + options.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE; + options.detect_tags = true; + options.fetch_encoded = false; + + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) { + if (perception.update(options) && perception.hasTags()) { + const cv::Mat& color = perception.color(); + const int width = color.cols; + const int height = color.rows; + if (width <= 0 || height <= 0) { + return false; + } + + const Eigen::Vector2d image_center((static_cast(width) - 1.0) * 0.5, + (static_cast(height) - 1.0) * 0.5); + bool found = false; + double best_score = std::numeric_limits::infinity(); + TagCenterPixel best_pixel; + + for (const auto& tag : perception.tags()) { + const Eigen::Vector3d p_c_tag_center = tag.T_c_t.block<3, 1>(0, 3); + Eigen::Vector2d uv = Eigen::Vector2d::Zero(); + if (!cmvr::ImageProcess::projectCameraPointToPixel( + perception.intrinsics(), p_c_tag_center, uv)) { + continue; + } + if (uv.x() < 0.0 || uv.x() >= static_cast(width) || + uv.y() < 0.0 || uv.y() >= static_cast(height)) { + continue; + } + + const double score = (uv - image_center).norm(); + if (score < best_score) { + best_score = score; + best_pixel.tag_id = tag.id; + best_pixel.u = std::clamp(static_cast(std::lround(uv.x())), 0, width - 1); + best_pixel.v = std::clamp(static_cast(std::lround(uv.y())), 0, height - 1); + found = true; + } + } + + if (found) { + pixel_out = best_pixel; + return true; + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + + return false; +} + +cmvr::config::TouchScreenTaskConfig loadMujocoTouchConfig(const std::filesystem::path& project_root) +{ + cmvr::config::TouchScreenTaskRootConfig root_config; + if (!ProtoMessageIo::getProtoFromAsciiFile( + (project_root / "cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt").string(), + &root_config)) { + throw std::runtime_error("failed to load touch_screen_task_mujoco.pb.txt"); + } + return root_config.touch_screen_task(); +} + +cmvr::device::DeviceManager& getTestDeviceManager() { + static bool initialized = [] { + cmvr::ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt"); + cmvr::config::DeviceManagerRootConfig root_cfg; + if (!cmvr::ConfigHelper::loadConfigFile("manager/device_manager.pb.txt", root_cfg)) { + throw std::runtime_error("Failed to load test DeviceManagerConfig"); + } + cmvr::device::DeviceManager::getInstance(root_cfg.device_manager()); + return true; + }(); + (void)initialized; + return cmvr::device::DeviceManager::getInstance(); +} + +cmvr::task::TaskManager& getTestTaskManager() { + static bool initialized = [] { + (void)getTestDeviceManager(); + cmvr::config::TaskManagerRootConfig root_cfg; + if (!cmvr::ConfigHelper::loadConfigFile("manager/task_manager.pb.txt", root_cfg)) { + throw std::runtime_error("Failed to load test TaskManagerConfig"); + } + cmvr::task::TaskManager::getInstance(root_cfg.task_manager()); + return true; + }(); + (void)initialized; + return cmvr::task::TaskManager::getInstance(); +} + +void run_touch_once(int u, int v) { + + auto task = getTestTaskManager().getTouchScreenTask(); + if (!task) { + CMVR_LOG(ERROR) << "[TouchScreenTaskTest] TouchScreenTask not found"; + return; + } + + ASSERT_TRUE(task->touch(u, v)) << "touch failed"; + + bool align_reached = false; + bool touch_triggered = false; + auto last_step_time = std::chrono::steady_clock::now(); + bool first_step = true; + while (task->isBusy()) { + const auto now = std::chrono::steady_clock::now(); + double dt = 0.02; + if (!first_step) { + dt = std::chrono::duration(now - last_step_time).count(); + dt = std::clamp(dt, 0.005, 0.05); + } + last_step_time = now; + first_step = false; + + const bool step_ok = task->step(dt); + const auto& p_c_target = task->tracker().lastTargetInCamera(); + std::cout << "phase=" << cmvr::task::TouchScreenTask::phaseToString(task->phase()) + << ", status=" << cmvr::task::TouchScreenTask::statusToString(task->lastStatus()) + << ", active_tag=" << task->lastActiveTagId() + << ", target_c=[" << p_c_target.x() << ", " + << p_c_target.y() << ", " + << p_c_target.z() << "]" + << ", nonzero_count=" << task->lastTouchNonzeroCount() + << ", pressure_sum=" << task->lastTouchPressureSum() + << ", err_c=[" << task->lastAlignErrorCamera().x() << ", " + << task->lastAlignErrorCamera().y() << ", " + << task->lastAlignErrorCamera().z() << "]\n"; + + if (task->lastStatus() == cmvr::task::TouchScreenTask::Status::ALIGN_REACHED) { + std::cout << "align reached, target_c=[" << p_c_target.x() << ", " + << p_c_target.y() << ", " + << p_c_target.z() << "]\n"; + align_reached = true; + } + if (task->lastStatus() == cmvr::task::TouchScreenTask::Status::TOUCH_TRIGGERED) { + std::cout << "touch triggered, nonzero_count=" << task->lastTouchNonzeroCount() + << ", pressure_sum=" << task->lastTouchPressureSum() << "\n"; + touch_triggered = true; + } + if (!step_ok) { + const auto failed_status = task->lastStatus(); + task->stop(); + FAIL() << "touch flow failed, status=" + << cmvr::task::TouchScreenTask::statusToString(failed_status); + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + const auto final_status = task->lastStatus(); + ASSERT_TRUE(align_reached) + << "align was never reached, final status=" + << cmvr::task::TouchScreenTask::statusToString(final_status); + ASSERT_TRUE(touch_triggered) + << "touch was never triggered, final status=" + << cmvr::task::TouchScreenTask::statusToString(final_status); + ASSERT_TRUE(task->isFinished()) + << "touch did not finish successfully, final status=" + << cmvr::task::TouchScreenTask::statusToString(final_status); + std::cout << "touch done, final status=" + << cmvr::task::TouchScreenTask::statusToString(final_status) << "\n"; + task->stop(); +} + +} // namespace + +TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) { + const auto project_root = findProjectRoot(); + ASSERT_FALSE(project_root.empty()); + + for (const auto* file_name : {"touch_screen_task.pb.txt", + "touch_screen_task_mujoco.pb.txt"}) { + cmvr::config::TouchScreenTaskRootConfig root_config; + const auto config_path = project_root / "cmvr-es/config/tasks/touch_screen_task" / + file_name; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(config_path.string(), &root_config)) + << config_path; + + const auto& config = root_config.touch_screen_task(); + EXPECT_TRUE(config.has_initialization()); + EXPECT_TRUE(config.has_perception()); + EXPECT_TRUE(config.has_alignment()); + EXPECT_TRUE(config.alignment().has_ibvs()); + EXPECT_TRUE(config.alignment().ibvs().has_camera_link()); + EXPECT_TRUE(config.has_touch()); + EXPECT_EQ(config.touch().motion_case(), + cmvr::config::TouchScreenTaskTouchConfig::kSpeedL); + EXPECT_TRUE(config.has_retract()); + } +} + +TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) { + const auto project_root = findProjectRoot(); + ASSERT_FALSE(project_root.empty()); + cmvr::ConfigHelper::setConfigRootFromFile( + (project_root / "cmvr-es/config/cmvr_es.pb.txt").string()); + + cmvr::task::TaskManager::destroyInstance(); + cmvr::device::DeviceManager::destroyInstance(); + + cmvr::config::DeviceManagerConfig device_manager_config; + device_manager_config.set_name("touch_screen_mujoco_test"); + device_manager_config.set_version("test"); + auto* motor_entry = device_manager_config.add_devices(); + motor_entry->set_id("mujoco_motors"); + motor_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM); + motor_entry->set_config_file("devices/motor/mujoco_motors.pb.txt"); + motor_entry->set_enable(true); + auto* arm_entry = device_manager_config.add_devices(); + arm_entry->set_id("right_arm_mujoco"); + arm_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM); + arm_entry->set_config_file("devices/arm/arm_mujoco.pb.txt"); + arm_entry->set_enable(true); + + auto& device_manager = cmvr::device::DeviceManager::getInstance(device_manager_config); + auto motor_system = device_manager.getDevice("mujoco_motors"); + if (!motor_system) { + CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MotorSystem not found: mujoco_motors"; + return; + } + auto bridge = cmvr::device::MotorSystem::mujocoBridgeFor("mujoco_motors"); + if (!bridge) { + CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco bridge not found: mujoco_motors"; + return; + } + auto arm = device_manager.getDevice("right_arm_mujoco"); + if (!arm) { + CMVR_LOG(ERROR) << "[TouchScreenTaskTest] RobotArm not found: right_arm_mujoco"; + return; + } + + TouchMujocoViewer viewer( + (project_root / "model/xiaoyan_description/dual_arm.xml").string(), bridge); + + struct Outcome { + bool init_ok{false}; + bool touch_ok{false}; + bool align_reached{false}; + bool touch_triggered{false}; + bool retracting_seen{false}; + bool finished{false}; + cmvr::task::TouchScreenTask::Phase final_phase{cmvr::task::TouchScreenTask::Phase::IDLE}; + cmvr::task::TouchScreenTask::Status final_status{cmvr::task::TouchScreenTask::Status::IDLE}; + double max_pressure{0.0}; + double speedl_command_norm{0.0}; + std::string worker_error; + } outcome; + + std::thread scenario([&] { + try { + if (!bridge->waitUntilReady(std::chrono::seconds(10))) { + throw std::runtime_error("MuJoCo right-arm joints, actuators, or hand_cam are not ready"); + } + auto camera = viewer.waitCameraReady(std::chrono::seconds(10)); + if (!camera) { + throw std::runtime_error("MuJoCo camera is not ready"); + } + + const auto touch_config = loadMujocoTouchConfig(project_root); + device_manager.registerDevice(touch_config.devices().camera_id(), camera); + device_manager.registerDevice(std::make_shared()); + + cmvr::config::TaskManagerConfig task_manager_config; + auto* task_entry = task_manager_config.add_tasks(); + task_entry->set_id(touch_config.id()); + task_entry->set_type(cmvr::config::TaskConfigEntry::TASK_TYPE_TOUCH_SCREEN); + task_entry->set_run_mode(cmvr::config::TaskConfigEntry::TASK_RUN_MODE_PERIODIC_STEP); + task_entry->set_control_period_s(0.001); + task_entry->set_config_file("tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt"); + task_entry->set_enable(true); + + auto& task_manager = cmvr::task::TaskManager::getInstance(task_manager_config); + auto task = task_manager.getTouchScreenTask(touch_config.id()); + if (!task) { + throw std::runtime_error("TouchScreenTask not found: " + touch_config.id()); + } + outcome.init_ok = task->lastStatus() != cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED; + task_manager.startRunTask(); + + TagCenterPixel target_pixel; + if (!detectTagCenterPixel(camera, + touch_config.perception().apriltag().tag_size_m(), + std::chrono::seconds(3), + target_pixel)) { + throw std::runtime_error("failed to detect MuJoCo AprilTag center pixel"); + } + std::cout << "[TouchScreenTaskMujocoTest] target tag center pixel: tag_id=" + << target_pixel.tag_id + << ", u=" << target_pixel.u + << ", v=" << target_pixel.v + << std::endl; + + outcome.touch_ok = task->touch(target_pixel.u, target_pixel.v); + if (!outcome.touch_ok) { + outcome.final_status = task->lastStatus(); + return; + } + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(20); + int step_count = 0; + while (task->isBusy() && std::chrono::steady_clock::now() < deadline) { + outcome.final_phase = task->phase(); + outcome.final_status = task->lastStatus(); + outcome.max_pressure = std::max(outcome.max_pressure, task->lastTouchPressureSum()); + + const auto speedl = arm->getSpeedLCommandTwistBase(); + outcome.speedl_command_norm = std::max( + outcome.speedl_command_norm, + std::sqrt(speedl.vx * speedl.vx + speedl.vy * speedl.vy + speedl.vz * speedl.vz)); + + if (task->lastStatus() == cmvr::task::TouchScreenTask::Status::ALIGN_REACHED) { + outcome.align_reached = true; + } + if (task->lastStatus() == cmvr::task::TouchScreenTask::Status::TOUCH_TRIGGERED) { + outcome.touch_triggered = true; + } + if (task->phase() == cmvr::task::TouchScreenTask::Phase::RETRACTING) { + outcome.retracting_seen = true; + } + + if ((step_count % 20) == 0) { + const auto& target_c = task->tracker().lastTargetInCamera(); + std::cout << "[TouchScreenTaskMujocoTest] phase=" + << cmvr::task::TouchScreenTask::phaseToString(task->phase()) + << ", status=" + << cmvr::task::TouchScreenTask::statusToString(task->lastStatus()) + << ", active_tag=" << task->lastActiveTagId() + << ", target_c=[" << target_c.x() << ", " + << target_c.y() << ", " << target_c.z() << "]" + << ", pressure=" << task->lastTouchPressureSum() + << std::endl; + } + + ++step_count; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + outcome.finished = task->isFinished(); + outcome.final_phase = task->phase(); + outcome.final_status = task->lastStatus(); + task_manager.stopRunTask(); + std::cout << "[TouchScreenTaskMujocoTest] scenario finished; close the MuJoCo viewer window to finish gtest." + << std::endl; + } catch (const std::exception& error) { + try { + cmvr::task::TaskManager::getInstance().stopRunTask(); + } catch (...) { + } + outcome.worker_error = error.what(); + std::cerr << "[TouchScreenTaskMujocoTest] scenario error: " << outcome.worker_error + << "\nClose the MuJoCo viewer window to finish gtest." << std::endl; + } + }); + + viewer.setRunning(true); + viewer.run(); + scenario.join(); + cmvr::task::TaskManager::destroyInstance(); + cmvr::device::DeviceManager::destroyInstance(); + + std::cout << "[TouchScreenTaskMujocoTest] final phase=" + << cmvr::task::TouchScreenTask::phaseToString(outcome.final_phase) + << ", status=" + << cmvr::task::TouchScreenTask::statusToString(outcome.final_status) + << ", max_pressure=" << outcome.max_pressure + << ", max_speedl_linear=" << outcome.speedl_command_norm + << std::endl; + + EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error; + EXPECT_TRUE(outcome.init_ok) + << cmvr::task::TouchScreenTask::statusToString(outcome.final_status); + EXPECT_TRUE(outcome.touch_ok) + << cmvr::task::TouchScreenTask::statusToString(outcome.final_status); + EXPECT_TRUE(outcome.align_reached); + EXPECT_GT(outcome.speedl_command_norm, 0.005); + EXPECT_FALSE(outcome.touch_triggered); + EXPECT_LE(outcome.max_pressure, 0.0); + EXPECT_TRUE(outcome.retracting_seen); + EXPECT_FALSE(outcome.finished); + EXPECT_EQ(outcome.final_phase, cmvr::task::TouchScreenTask::Phase::FAILED) + << cmvr::task::TouchScreenTask::phaseToString(outcome.final_phase); + EXPECT_EQ(outcome.final_status, cmvr::task::TouchScreenTask::Status::TOUCH_FORWARD_TIMEOUT) + << cmvr::task::TouchScreenTask::statusToString(outcome.final_status); +} + +TEST(TouchScreenTaskTest, DISABLED_RunTouchOnceOnRealRobot) { + run_touch_once(kRealTargetU, kRealTargetV); +} diff --git a/cmvr-es/utils/CMakeLists.txt b/cmvr-es/utils/CMakeLists.txt deleted file mode 100644 index faec46ab..00000000 --- a/cmvr-es/utils/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -add_library(utils STATIC - base/src/thread_pool.cpp - base/src/timer.cpp - base/src/abstract_interpolation.cpp - dynamics/src/inertial.cpp - dynamics/src/joint.cpp - dynamics/src/link.cpp - dynamics/src/robot.cpp - math/src/qp_solver.cpp - math/src/se2.cpp - math/src/se3.cpp - math/src/so3.cpp - controller/src/cartesian_controller.cpp -) - -target_include_directories(utils PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -target_link_libraries(utils PRIVATE - osqp - OsqpEigen - fcl - glog - tinyxml2 -) - -add_library(cmvr_es::utils ALIAS utils) -install(TARGETS utils LIBRARY DESTINATION lib) diff --git a/cmvr-es/utils/base/include/abstract_interpolation.h b/cmvr-es/utils/base/include/abstract_interpolation.h deleted file mode 100644 index ffce43cb..00000000 --- a/cmvr-es/utils/base/include/abstract_interpolation.h +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef INTERPOLATION_BASE_H -#define INTERPOLATION_BASE_H - -#include "../../math/include/geometry.h" -#include -#include - -namespace cmvr::math { - - /** - * 插值基类，用于机械臂位姿插值 - * 支持三维位姿(Pose3d)和二维位姿(Pose2d)的插值 - */ - class AbstractInterpolation { - public: - // 构造函数 - AbstractInterpolation(const std::vector& waypoints); - AbstractInterpolation(const std::vector& waypoints); - - // 析构函数 - virtual ~AbstractInterpolation() = default; - - /** - * 插值计算接口 - * @param t 插值参数，范围[0, 1]，0表示起点，1表示终点 - * @return 插值得到的位姿 - */ - virtual Pose3d interpolate_3d(double t) const = 0; - virtual Pose2d interpolate_2d(double t) const = 0; - - /** - * 生成轨迹点序列 - * @param num_points 生成的轨迹点数量，包括起点和终点 - * @return 轨迹点序列 - */ - std::vector generate_trajectory_3d(size_t num_points) const; - std::vector generate_trajectory_2d(size_t num_points) const; - - protected: - // 路径点存储 - std::vector waypoints_3d_; - std::vector waypoints_2d_; - - /** - * 辅助函数：线性插值单个数值 - * @param a 起始值 - * @param b 目标值 - * @param t 插值参数[0,1] - * @return 插值结果 - */ - static double linear_interpolate(double a, double b, double t); - - /** - * 辅助函数：四元数球面线性插值(slerp) - * @param q1 起始四元数 - * @param q2 目标四元数 - * @param t 插值参数[0,1] - * @return 插值结果 - */ - static Quat slerp(const Quat& q1, const Quat& q2, double t); - - /** - * 检查t的范围并进行约束 - * @param t 原始参数 - * @return 约束在[0,1]范围内的参数 - */ - static double clamp_t(double t); - }; - -} // namespace cmvr::math - -#endif // INTERPOLATION_BASE_H diff --git a/cmvr-es/utils/base/include/logger.h b/cmvr-es/utils/base/include/logger.h deleted file mode 100644 index cb6891d4..00000000 --- a/cmvr-es/utils/base/include/logger.h +++ /dev/null @@ -1,53 +0,0 @@ -// -// Created by xtkuang on 2025/6/3. -// - -#ifndef LOGGER_H -#define LOGGER_H - -#include -#include -#include -#include "rapidxml/xml_parser.h" - -using namespace std; - -inline bool initLogger(const XmlNode &cfg) { - google::InitGoogleLogging("cmvr_es"); - - std::string temp = cfg.getAttrString("dir"); - if (temp.empty()) { - std::cerr << "Error: no directory specified" << std::endl; - return false; - } else { - // 判断目录是否存在 - if (access(temp.c_str(), F_OK) != 0) { - // 创建目录 - if (mkdir(temp.c_str(), 0755) != 0) { - std::cerr << "Failed to create directory: " << temp << std::endl; - return false; - } - } - FLAGS_log_dir = temp; - } - - temp = cfg.getAttrDefault("level", "info"); - if (temp == "info") { - FLAGS_minloglevel = google::INFO; - } else if (temp == "warn") { - FLAGS_minloglevel = google::WARNING; - } else if (temp == "error") { - FLAGS_minloglevel = google::ERROR; - } else if (temp == "fatal") { - FLAGS_minloglevel = google::FATAL; - } - - FLAGS_logbufsecs = cfg.getAttrDefault("bufSize", 5); - FLAGS_max_log_size = cfg.getAttrDefault("logSize", 1024); - FLAGS_logtostderr = false; - FLAGS_alsologtostderr = true; - - return true; -} - -#endif //LOGGER_H diff --git a/cmvr-es/utils/base/include/os.h b/cmvr-es/utils/base/include/os.h deleted file mode 100644 index 760fe173..00000000 --- a/cmvr-es/utils/base/include/os.h +++ /dev/null @@ -1,33 +0,0 @@ -// -// Created by xtkuang on 2025/6/11. -// - -#ifndef OS_H -#define OS_H - -#include -#include -#include - -inline bool pathExists(const std::string& path) { - struct stat info; - return stat(path.c_str(), &info) == 0; -} - -inline std::vector splitString(const std::string& pattern, const std::string& delimiter) { - std::vector result; - if (delimiter.empty()) { - result.push_back(pattern); // 若分隔符为空，则原样返回 - return result; - } - size_t start = 0; - size_t end; - while ((end = pattern.find(delimiter, start)) != std::string::npos) { - result.push_back(pattern.substr(start, end - start)); - start = end + delimiter.length(); - } - result.push_back(pattern.substr(start)); - return result; -} - -#endif //OS_H \ No newline at end of file diff --git a/cmvr-es/utils/base/include/thread_pool.h b/cmvr-es/utils/base/include/thread_pool.h deleted file mode 100644 index 1f8fee79..00000000 --- a/cmvr-es/utils/base/include/thread_pool.h +++ /dev/null @@ -1,80 +0,0 @@ -// -// Created by xtkuang on 2025/5/13. -// - -#ifndef CMVR_ES_THREAD_POOL_H -#define CMVR_ES_THREAD_POOL_H - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#pragma warning(disable:4996) -#include "glog/logging.h" - -class InterruptFlag { -public: - void request_stop(); - [[nodiscard]] bool stop_requested() const; - -private: - std::atomic flag_ {false}; -}; - -// 提交任务（带 future 返回） -class TaskHandle { -public: - using TaskFuncType = std::function; - using ReturnType = void; - - TaskHandle(std::shared_ptr> promise) - : future_(promise->get_future()) {} - - std::future& get_future() { return future_; } - -private: - std::future future_; -}; - - -class ThreadPool { -public: - ThreadPool(size_t min_threads, size_t max_threads); - ~ThreadPool(); - - // 提交任务（不带返回值） - void submit(std::function task); - - TaskHandle submit_with_future(std::function f); - - void shutdown(); - -private: - void worker_loop(size_t id); - void monitor_loop(); - - boost::lockfree::queue*> task_queue_; - std::atomic pending_tasks_; - - std::vector threads_; - std::vector> flags_; - - std::mutex control_mutex_; - std::condition_variable control_cv_; - std::atomic shutdown_requested_ = false; - - const size_t min_threads_; - const size_t max_threads_; - - std::thread monitor_thread_; -}; - - -#endif //CMVR_ES_THREAD_POOL_H diff --git a/cmvr-es/utils/base/include/timer.h b/cmvr-es/utils/base/include/timer.h deleted file mode 100644 index 81ab7627..00000000 --- a/cmvr-es/utils/base/include/timer.h +++ /dev/null @@ -1,37 +0,0 @@ -// -// Created by xtkuang on 2025/5/14. -// - -#ifndef CMVR_ES_TIMER_H -#define CMVR_ES_TIMER_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class FDTimer { -public: - using Callback = std::function; - - FDTimer(); - ~FDTimer(); - void start(std::chrono::nanoseconds interval, Callback callback); - void stop(); - [[nodiscard]] bool is_running() const; - -private: - static timespec toTimespec(std::chrono::nanoseconds ns); - - int fd_; - std::atomic running_; - Callback callback_; - std::thread worker_thread_; -}; - -#endif //CMVR_ES_TIMER_H diff --git a/cmvr-es/utils/base/src/abstract_interpolation.cpp b/cmvr-es/utils/base/src/abstract_interpolation.cpp deleted file mode 100644 index 33ccf658..00000000 --- a/cmvr-es/utils/base/src/abstract_interpolation.cpp +++ /dev/null @@ -1,107 +0,0 @@ -#include "../include/abstract_interpolation.h" -#include -#include - -namespace cmvr::math { - -// 构造函数实现 -AbstractInterpolation::AbstractInterpolation(const std::vector& waypoints) - : waypoints_3d_(waypoints) { - if (waypoints.size() < 2) { - throw std::invalid_argument("至少需要2个路径点才能进行插值"); - } -} - -AbstractInterpolation::AbstractInterpolation(const std::vector& waypoints) - : waypoints_2d_(waypoints) { - if (waypoints.size() < 2) { - throw std::invalid_argument("至少需要2个路径点才能进行插值"); - } -} - -// 生成3D轨迹 -std::vector AbstractInterpolation::generate_trajectory_3d(size_t num_points) const { - if (num_points < 2) { - throw std::invalid_argument("轨迹点数量至少为2"); - } - - std::vector trajectory; - trajectory.reserve(num_points); - - for (size_t i = 0; i < num_points; ++i) { - double t = static_cast(i) / (num_points - 1); - trajectory.push_back(interpolate_3d(t)); - } - - return trajectory; -} - -// 生成2D轨迹 -std::vector AbstractInterpolation::generate_trajectory_2d(size_t num_points) const { - if (num_points < 2) { - throw std::invalid_argument("轨迹点数量至少为2"); - } - - std::vector trajectory; - trajectory.reserve(num_points); - - for (size_t i = 0; i < num_points; ++i) { - double t = static_cast(i) / (num_points - 1); - trajectory.push_back(interpolate_2d(t)); - } - - return trajectory; -} - -// 线性插值辅助函数 -double AbstractInterpolation::linear_interpolate(double a, double b, double t) { - t = clamp_t(t); - return a + t * (b - a); -} - -// 四元数球面线性插值 -Quat AbstractInterpolation::slerp(const Quat& q1, const Quat& q2, double t) { - t = clamp_t(t); - - Quat result; - double dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z; - - // 确保我们选择最短路径 - if (dot < 0.0) { - Quat q2_neg = {-q2.w, -q2.x, -q2.y, -q2.z}; - return slerp(q1, q2_neg, t); - } - - // 防止数值问题，接近时使用线性插值 - if (dot > 0.9995) { - result.w = linear_interpolate(q1.w, q2.w, t); - result.x = linear_interpolate(q1.x, q2.x, t); - result.y = linear_interpolate(q1.y, q2.y, t); - result.z = linear_interpolate(q1.z, q2.z, t); - return result; - } - - // 计算角度和插值系数 - double theta_0 = std::acos(dot); - double theta = theta_0 * t; - double sin_theta = std::sin(theta); - double sin_theta_0 = std::sin(theta_0); - - double s1 = std::cos(theta) - dot * sin_theta / sin_theta_0; - double s2 = sin_theta / sin_theta_0; - - result.w = s1 * q1.w + s2 * q2.w; - result.x = s1 * q1.x + s2 * q2.x; - result.y = s1 * q1.y + s2 * q2.y; - result.z = s1 * q1.z + s2 * q2.z; - - return result; -} - -// 参数范围约束 -double AbstractInterpolation::clamp_t(double t) { - return std::max(0.0, std::min(1.0, t)); -} - -} // namespace cmvr::math - \ No newline at end of file diff --git a/cmvr-es/utils/base/src/thread_pool.cpp b/cmvr-es/utils/base/src/thread_pool.cpp deleted file mode 100644 index 34ede933..00000000 --- a/cmvr-es/utils/base/src/thread_pool.cpp +++ /dev/null @@ -1,102 +0,0 @@ -// -// Created by xtkuang on 2025/5/13. -// - -#include "../include/thread_pool.h" -#include - -void InterruptFlag::request_stop() { - flag_.store(true, std::memory_order_relaxed); -} - -bool InterruptFlag::stop_requested() const { - return flag_.load(std::memory_order_relaxed); -} - -ThreadPool::ThreadPool(size_t min_threads, size_t max_threads) - : min_threads_(min_threads), max_threads_(max_threads), task_queue_(1024), pending_tasks_(0) { - for (size_t i = 0; i < min_threads_; ++i) { - auto flag = std::make_unique(); - flags_.push_back(std::move(flag)); - threads_.emplace_back([this, i]() { this->worker_loop(i); }); - } - monitor_thread_ = std::thread(&ThreadPool::monitor_loop, this); -} - -ThreadPool::~ThreadPool() { - shutdown(); -} - -TaskHandle ThreadPool::submit_with_future(std::function f) { - auto promise = std::make_shared>(); - TaskHandle handle(promise); - -// auto wrapper = [promise, f](InterruptFlag& flag) mutable { -// if (flag.stop_requested()) { -// promise->set_exception(std::make_exception_ptr(std::runtime_error("Task interrupted"))); -// return; -// } -// try { -// if constexpr (std::is_same_v) { -// f(); // 直接调用，不返回任何值 -// promise->set_value(); // 无参调用 -// } else { -// promise->set_value(f()); // 非 void 类型，正常设置返回值 -// } -// } catch (...) { -// promise->set_exception(std::current_exception()); -// } -// }; -// -// submit(wrapper); - return handle; -} - -void ThreadPool::submit(std::function task) { - pending_tasks_.fetch_add(1, std::memory_order_relaxed); - task_queue_.push(new std::function(std::move(task))); -} - -void ThreadPool::worker_loop(size_t id) { - InterruptFlag& my_flag = *flags_[id]; - while (!shutdown_requested_.load() && !my_flag.stop_requested()) { - std::function* task_ptr = nullptr; - if (task_queue_.pop(task_ptr)) { - pending_tasks_.fetch_sub(1, std::memory_order_relaxed); - std::unique_ptr> task(task_ptr); - (*task)(my_flag); - } else { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - } -} - -void ThreadPool::monitor_loop() { - while (!shutdown_requested_.load()) { - size_t backlog = pending_tasks_.load(std::memory_order_relaxed); - - if (backlog > threads_.size() && threads_.size() < max_threads_) { - auto flag = std::make_unique(); - size_t id = flags_.size(); - flags_.push_back(std::move(flag)); - threads_.emplace_back([this, id]() { this->worker_loop(id); }); - LOG(INFO) << "[ThreadPool Monitor] Expanded to " << threads_.size() << " threads"; - } else if (backlog < threads_.size() / 2 && threads_.size() > min_threads_) { - flags_.back()->request_stop(); - LOG(INFO) << "[ThreadPool Monitor] Shrinking thread pool"; - } - - std::this_thread::sleep_for(std::chrono::seconds(2)); - } -} - -void ThreadPool::shutdown() { - shutdown_requested_.store(true); - for (auto& flag : flags_) { - flag->request_stop(); - } - for (auto& thread : threads_) { - if (thread.joinable()) thread.join(); - } - if (monitor_thread_.joinable()) monitor_thread_.join(); -} \ No newline at end of file diff --git a/cmvr-es/utils/base/src/timer.cpp b/cmvr-es/utils/base/src/timer.cpp deleted file mode 100644 index 99892952..00000000 --- a/cmvr-es/utils/base/src/timer.cpp +++ /dev/null @@ -1,87 +0,0 @@ -// -// Created by xtkuang on 2025/5/14. -// - -#include "utils/base/include/timer.h" - -FDTimer::FDTimer() : fd_(-1), running_(false) {} - -FDTimer::~FDTimer() { - stop(); -} - -void FDTimer::start(std::chrono::nanoseconds interval, Callback callback) { - if (running_) return; - - // 确保最小间隔 - if (interval < std::chrono::milliseconds(1)) { - interval = std::chrono::milliseconds(1); - } - - fd_ = timerfd_create(CLOCK_MONOTONIC, 0); // 阻塞模式 - if (fd_ == -1) { - throw std::system_error(errno, std::system_category(), "timerfd_create failed"); - } - - itimerspec its{}; - its.it_interval = toTimespec(interval); - its.it_value = its.it_interval; - - if (timerfd_settime(fd_, 0, &its, nullptr) == -1) { - close(fd_); - fd_ = -1; - throw std::system_error(errno, std::system_category(), "timerfd_settime failed"); - } - - running_ = true; - callback_ = std::move(callback); - - worker_thread_ = std::thread([this]() { - uint64_t expirations; - while (running_) { - ssize_t s = read(fd_, &expirations, sizeof(expirations)); - if (!running_) break; - - if (s != sizeof(expirations)) { - std::cerr << "Error reading timerfd: " << strerror(errno) << std::endl; - break; - } - - if (callback_) { - try { - callback_(); - } catch (const std::exception &e) { - std::cerr << "Callback error: " << e.what() << std::endl; - } - } - } - }); -} - - -void FDTimer::stop() { - if (!running_) return; - - running_ = false; - if (fd_ != -1) { - uint64_t dummy = 1; - write(fd_, &dummy, sizeof(dummy)); - } - if (worker_thread_.joinable()) { - worker_thread_.join(); - } - if (fd_ != -1) { - close(fd_); - fd_ = -1; - } -} - -bool FDTimer::is_running() const { - return running_; -} - -timespec FDTimer::toTimespec(std::chrono::nanoseconds ns) { - const auto sec = std::chrono::duration_cast(ns); - ns -= sec; - return timespec{sec.count(), ns.count()}; -} diff --git a/cmvr-es/utils/controller/include/cartesian_controller.h b/cmvr-es/utils/controller/include/cartesian_controller.h deleted file mode 100644 index 9a5eaf09..00000000 --- a/cmvr-es/utils/controller/include/cartesian_controller.h +++ /dev/null @@ -1,118 +0,0 @@ -// -// Created by xtkuang on 2025/7/21. -// - -#ifndef CMVR_ES_CARTESIAN_CONTROLLER_H -#define CMVR_ES_CARTESIAN_CONTROLLER_H - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "../../math/include/se3.h" -#include "../../math/include/qp_solver.h" -#include "../../dynamics/include/robot.h" - -namespace cmvr::ctrl{ - using cmvr::math::SE3; - - // ----------------------------------------------------------------------------- - // Task & collision descriptions - // ----------------------------------------------------------------------------- - struct PoseTarget { - std::string link_name; //!< link to be controlled - Eigen::Matrix4d T_target; //!< desired pose w.r.t *base_link* - double w_posrot = 0.5; // [0→pure pos, 1→pure rot] - double weight = 1.0; // task weight (0‑1 soft, 1 hard) - }; - - struct CollisionPair { std::string link_A, link_B; }; - - // ----------------------------------------------------------------------------- - // CartesianController declaration (implementation in .cpp) - // ----------------------------------------------------------------------------- - template - class CartesianController { - public: - using RobotT = cmvr::dyn::Robot; - using VecD = Eigen::Vector; - using Mat6D = Eigen::Matrix; - - enum class Mode { Position, Velocity, Torque }; - - CartesianController(std::shared_ptr robot, double dsafe = 0.05, double lambda = 1e-2); - /* ------------------------- configuration ----------------------------- */ - void setCollisionPairs(const std::vector& pairs); - - // Joint limits (size = DOF). Any value = std::nullopt → no limit. - void setPositionLimits(const std::vector>& q_min, const std::vector>& q_max); - void setVelocityLimits(const std::vector>& qd_max); - void setAccelerationLimits(const std::vector>& qdd_max); - - /* --------------------------- main API -------------------------------- */ - /** - * Compute joint command given Cartesian objectives. - * @param state current (mutable) robot state - * @param base reference frame (string) - * @param targets list of PoseTarget - * @param dt controller step [s] - * @param mode desired output type - * @param out_cmd filled with q / q̇ / τ - * @return true if IK converged (position) or soft OK (others) - */ - bool compute( - std::shared_ptr>& state, - const std::string& base_link, - const std::vector& targets, - double dt, - Mode mode, - VecD& out_cmd, - int max_iters = 60, - double tol = 1e-3, - double alpha_h = 10.0); - - private: - /* -------- internal helpers (implemented in .cpp) --------------------- */ - bool solveIK(std::shared_ptr>& state, const std::string& base, const std::vector& targets, - const VecD& q_init, VecD& q_out, int max_iters, double tol, double alpha_h) const; - - bool distanceConstraint(const CollisionPair& cp, std::shared_ptr>& state, - const std::vector>& objs, - double& h_out, Eigen::RowVectorXd& J_out) const; - - inline void clampVec(VecD& v, const std::vector>& lo, const std::vector>& hi) const; - - /* -------------------------- data ------------------------------------ */ - std::shared_ptr robot_; - std::vector pairs_; - double d_safe_ = 0.05; - double lambda_ = 1e-2; - mutable math::QPSolver solver_; - - // joint limits (optional) - std::vector> q_min_ = std::vector>(DOF); - std::vector> q_max_ = std::vector>(DOF); - std::vector> qd_max_ = std::vector>(DOF); - std::vector> qdd_max_= std::vector>(DOF); - - // FCL reusable buffers - mutable fcl::DistanceRequestd dist_req_{}; - mutable fcl::DistanceResultd dist_res_{}; - }; -} - -// ------------------------------ explicit instantiation ----------------------- -extern template class cmvr::ctrl::CartesianController<7>; -extern template class cmvr::ctrl::CartesianController<14>; -extern template class cmvr::ctrl::CartesianController<20>; - - -#endif //CMVR_ES_CARTESIAN_CONTROLLER_H diff --git a/cmvr-es/utils/controller/src/cartesian_controller.cpp b/cmvr-es/utils/controller/src/cartesian_controller.cpp deleted file mode 100644 index 34d022b5..00000000 --- a/cmvr-es/utils/controller/src/cartesian_controller.cpp +++ /dev/null @@ -1,263 +0,0 @@ -#include "../include/cartesian_controller.h" - -using namespace cmvr::ctrl; - -template -CartesianController::CartesianController(std::shared_ptr robot, double dsafe, double lambda) -: robot_(std::move(robot)), d_safe_(dsafe), lambda_(lambda), solver_() -{ - dist_req_.enable_nearest_points = true; -} - -// === configuration setters ================================================== -template -void CartesianController::setCollisionPairs(const std::vector& p) -{ - pairs_ = p; -} - -template -void CartesianController::setPositionLimits(const std::vector>& qmin, - const std::vector>& qmax) -{ - if (qmin.size()==DOF) q_min_ = qmin; - if (qmax.size()==DOF) q_max_ = qmax; -} - -template -void CartesianController::setVelocityLimits(const std::vector>& qdmax) -{ - if (qdmax.size()==DOF) qd_max_ = qdmax; -} - -template -void CartesianController::setAccelerationLimits(const std::vector>& qddmax) -{ - if (qddmax.size()==DOF) qdd_max_ = qddmax; -} - -// === helpers =============================================================== - -template -inline void CartesianController::clampVec( - VecD& v, const std::vector>& lo,const std::vector>& hi) const -{ - for (int i=0;i -bool CartesianController::compute( - std::shared_ptr>& state, - const std::string& base_link, - const std::vector& targets, - double dt, - Mode mode, - VecD& out_cmd, - int max_iters, - double tol, - double alpha_h) -{ - // 1) Inverse kinematics -> q_des - VecD q_init = state->GetQ(); - VecD q_des; - bool ik_ok = solveIK(state, base_link, targets, q_init, q_des, - max_iters, tol, alpha_h); - - if (!ik_ok && mode == Mode::Position) return false; // hard failure - - switch (mode) { - case Mode::Position: - out_cmd = q_des; - state->SetQ(q_des); - return ik_ok; - - case Mode::Velocity: { - const double Kp = 1.0; // [rad/s] per rad error - out_cmd = Kp * (q_des - q_init); - std::cout << "=================== q_des = " << q_des - << "q_init = " << q_init << std::endl; - const auto qd_lim = robot_->GetLimitQdotUpper(state); - for (int i = 0; i < DOF; ++i) { - // 打印原始的速度命令和限制值 - std::cout << "Before clamping - out_cmd[" << i << "] = " << out_cmd[i] - << ", qd_lim(" << i << ") = " << qd_lim(i) - << ", -qd_lim(" << i << ") = " << -qd_lim(i) << std::endl; - - // 对 out_cmd 进行限速 - out_cmd[i] = std::clamp(out_cmd[i], -qd_lim(i), qd_lim(i)); - - // 打印被clamp之后的值 - std::cout << "After clamping - out_cmd[" << i << "] = " << out_cmd[i] - << ", qd_lim(" << i << ") = " << qd_lim(i) - << ", -qd_lim(" << i << ") = " << -qd_lim(i) << std::endl; - } - return ik_ok; - } - case Mode::Torque: { - const double Kp = 100.0; // Nm/rad - const double Kd = 10.0; // Nm·s/rad - VecD qdot = state->GetQdot(); - VecD q_err = q_des - q_init; - VecD g = robot_->ComputeGravityTerm(state); - out_cmd = Kp * q_err - Kd * qdot + g; - return ik_ok; - } - } - return false; // unreachable -} - -// === solveIK ============================================================== - -template -bool CartesianController::solveIK( - std::shared_ptr>& state, - const std::string& base_link, - const std::vector& targets, - const VecD& q_init, - VecD& q_out, - int max_iters, - double tol, - double alpha_h) const -{ - VecD q = q_init; - const int dof = robot_->GetDOF(); - - // Pre‑allocated containers - Eigen::MatrixXd A_cost; - Eigen::VectorXd b_cost; - std::vector J_ineq; // for future CBF use - std::vector b_ineq; - std::vector> objs; - - // Joint hard limits (position / velocity / accel) - const VecD q_min_global = robot_->GetLimitQLower(state); - const VecD q_max_global = robot_->GetLimitQUpper(state); - const VecD qd_max_global = robot_->GetLimitQdotUpper(state); - const VecD qdd_max_global = robot_->GetLimitQddotUpper(state); - - for (int iter = 0; iter < max_iters; ++iter) { - // 1) Build stacked task Jacobian & error -------------------- - size_t n_eq = 6 * targets.size(); - A_cost.resize(n_eq + dof, dof); // +damping rows later - b_cost.resize(n_eq + dof); - - size_t r = 0; - for (const auto& tgt : targets) { - auto from_idx = robot_->GetLinkIdx(base_link); - auto to_idx = robot_->GetLinkIdx(tgt.link_name); - - Eigen::Matrix4d T_curr = robot_->GetTransformation(state, from_idx, to_idx); - Mat6D J_body = robot_->GetBodyJacobian(state, from_idx, to_idx); - - Eigen::Matrix dx = SE3::Log(T_curr.inverse()*tgt.T_target); - dx.head<3>() *= (1.0 - tgt.w_posrot); - dx.tail<3>() *= tgt.w_posrot; - - double sw = std::sqrt(std::clamp(tgt.weight, 0.0, 1.0)); - A_cost.block(r, 0, 6, dof) = sw * J_body; - b_cost.segment(r, 6) = sw * dx; - r += 6; - } - - // 1.b) damping rows ----------------------------------------- - A_cost.bottomRows(dof) = std::sqrt(lambda_) * Eigen::MatrixXd::Identity(dof, dof); - b_cost.tail(dof).setZero(); - - // 2) Setup QP cost ||A dq - b||^2 ------------------------- - solver_.Setup(dof, dof); // ★fix 现在 solver_ 可写 - solver_.SetCostFunction(A_cost, b_cost); // ★fix - - // 3) Joint delta bounds (pos / vel / accel) ----------------- - VecD lb, ub; - VecD qdot = state->GetQdot(); - VecD qdd_lim = qdd_max_global; - for (int i = 0; i < dof; ++i) { - double pos_lb = q_min_global(i) - q(i); - double pos_ub = q_max_global(i) - q(i); - - double vel_lb = -qd_max_global(i); - double vel_ub = qd_max_global(i); - - // Convert accel limit to delta‑q per iter (assume dt=1 for IK) - double acc_lb = -qdd_lim(i); - double acc_ub = qdd_lim(i); - - lb(i) = std::max({pos_lb, vel_lb, acc_lb}); - ub(i) = std::min({pos_ub, vel_ub, acc_ub}); - if (lb(i) > ub(i)) lb(i) = ub(i) = 0.0; - } - solver_.SetConstraintsFunction(Eigen::MatrixXd::Identity(dof, dof), lb, ub); - - // 4) Solve --------------------------------------------------- - VecD dq; - try { - dq = solver_.Solve(); - } catch (const std::exception& e) { - std::cerr << "[CartesianController] QP failed: " << e.what() << std::endl; - return false; - } - - // 5) Step & forward kinematics ------------------------------ - q += dq; - state->SetQ(q); - robot_->ComputeForwardKinematics(state); - - // 6) Convergence check on primary tasks --------------------- - double err_norm = b_cost.head(n_eq).norm(); - if (err_norm < tol) { q_out = q; return true; } - if (dq.norm() < 1e-6) { q_out = q; return false; } - } - q_out = q; // best effort - return false; -} - -// === distanceConstraint ===================================================== -template -bool CartesianController::distanceConstraint( - const CollisionPair& cp, - std::shared_ptr>& state, - const std::vector>& objs, - double& h_out, - Eigen::RowVectorXd& J_out) const -{ - /* 1. 取内部索引 --------------------------------------------------------- */ - const auto idxA = robot_->GetLinkIdx(cp.link_A); - const auto idxB = robot_->GetLinkIdx(cp.link_B); - if (idxA.link_idx < 0 || idxB.link_idx < 0) return false; - - /* 2. 距离查询 ----------------------------------------------------------- */ - dist_res_.clear(); - fcl::distance(objs[idxA.link_idx].get(), objs[idxB.link_idx].get(), - dist_req_, dist_res_); - - const double d = dist_res_.min_distance; - h_out = d - d_safe_; // CBF 函数 h(q) - - if (d > 2.0 * d_safe_) return false; // 太远：忽略该对 - - /* 3. 最近点法向量 ------------------------------------------------------- */ - Eigen::Vector3d pA(dist_res_.nearest_points[0].data()); - Eigen::Vector3d pB(dist_res_.nearest_points[1].data()); - Eigen::Vector3d n = pB - pA; - if (n.norm() < 1e-8) n.setZero(); else n.normalize(); - - /* 4. 关节梯度 ----------------------------------------------------------- */ - // Body Jacobian (6×DOF) - Eigen::Matrix JA = robot_->GetBodyJacobian(state, idxA, idxA); - Eigen::Matrix JB = robot_->GetBodyJacobian(state, idxB, idxB); - - // 只保留线速度行 (前三行) - J_out.resize(DOF); - J_out = n.transpose() * (JB.template topRows<3>() - JA.template topRows<3>()); - - return true; // 需要加入不等式 -} - -template class cmvr::ctrl::CartesianController<7>; -template class cmvr::ctrl::CartesianController<14>; -template class cmvr::ctrl::CartesianController<20>; \ No newline at end of file diff --git a/cmvr-es/utils/dynamics/include/inertial.h b/cmvr-es/utils/dynamics/include/inertial.h deleted file mode 100644 index c9a7e4c6..00000000 --- a/cmvr-es/utils/dynamics/include/inertial.h +++ /dev/null @@ -1,54 +0,0 @@ -// -// Created by xtkuang on 2025/7/8. -// - -#ifndef INERTIAL_H -#define INERTIAL_H - - -#pragma once - -#include - -#include "../../math/include/se3.h" - -namespace cmvr::dyn { - -class Inertial; - -class Inertial { -public: - using MatrixType = Eigen::Matrix; - - static MatrixType I(double mass); - - static MatrixType I(double mass, double ixx, double iyy, double izz, - const math::SE3::MatrixType& T = math::SE3::Identity()); - - static MatrixType I(double mass, double ixx, double iyy, double izz, - const Eigen::Vector3d& com); - - static MatrixType I(double mass, double ixx, double iyy, double izz, double ixy, double ixz, double iyz, - const math::SE3::MatrixType& T = math::SE3::Identity()); - - static MatrixType I(double mass, double ixx, double iyy, double izz, double ixy, double ixz, double iyz, - const Eigen::Vector3d& com); - - static MatrixType I(double mass, const Eigen::Vector3d& inertia, - const math::SE3::MatrixType& T = math::SE3::Identity()); - - static MatrixType I(double mass, const Eigen::Matrix& inertia, - const math::SE3::MatrixType& T = math::SE3::Identity()); - - static MatrixType Transform(const math::SE3::MatrixType& T, const MatrixType& I); - - static Eigen::Vector3d GetCOM(const MatrixType& I); - - static double GetMass(const MatrixType& I); - - // [I_{xx}, I_{yy}, I_{zz}, I_{xy}, I_{xz}, I_{yz}]^T - static Eigen::Vector GetInertia(const MatrixType& I); -}; - -} -#endif //INERTIAL_H \ No newline at end of file diff --git a/cmvr-es/utils/dynamics/include/joint.h b/cmvr-es/utils/dynamics/include/joint.h deleted file mode 100644 index 04521b85..00000000 --- a/cmvr-es/utils/dynamics/include/joint.h +++ /dev/null @@ -1,108 +0,0 @@ -// -// Created by xtkuang on 2025/7/7. -// - -#ifndef JOINT_H -#define JOINT_H - -#include -#include -#include "../../math/include/constants.h" -#include "../../math/include/liegroup.h" -#include "link.h" - -namespace cmvr::dyn { -class Joint : public std::enable_shared_from_this { -public: - template - friend class Robot; - - static std::shared_ptr Make(std::string name, math::se3v::MatrixType S); - - static std::shared_ptr MakeRevoluteJoint(std::string name, - const math::SE3::MatrixType &T = math::SE3::Identity(), - const Eigen::Vector3d &axis = {0, 0, 1}); - - static std::shared_ptr MakePrismaticJoint(std::string name, - const math::SE3::MatrixType &T = math::SE3::Identity(), - const Eigen::Vector3d &axis = {0, 0, 1}); - - static std::shared_ptr MakeFixedJoint(std::string name); - - std::string GetName() const; - - void ConnectLinks(const std::shared_ptr &parent_link, const std::shared_ptr &child_link, - const math::SE3::MatrixType &T_pj = math::SE3::Identity(), - const math::SE3::MatrixType &T_jc = math::SE3::Identity()); - - void Disconnect(); - - void SetLimitQ(double lower, double upper); - - void SetLimitQdot(double lower, double upper); - - void SetLimitQddot(double lower, double upper); - - void SetLimitTorque(double value); - - double GetLimitQLower() const; - - double GetLimitQUpper() const; - - double GetLimitQdotLower() const; - - double GetLimitQdotUpper() const; - - double GetLimitQddotLower() const; - - double GetLimitQddotUpper() const; - - double GetLimitTorque() const; - - void SetLimitQLower(double val); - - void SetLimitQUpper(double val); - - void SetLimitQdotLower(double val); - - void SetLimitQdotUpper(double val); - - void SetLimitQddotLower(double val); - - void SetLimitQddotUpper(double val); - - std::weak_ptr GetParentLink(); - - std::shared_ptr GetChildLink(); - - std::shared_ptr GetChildLink() const; - - bool IsFixed() const; - -private: - explicit Joint(std::string name); - - Joint(std::string name, math::se3v::MatrixType S); - -private: - std::string name_{}; - bool fixed_; - math::se3v::MatrixType S_; - double limit_torque_{(std::numeric_limits::max)()}; - double limit_q_lower_{-(std::numeric_limits::max)()}; - double limit_q_upper_{(std::numeric_limits::max)()}; - double limit_qdot_lower_{-(std::numeric_limits::max)()}; - double limit_qdot_upper_{(std::numeric_limits::max)()}; - // double limit_qddot_lower_{-(std::numeric_limits::max)()}; - // double limit_qddot_upper_{(std::numeric_limits::max)()}; - double limit_qddot_lower_{-10.}; - double limit_qddot_upper_{10.}; // (rad/s^2) - - std::weak_ptr parent_link_; - std::shared_ptr child_link_{nullptr}; - math::SE3::MatrixType T_pj_, T_jc_; -}; -} - - -#endif //JOINT_H diff --git a/cmvr-es/utils/dynamics/include/link.h b/cmvr-es/utils/dynamics/include/link.h deleted file mode 100644 index 32c9791b..00000000 --- a/cmvr-es/utils/dynamics/include/link.h +++ /dev/null @@ -1,143 +0,0 @@ -// -// Created by xtkuang on 2025/7/8. -// - -#ifndef LINK_H -#define LINK_H - -#pragma once - -#include -#include - -#include "inertial.h" -#include "../../math/include/liegroup.h" - -namespace cmvr::dyn { -template -class Robot; -class Link; -class Joint; -class Collision; -struct CollisionResult; -class Geom; -class GeomCapsule; - -enum class GeomType { kCapsule = 0 }; - -class Link : public std::enable_shared_from_this { -public: - template - friend class Robot; - - friend class Joint; - - static std::shared_ptr Make(std::string name, Inertial::MatrixType I = Inertial::I(1.)); - - std::string GetName() const; - - std::weak_ptr GetParentJoint(); - - std::vector > GetChildJointList(); - - const std::vector > &GetChildJointList() const; - - void AddCollision(const std::shared_ptr &collision); - - std::vector > GetCollisions(); - - const std::vector > &GetCollisions() const; - -private: - Link(std::string name, Inertial::MatrixType I); - -private: - std::string name_{}; - Inertial::MatrixType I_{}; - - std::weak_ptr parent_joint_; - std::vector > child_joints_; - - std::vector > collisions_; -}; - -class Collision : public std::enable_shared_from_this { -public: - explicit Collision(std::string name); - - void SetOrigin(const math::SE3::MatrixType &T); - - math::SE3::MatrixType GetOrigin() const; - - void AddGeom(const std::shared_ptr &geom); - - std::vector > GetGeoms(); - - const std::vector > &GetGeoms() const; - -private: - std::string name_; - math::SE3::MatrixType T_{math::SE3::Identity()}; - std::vector > geoms_; -}; - -class Geom : public std::enable_shared_from_this { -public: - Geom(unsigned int coltype = 0, unsigned int colaffinity = 0) : coltype_(coltype), colaffinity_(colaffinity) { - } - - virtual ~Geom() = default; - - virtual GeomType GetType() const = 0; - - unsigned int GetColtype() const { return coltype_; } - - unsigned int GetColaffinity() const { return colaffinity_; } - - virtual std::optional ComputeMinimumDistance(const math::SE3::MatrixType &T, - const Geom &other_geom, - const math::SE3::MatrixType &other_T) const = 0; - - bool Filter(const Geom &other_geom) const { - return (coltype_ & other_geom.colaffinity_) || (other_geom.coltype_ & colaffinity_); - } - -protected: - unsigned int coltype_; - unsigned int colaffinity_; -}; - -class GeomCapsule : public Geom { -public: - GeomCapsule(double length, double radius, unsigned int coltype = 0, unsigned int colaffinity = 0); - - GeomCapsule(Eigen::Vector3d sp, Eigen::Vector3d ep, double radius, unsigned int coltype = 0, - unsigned int colaffinity = 0); - - GeomType GetType() const override; - - std::optional ComputeMinimumDistance(const math::SE3::MatrixType &T, const Geom &other_geom, - const math::SE3::MatrixType &other_T) const override; - - Eigen::Vector3d GetStartPoint() const; - - Eigen::Vector3d GetEndPoint() const; - - double GetRadius() const; - -private: - Eigen::Vector3d sp_; - Eigen::Vector3d ep_; - double radius_; -}; - -struct CollisionResult { - std::string link1; - std::string link2; - Eigen::Vector3d position1; - Eigen::Vector3d position2; - double distance; -}; - -} -#endif //LINK_H diff --git a/cmvr-es/utils/dynamics/include/robot.h b/cmvr-es/utils/dynamics/include/robot.h deleted file mode 100644 index f99aa087..00000000 --- a/cmvr-es/utils/dynamics/include/robot.h +++ /dev/null @@ -1,288 +0,0 @@ -// -// Created by xtkuang on 2025/7/8. -// - -#ifndef ROBOT_H -#define ROBOT_H - - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "inertial.h" -#include "joint.h" -#include "link.h" -#include "state.h" -#include "../../math/include/liegroup.h" -#include "../../math/include/qp_solver.h" - -namespace cmvr::dyn { -/********************************************************** - * FORWARD DECLARATION - **********************************************************/ - -// -template -class Robot; -class Link; -class Joint; - -/********************************************************** - * MOBILE BASE - **********************************************************/ - -enum class MobileBaseType { - None, // - Differential, // - Mecanum // -}; - -struct MobileBase { - MobileBaseType type{MobileBaseType::None}; - math::SE3::MatrixType T; // front = x-axis - std::vector joints; - std::vector params; -}; - -struct MobileBaseDifferential : public MobileBase { - unsigned int right_wheel_idx{}; - unsigned int left_wheel_idx{}; - double wheel_base{0.}; - double wheel_radius{0.}; -}; - -struct MobileBaseMecanum : public MobileBase { - unsigned int fr_wheel_idx{}; - unsigned int fl_wheel_idx{}; - unsigned int rr_wheel_idx{}; - unsigned int rl_wheel_idx{}; - double L_x{0.}; - double L_y{0.}; - double wheel_radius{0.}; -}; - -/********************************************************** - * ROBOT - **********************************************************/ - -struct RobotConfiguration { - std::string name; - std::shared_ptr base_link; - std::shared_ptr mobile_base; -}; - -template -class Robot { -public: - template - using ContainerType = typename std::conditional_t<(N > 0), std::array, std::vector >; - - struct Link_ { - struct SubLink_ { - std::shared_ptr link; - Inertial::MatrixType J_wrt_p; - math::SE3::MatrixType M_wrt_p; - math::SE3::MatrixType M_wrt_base; - }; - - std::vector links; - Inertial::MatrixType J{Inertial::MatrixType::Zero()}; // Link_ inertial wrt base link - math::SE3::MatrixType M{math::SE3::Identity()}; // Base link to Link_ frame - Inertial::MatrixType I{Inertial::MatrixType::Zero()}; // Link_ inertial wrt Link_ frame - int depth{0}; - int parent_joint_idx{-1}; - std::vector child_joint_idx{}; - - void SetBaseLink(const std::shared_ptr &link, const math::SE3::MatrixType &T) { - // Rest link information - links.clear(); - J = Inertial::MatrixType::Zero(); - M = T; - AddLink(link, math::SE3::Identity()); - } - - int AddLink(const std::shared_ptr &link, const math::SE3::MatrixType &M_wrt_p) { - Inertial::MatrixType J_wrt_p = Inertial::Transform(M_wrt_p, link->I_); - int idx = links.size(); - SubLink_ l; - l.link = link; - l.J_wrt_p = J_wrt_p; - l.M_wrt_p = M_wrt_p; - l.M_wrt_base = M * M_wrt_p; - links.push_back(l); - I += J_wrt_p; - J += Inertial::Transform(M, J_wrt_p); - return idx; - } - }; - - struct Joint_ { - std::shared_ptr joint{nullptr}; - math::se3v::MatrixType S{math::se3v::MatrixType::Zero()}; // zero pose - - int parent_link_idx{}; - int child_link_idx{}; - }; - - struct LinkIdx_ { - int link_idx; - int sub_link_idx; - }; - - explicit Robot(const RobotConfiguration &robot_configuration); - - std::shared_ptr GetBase(); - - std::vector GetLinkNames() const; - - std::vector GetJointNames() const; - - std::shared_ptr GetLink(const std::string &name) const; - - std::shared_ptr GetLink(std::shared_ptr> state, int index) const; - - LinkIdx_ GetLinkIdx(const std::string& name) const; - - template, typename JointContainer = std::vector > - std::shared_ptr> MakeState(const LinkContainer &link_names, const JointContainer &joint_names); - - int GetDOF() const; - - int GetNumberOfJoints() const; - - void ComputeForwardKinematics(std::shared_ptr> state); - - void ComputeDiffForwardKinematics(std::shared_ptr> state); - - void Compute2ndDiffForwardKinematics(std::shared_ptr> state); - - void ComputeInverseDynamics(std::shared_ptr> state); - - Eigen::Vector ComputeGravityTerm(std::shared_ptr> state); - - Eigen::Matrix ComputeMassMatrix(std::shared_ptr> state); - - Eigen::Matrix ComputeReflectiveInertia(std::shared_ptr> state, unsigned int from, unsigned int to); - - math::SE3::MatrixType ComputeTransformation(std::shared_ptr> state, unsigned int from, unsigned int to); - - math::se3v::MatrixType ComputeBodyVelocity(std::shared_ptr> state, unsigned int from, unsigned int to); - - Eigen::Matrix ComputeSpaceJacobian(std::shared_ptr> state, unsigned int from, unsigned int to); - - Eigen::Matrix ComputeBodyJacobian(std::shared_ptr> state, unsigned int from, unsigned int to); - - double ComputeMass(std::shared_ptr> state, unsigned int target_link); - - Eigen::Vector3d ComputeCenterOfMass(std::shared_ptr> state, unsigned int ref_link, unsigned int target_link); - - Eigen::Vector3d ComputeCenterOfMass(std::shared_ptr> state, unsigned int ref_link, const std::vector &target_links); - - Eigen::Matrix ComputeCenterOfMassJacobian(std::shared_ptr> state, unsigned int ref_link, unsigned int target_link); - - Inertial::MatrixType ComputeTotalInertial(std::shared_ptr> state, unsigned int ref_link); - - Eigen::Vector3d ComputeCenterOfMass(std::shared_ptr> state, unsigned int ref_link); - - Eigen::Matrix ComputeCenterOfMassJacobian(std::shared_ptr> state, unsigned int ref_link); - - std::vector DetectCollisionsOrNearestLinks(std::shared_ptr> state, int collision_threshold = 0); - - // TODO: ComputeBodyJacobianDot - // TODO: ComputeBodyAcceleration - // TODO: InverseDiffDynamics - - Eigen::Vector GetLimitQLower(const std::shared_ptr> &state); - - Eigen::Vector GetLimitQUpper(const std::shared_ptr> &state); - - Eigen::Vector GetLimitQdotLower(const std::shared_ptr> &state); - - Eigen::Vector GetLimitQdotUpper(const std::shared_ptr> &state); - - Eigen::Vector GetLimitQddotLower(const std::shared_ptr> &state); - - Eigen::Vector GetLimitQddotUpper(const std::shared_ptr> &state); - - Eigen::Vector GetLimitTorque(const std::shared_ptr> &state); - - Eigen::Vector GetJointProperty(const std::shared_ptr> &state, const std::function)> &getter); - - void ComputeMobilityInverseDiffKinematics(std::shared_ptr> state, // - const Eigen::Vector2d &linear_velocity, // (m/s) - double angular_velocity // (rad/s) - ); - - void ComputeMobilityInverseDiffKinematics(std::shared_ptr> state, // - const math::se2v::MatrixType &body_velocity // w, x, y - ); - - math::se2v::MatrixType ComputeMobilityDiffKinematics( // - std::shared_ptr> state // - ); - - static int CountJoints(const std::shared_ptr &base_link, bool include_fixed=false); - - math::SE3::MatrixType GetLinkT(std::shared_ptr> state, const LinkIdx_ &idx); - - math::se3v::MatrixType GetLinkV(std::shared_ptr> state, const LinkIdx_ &idx); - - math::SE3::MatrixType GetJointT(std::shared_ptr> state, int joint_idx); - - math::se3v::MatrixType GetJointV(std::shared_ptr> state, int joint_idx); - - math::se3v::MatrixType GetJointVdot(std::shared_ptr> state, int joint_idx); - - math::SE3::MatrixType GetTransformation(std::shared_ptr> state, const LinkIdx_ &from, const LinkIdx_ &to); - - Eigen::Matrix GetSpaceJacobian(std::shared_ptr> state, const LinkIdx_ &from, const LinkIdx_ &to); - - Eigen::Matrix GetBodyJacobian(std::shared_ptr> state, const LinkIdx_ &from, const LinkIdx_ &to); - -protected: - Robot() = default; - - void Build(const RobotConfiguration &rc); - -private: - ContainerType links_; - ContainerType joints_; - - std::unordered_map link_idx_; // link name to (parent idx, sub link idx) - std::unordered_map joint_idx_; // joint name to joint idx - - size_t n_links_{}; - size_t n_joints_{}; - - /** - * MOBILE BASE - */ - std::shared_ptr mobile_base_; -}; - -RobotConfiguration LoadRobotFromURDFData(const std::string &model, const std::string &base_link_name); - -RobotConfiguration LoadRobotFromURDF(const std::string &path, const std::string &base_link_name); - -inline std::string to_string(GeomType type) { - switch (type) { - case GeomType::kCapsule: - return "capsule"; - } - return "unknown"; -} -} - -#include "../src/robot.tpp" - -#endif //ROBOT_H diff --git a/cmvr-es/utils/dynamics/include/state.h b/cmvr-es/utils/dynamics/include/state.h deleted file mode 100644 index 65e2db3b..00000000 --- a/cmvr-es/utils/dynamics/include/state.h +++ /dev/null @@ -1,141 +0,0 @@ -// -// Created by xtkuang on 2025/7/8. -// - -#ifndef STATE_H -#define STATE_H - -#pragma once - -#include -#include "../../math/include/se3.h" - -namespace cmvr::dyn { -template -class Robot; - -template -class State { -public: - template - friend class Robot; - - template - using ContainerType = typename std::conditional_t<(N > 0), std::array, std::vector >; - - unsigned int GetBaseLinkIdx() const { // NOLINT - return base_link_user_idx; - } - - template - void SetQ(const Eigen::MatrixBase &new_q) { - Set(q, new_q.eval()); - } - - Eigen::Vector GetQ() { return q(utr_joint_map); } - - template - void SetQdot(const Eigen::MatrixBase &new_qdot) { - Set(qdot, new_qdot.eval()); - } - - Eigen::Vector GetQdot() { return qdot(utr_joint_map); } - - template - void SetQddot(const Eigen::MatrixBase &new_qddot) { - Set(qddot, new_qddot.eval()); - } - - Eigen::Vector GetQddot() { return qddot(utr_joint_map); } - - template - void SetTau(const Eigen::MatrixBase &new_tau) { - Set(tau, new_tau.eval()); - } - - Eigen::Vector GetTau() { return tau(utr_joint_map); } - - void SetV0(const math::se3v::MatrixType &new_V0) { V0 = new_V0; } - - // Vdot of root link in root frame - void SetVdot0(const math::se3v::MatrixType &new_Vdot0) { Vdot0 = new_Vdot0; } - - void SetGravity(const math::se3v::MatrixType &gravity) { Vdot0 = -gravity; } - - ContainerType GetJointNames() const { return joint_names; } - - [[nodiscard]] std::vector GetLinkNames() const { return link_names; } - -private: - explicit State(int dof) { - if (!(DOF < 0 || dof == DOF)) { - throw std::runtime_error("State initialization failed"); - } - - if constexpr (DOF < 0) { - q.resize(dof); - qdot.resize(dof); - qddot.resize(dof); - tau.resize(dof); - E.resize(dof + 1, math::SE3::Identity()); - S.resize(6, dof); - T.resize(dof + 1, math::SE3::Identity()); - V.resize(6, dof); - Vdot.resize(6, dof); - F.resize(6, dof); - joint_names.resize(dof); - utr_joint_map.resize(dof); - rtu_joint_map.resize(dof); - } else { - E.fill(math::SE3::Identity()); - T.fill(math::SE3::Identity()); - } - - q.setZero(); - qdot.setZero(); - qddot.setZero(); - S.setZero(); - V.setZero(); - Vdot0.setZero(); - F.setZero(); - } - - template - void Set(Eigen::Vector &s, const Eigen::Vector &i) { - if (s.size() == i.size()) { - s = i(rtu_joint_map); - } else if (s.size() > i.size()) { - s.template head<>(i.size()) = i(rtu_joint_map.template head<>(i.size())); - } else { - throw std::runtime_error("i.size cannot be greater than s.size"); - } - } - - ContainerType joint_names; - Eigen::Vector utr_joint_map; // user joint idx -> robot joint idx - Eigen::Vector rtu_joint_map; // robot joint idx -> user joint idx - - unsigned int base_link_user_idx; - std::vector link_names; - std::vector::LinkIdx_> utr_link_map; // user link idx -> robot dummy/sub link idx - -public: - Eigen::Vector q; - Eigen::Vector qdot; - Eigen::Vector qddot; - Eigen::Vector tau; - math::se3v::MatrixType V0{math::se3v::MatrixType::Zero()}; - math::se3v::MatrixType Vdot0{math::se3v::MatrixType::Zero()}; - - ContainerType E{}; // exponential mapping - Eigen::Matrix S; // S(q_0,...q_i) in root frame - - ContainerType T{}; // product of exponential - Eigen::Matrix V; - Eigen::Matrix Vdot; - Eigen::Matrix F; -}; - -} - -#endif //STATE_H diff --git a/cmvr-es/utils/dynamics/src/inertial.cpp b/cmvr-es/utils/dynamics/src/inertial.cpp deleted file mode 100644 index 3c3bf067..00000000 --- a/cmvr-es/utils/dynamics/src/inertial.cpp +++ /dev/null @@ -1,69 +0,0 @@ -// -// Created by xtkuang on 2025/7/8. -// - -#include "../include/inertial.h" - -using namespace cmvr::dyn; - -Inertial::MatrixType Inertial::I(double mass) { - return mass * MatrixType::Identity(); -} - -Inertial::MatrixType Inertial::I(double mass, double Ixx, double Iyy, double Izz, const math::SE3::MatrixType &T) { - return I(mass, Ixx, Iyy, Izz, 0, 0, 0, T); -} - -Inertial::MatrixType Inertial::I(double mass, double Ixx, double Iyy, double Izz, const Eigen::Vector3d &com) { - return I(mass, Ixx, Iyy, Izz, 0, 0, 0, math::SE3::T(math::SO3::Identity(), com)); -} - -Inertial::MatrixType Inertial::I(double mass, double ixx, double iyy, double izz, double ixy, double ixz, double iyz, - const math::SE3::MatrixType &T) { - MatrixType I; - I(0, 0) = ixx; - I(0, 1) = ixy; - I(0, 2) = ixz; - I(1, 0) = ixy; - I(1, 1) = iyy; - I(1, 2) = iyz; - I(2, 0) = ixz; - I(2, 1) = iyz; - I(2, 2) = izz; - I.block<3, 3>(0, 3).setZero(); - I.block<3, 3>(3, 0).setZero(); - I.block<3, 3>(3, 3) = mass * Eigen::Matrix3d::Identity(); - return Transform(T, I); -} - -Inertial::MatrixType Inertial::I(double mass, double ixx, double iyy, double izz, double ixy, double ixz, double iyz, - const Eigen::Vector3d &com) { - return I(mass, ixx, iyy, izz, ixy, ixz, iyz, math::SE3::T(math::SO3::Identity(), com)); -} - -Inertial::MatrixType Inertial::I(double mass, const Eigen::Vector3d &inertia, const math::SE3::MatrixType &T) { - return I(mass, inertia(0), inertia(1), inertia(2), T); -} - -Inertial::MatrixType Inertial::I(double mass, const Eigen::Matrix &inertia, - const math::SE3::MatrixType &T) { - return I(mass, inertia(0), inertia(1), inertia(2), inertia(3), inertia(4), inertia(5), T); -} - -Inertial::MatrixType Inertial::Transform(const math::SE3::MatrixType &T, const Inertial::MatrixType &I) { - math::SE3::MatrixType invT = math::SE3::Inverse(T); - return math::SE3::Ad(invT).transpose() * I * math::SE3::Ad(invT); -} - -Eigen::Vector3d Inertial::GetCOM(const Inertial::MatrixType &I) { - double mass = GetMass(I); - return Eigen::Vector3d{-I(1, 5), I(0, 5), -I(0, 4)} / mass; -} - -double Inertial::GetMass(const Inertial::MatrixType &I) { - return I(3, 3); -} - -Eigen::Vector Inertial::GetInertia(const Inertial::MatrixType &I) { - return {I(0, 0), I(1, 1), I(2, 2), I(0, 1), I(0, 2), I(1, 2)}; -} diff --git a/cmvr-es/utils/dynamics/src/joint.cpp b/cmvr-es/utils/dynamics/src/joint.cpp deleted file mode 100644 index 5332bc74..00000000 --- a/cmvr-es/utils/dynamics/src/joint.cpp +++ /dev/null @@ -1,155 +0,0 @@ -// -// Created by xtkuang on 2025/7/7. -// -#include -#include "../include/joint.h" -#include "../include/robot.h" - -using namespace std; -using namespace cmvr::math; -using namespace cmvr::dyn; - -shared_ptr Joint::Make(string name, se3v::MatrixType S) { - return shared_ptr(new Joint(std::move(name), std::move(S))); -} - -shared_ptr Joint::MakeRevoluteJoint(string name, const SE3::MatrixType &T, const Eigen::Vector3d &axis) { - return Make(std::move(name), SE3::Ad(T, {axis(0), axis(1), axis(2), 0, 0, 0})); -} - -shared_ptr Joint::MakePrismaticJoint(std::string name, const math::SE3::MatrixType &T, - const Eigen::Vector3d &axis) { - return Make(std::move(name), SE3::Ad(T, {0, 0, 0, axis(0), axis(1), axis(2)})); -} - -shared_ptr Joint::MakeFixedJoint(string name) { - return shared_ptr(new Joint(std::move(name))); -} - -std::string Joint::GetName() const { - return name_; -} - -void Joint::ConnectLinks(const shared_ptr &parent_link, const shared_ptr &child_link, - const SE3::MatrixType &T_pj, const SE3::MatrixType &T_jc) { - Disconnect(); - - if (!child_link->parent_joint_.expired()) { - auto j = child_link->parent_joint_.lock(); - j->Disconnect(); - } - - parent_link_ = parent_link; - child_link_ = child_link; - - T_pj_ = T_pj; - T_jc_ = T_jc; - - parent_link->child_joints_.push_back(this->shared_from_this()); - child_link->parent_joint_ = this->weak_from_this(); -} - -void Joint::Disconnect() { - if (!parent_link_.expired()) { - auto parent_lock = parent_link_.lock(); - auto &joints = parent_lock->child_joints_; - joints.erase(remove_if(joints.begin(), joints.end(), [n = name_](auto &j) { return j->name_ == n; }), - joints.end()); - } - if (child_link_) { - child_link_->parent_joint_.reset(); - } -} - -void Joint::SetLimitQ(double lower, double upper) { - limit_q_lower_ = lower; - limit_q_upper_ = upper; -} - -void Joint::SetLimitQdot(double lower, double upper) { - limit_qdot_lower_ = lower; - limit_qdot_upper_ = upper; -} - -void Joint::SetLimitQddot(double lower, double upper) { - limit_qddot_lower_ = lower; - limit_qddot_upper_ = upper; -} - -void Joint::SetLimitTorque(double value) { - limit_torque_ = value; -} - -double Joint::GetLimitQLower() const { - return limit_q_lower_; -} - -double Joint::GetLimitQUpper() const { - return limit_q_upper_; -} - -double Joint::GetLimitQdotLower() const { - return limit_qdot_lower_; -} - -double Joint::GetLimitQdotUpper() const { - return limit_qdot_upper_; -} - -double Joint::GetLimitQddotLower() const { - return limit_qddot_lower_; -} - -double Joint::GetLimitQddotUpper() const { - return limit_qddot_upper_; -} - -double Joint::GetLimitTorque() const { - return limit_torque_; -} - -void Joint::SetLimitQLower(double val) { - limit_q_lower_ = val; -} - -void Joint::SetLimitQUpper(double val) { - limit_q_upper_ = val; -} - -void Joint::SetLimitQdotLower(double val) { - limit_qdot_lower_ = val; -} - -void Joint::SetLimitQdotUpper(double val) { - limit_qdot_upper_ = val; -} - -void Joint::SetLimitQddotLower(double val) { - limit_qddot_lower_ = val; -} - -void Joint::SetLimitQddotUpper(double val) { - limit_qddot_upper_ = val; -} - -weak_ptr Joint::GetParentLink() { - return parent_link_; -} - -shared_ptr Joint::GetChildLink() { - return child_link_; -} - -shared_ptr Joint::GetChildLink() const { - return child_link_; -} - -bool Joint::IsFixed() const { - return fixed_; -} - -Joint::Joint(string name) : name_(std::move(name)), fixed_(true) { -} - -Joint::Joint(string name, se3v::MatrixType S) : name_(std::move(name)), S_(std::move(S)), fixed_(false) { -} diff --git a/cmvr-es/utils/dynamics/src/link.cpp b/cmvr-es/utils/dynamics/src/link.cpp deleted file mode 100644 index 8e40667a..00000000 --- a/cmvr-es/utils/dynamics/src/link.cpp +++ /dev/null @@ -1,194 +0,0 @@ -// -// Created by xtkuang on 2025/7/8. -// - -#include "../include/link.h" -#include - -using namespace std; -using namespace cmvr::math; - -namespace { - std::tuple compute_min_distance_two_segments( - const std::pair &s1, // first capsule start point, end point - const std::pair &s2 // second capsule start point, end point - ) { - Eigen::Vector3d u = s1.second - s1.first; - Eigen::Vector3d v = s2.second - s2.first; - Eigen::Vector3d w = s1.first - s2.first; - - double a = u.dot(u); - double b = u.dot(v); - double c = v.dot(v); - double d = u.dot(w); - double e = v.dot(w); - - double D = a * c - b * b; - - double sc, sN, sD = D; - double tc, tN, tD = D; - - if (D < 0.000001) { - // Segments are (nearly) parallel - sN = 0.0; - sD = 1.0; - tN = e; - tD = c; - } else { - // Compute the closest points on the infinite lines - sN = (b * e - c * d); - tN = (a * e - b * d); - - if (sN < 0.0) { - sN = 0.0; - tN = e; - tD = c; - } else if (sN > sD) { - sN = sD; - tN = e + b; - tD = c; - } - } - - if (tN < 0.0) { - tN = 0.0; - if (-d < 0.0) { - sN = 0.0; - } else if (-d > a) { - sN = sD; - } else { - sN = -d; - sD = a; - } - } else if (tN > tD) { - tN = tD; - if ((-d + b) < 0.0) { - sN = 0.0; - } else if ((-d + b) > a) { - sN = sD; - } else { - sN = (-d + b); - sD = a; - } - } - - // Calculate the closest points - sc = (fabs(sN) < 1e-6 ? 0.0 : sN / sD); - tc = (fabs(tN) < 1e-6 ? 0.0 : tN / tD); - - Eigen::Vector3d dP = w + sc * u - tc * v; // Vector between the closest points - - // Return the shortest distance - return {dP.norm(), s1.first + sc * u, s2.first + tc * v}; - } -} // namespace - -namespace cmvr::dyn { - Collision::Collision(std::string name) : name_(std::move(name)) { - } - - void Collision::SetOrigin(const math::SE3::MatrixType &T) { - T_ = T; - } - - math::SE3::MatrixType Collision::GetOrigin() const { - return T_; - } - - void Collision::AddGeom(const std::shared_ptr &geom) { - geoms_.push_back(geom); - } - - std::vector > Collision::GetGeoms() { - return geoms_; - } - - const std::vector > &Collision::GetGeoms() const { - return geoms_; - } - - shared_ptr Link::Make(string name, Inertial::MatrixType I) { - return shared_ptr(new Link(std::move(name), std::move(I))); - } - - Link::Link(string name, Inertial::MatrixType I) : name_(std::move(name)), I_(std::move(I)) { - } - - std::string Link::GetName() const { - return name_; - } - - weak_ptr Link::GetParentJoint() { - return parent_joint_; - } - - std::vector > Link::GetChildJointList() { - return child_joints_; - } - - const std::vector > &Link::GetChildJointList() const { - return child_joints_; - } - - void Link::AddCollision(const std::shared_ptr &collision) { - collisions_.push_back(collision); - } - - std::vector > Link::GetCollisions() { - return collisions_; - } - - const std::vector > &Link::GetCollisions() const { - return collisions_; - } - - GeomCapsule::GeomCapsule(double length, double radius, unsigned int coltype, unsigned int colaffinity) - : GeomCapsule({0, 0, length / 2}, {0, 0, -length / 2}, radius, coltype, colaffinity) { - } - - GeomCapsule::GeomCapsule(Eigen::Vector3d sp, Eigen::Vector3d ep, double radius, unsigned int coltype, - unsigned int colaffinity) - : Geom(coltype, colaffinity), sp_(std::move(sp)), ep_(std::move(ep)), radius_(radius) { - } - - GeomType GeomCapsule::GetType() const { - return GeomType::kCapsule; - } - - std::optional GeomCapsule::ComputeMinimumDistance(const math::SE3::MatrixType &T, - const Geom &other_geom, - const math::SE3::MatrixType &other_T) const { - switch (other_geom.GetType()) { - case GeomType::kCapsule: { - const auto &other_capsule = dynamic_cast(other_geom); - auto dis = compute_min_distance_two_segments( - {math::SE3::Multiply(T, GetStartPoint()), math::SE3::Multiply(T, GetEndPoint())}, - { - math::SE3::Multiply(other_T, other_capsule.GetStartPoint()), - math::SE3::Multiply(other_T, other_capsule.GetEndPoint()) - }); - CollisionResult rv; - rv.link1 = ""; - rv.link2 = ""; - rv.position1 = std::get<1>(dis); - rv.position2 = std::get<2>(dis); - rv.distance = std::get<0>(dis) - (radius_ + other_capsule.GetRadius()); - return rv; - } - } - - return {}; - } - - Eigen::Vector3d GeomCapsule::GetStartPoint() const { - return sp_; - } - - Eigen::Vector3d GeomCapsule::GetEndPoint() const { - return ep_; - } - - double GeomCapsule::GetRadius() const { - return radius_; - } -} diff --git a/cmvr-es/utils/dynamics/src/robot.cpp b/cmvr-es/utils/dynamics/src/robot.cpp deleted file mode 100644 index 347b2d97..00000000 --- a/cmvr-es/utils/dynamics/src/robot.cpp +++ /dev/null @@ -1,301 +0,0 @@ -// -// Created by xtkuang on 2025/7/8. -// - -#include "tinyxml2.h" -#include "../include/robot.h" -#include - -using namespace std; -using namespace cmvr::math; - -namespace { - using namespace cmvr; - using namespace cmvr::dyn; - - RobotConfiguration _load_robot_from_urdf(tinyxml2::XMLDocument &doc, const std::string &base_link_name) { - using namespace tinyxml2; - - const auto &ToDoubleArray3 = [](const std::string &str) { - std::stringstream ss(str); - std::array values{}; - ss >> values[0] >> values[1] >> values[2]; - return values; - }; - - const auto &GetOriginT = [&](XMLElement *origin_element) { - if (origin_element) { - std::array rpy{0, 0, 0}; - if (origin_element->FindAttribute("rpy")) { - rpy = ToDoubleArray3(origin_element->Attribute("rpy")); - } - - std::array xyz{0, 0, 0}; - if (origin_element->FindAttribute("xyz")) { - xyz = ToDoubleArray3(origin_element->Attribute("xyz")); - } - - return math::SE3::T(math::SO3::FromRPY({rpy[0], rpy[1], rpy[2]}), - Eigen::Vector3d{xyz[0], xyz[1], xyz[2]}); - } - return math::SE3::Identity(); - }; - - std::string robot_name{}; - std::unordered_map > links; - auto mobile_base = std::make_shared(); - mobile_base->type = MobileBaseType::None; - - XMLElement *robot = doc.FirstChildElement("robot"); - - if (robot->FindAttribute("name")) { - robot_name = robot->Attribute("name"); - } - - XMLElement *link = robot->FirstChildElement("link"); - while (link) { - std::string name = link->Attribute("name"); - Inertial::MatrixType I; - - XMLElement *inertial_element = link->FirstChildElement("inertial"); - if (inertial_element) { - double mass = 0.; - math::SE3::MatrixType T{math::SE3::Identity()}; - double ixx{0.}, ixy{0.}, ixz{0.}, iyy{0.}, iyz{0.}, izz{0.}; - - XMLElement *mass_element = inertial_element->FirstChildElement("mass"); - if (mass_element) { - mass = mass_element->DoubleAttribute("value"); - } - - XMLElement *origin_element = inertial_element->FirstChildElement("origin"); - if (origin_element) { - T = GetOriginT(origin_element); - } - - XMLElement *inertia_element = inertial_element->FirstChildElement("inertia"); - if (inertia_element) { - for (auto &[k, v]: std::vector >{ - {"ixx", ixx}, {"ixy", ixy}, {"ixz", ixz}, {"iyy", iyy}, {"iyz", iyz}, {"izz", izz} - }) { - if (inertia_element->FindAttribute(k.c_str())) { - v = inertia_element->DoubleAttribute(k.c_str()); - } - } - } - - I = Inertial::I(mass, ixx, iyy, izz, ixy, ixz, iyz, T); - } - links[name] = Link::Make(name, I); - - XMLElement *collision_element = link->FirstChildElement("collision"); - while (collision_element) { - std::string collision_name; - if (collision_element->FindAttribute("name")) { - collision_name = collision_element->Attribute("name"); - } - - math::SE3::MatrixType T{math::SE3::Identity()}; - XMLElement *origin_element = collision_element->FirstChildElement("origin"); - if (origin_element) { - T = GetOriginT(origin_element); - } - - std::shared_ptr geom; - - XMLElement *geometry_element = collision_element->FirstChildElement("geometry"); - if (geometry_element) { - // capsule - XMLElement *capsule_element = geometry_element->FirstChildElement("capsule"); - if (capsule_element) { - geom = std::make_shared(capsule_element->DoubleAttribute("length", 0), // - capsule_element->DoubleAttribute("radius", 0), // - capsule_element->UnsignedAttribute("coltype", 0), // - capsule_element->UnsignedAttribute("colaffinity", 0) // - ); - } - } - - if (geom) { - auto collision = std::make_shared(collision_name); - collision->SetOrigin(T); - collision->AddGeom(geom); - links[name]->AddCollision(collision); - } - - collision_element = collision_element->NextSiblingElement("collision"); - } - - link = link->NextSiblingElement("link"); - } - - XMLElement *joint = robot->FirstChildElement("joint"); - while (joint) { - std::string name = joint->Attribute("name"); - - std::string type = joint->Attribute("type"); - if (joint->FindAttribute("in_model_type")) { - type = joint->Attribute("in_model_type"); - } - - std::string parent_name, child_name; - math::SE3::MatrixType T{math::SE3::Identity()}; - - XMLElement *parent_element = joint->FirstChildElement("parent"); - if (parent_element) { - if (parent_element->FindAttribute("link")) { - parent_name = parent_element->Attribute("link"); - } else { - throw std::runtime_error("There is no link tag in parent element."); - } - } else { - throw std::runtime_error("There is no parent tag in joint."); - } - - XMLElement *child_element = joint->FirstChildElement("child"); - if (child_element) { - if (child_element->FindAttribute("link")) { - child_name = child_element->Attribute("link"); - } else { - throw std::runtime_error("There is no link tag in child element."); - } - } else { - throw std::runtime_error("There is no child tag in joint."); - } - - XMLElement *origin_element = joint->FirstChildElement("origin"); - if (origin_element) { - T = GetOriginT(origin_element); - } - - XMLElement *axis_element = joint->FirstChildElement("axis"); - std::array axis{0, 0, 0}; - if (axis_element) { - if (axis_element->FindAttribute("xyz")) { - axis = ToDoubleArray3(axis_element->Attribute("xyz")); - } - } else { - throw std::runtime_error("There is no axis element in joint."); - } - - std::shared_ptr joint_ptr; - if (type == "revolute" || type == "continuous") { - joint_ptr = Joint::MakeRevoluteJoint(name, math::SE3::Identity(), {axis[0], axis[1], axis[2]}); - } else if (type == "prismatic") { - joint_ptr = Joint::MakePrismaticJoint(name, math::SE3::Identity(), {axis[0], axis[1], axis[2]}); - } else { - joint_ptr = Joint::MakeFixedJoint(name); - } - if (links.find(parent_name) == links.end()) { - std::stringstream ss; - ss << "Cannot find the link name (" << parent_name << ")"; - throw std::runtime_error(ss.str()); - } - if (links.find(child_name) == links.end()) { - std::stringstream ss; - ss << "Cannot find the link name (" << child_name << ")"; - throw std::runtime_error(ss.str()); - } - joint_ptr->ConnectLinks(links[parent_name], links[child_name], T, math::SE3::Identity()); - - XMLElement *limit_element = joint->FirstChildElement("limit"); - if (limit_element) { - if (limit_element->FindAttribute("effort")) { - joint_ptr->SetLimitTorque(limit_element->DoubleAttribute("effort")); - } - if (limit_element->FindAttribute("lower")) { - joint_ptr->SetLimitQLower(limit_element->DoubleAttribute("lower")); - } - if (limit_element->FindAttribute("upper")) { - joint_ptr->SetLimitQUpper(limit_element->DoubleAttribute("upper")); - } - if (limit_element->FindAttribute("velocity")) { - double v = limit_element->DoubleAttribute("velocity"); - joint_ptr->SetLimitQdotLower(-v); - joint_ptr->SetLimitQdotUpper(v); - } - if (limit_element->FindAttribute("acceleration")) { - double a = limit_element->DoubleAttribute("acceleration"); - joint_ptr->SetLimitQddotLower(-a); - joint_ptr->SetLimitQddotUpper(a); - } - } - - joint = joint->NextSiblingElement("joint"); - } - - XMLElement *mobile = robot->FirstChildElement("mobile"); - if (mobile) { - if (mobile->FindAttribute("type")) { - std::string type = mobile->Attribute("type"); - - mobile_base->type = MobileBaseType::None; - if (type == "differential") { - mobile_base->type = MobileBaseType::Differential; - } else if (type == "mecanum") { - mobile_base->type = MobileBaseType::Mecanum; - } - } else { - throw std::runtime_error("There is not type in mobile."); - } - - mobile_base->T = GetOriginT(mobile); - - { - std::string params = mobile->Attribute("joints"); - std::stringstream ss(params); - std::string j; - while (ss >> j) { - mobile_base->joints.push_back(j); - } - } - - { - std::string params = mobile->Attribute("params"); - std::stringstream ss(params); - double param; - while (ss >> param) { - mobile_base->params.push_back(param); - } - } - } - - if (links.find(base_link_name) == links.end()) { - throw std::runtime_error("There is no base link name"); - } - - RobotConfiguration rc; - - rc.name = robot_name; - rc.base_link = links[base_link_name]; - rc.mobile_base = mobile_base; - - return rc; - } -} // namespace - -namespace cmvr::dyn { - RobotConfiguration LoadRobotFromURDFData(const std::string &model, const std::string &base_link_name) { - using namespace tinyxml2; - - XMLDocument doc; - XMLError ok = doc.Parse(model.c_str()); - if (ok != XMLError::XML_SUCCESS) { - throw std::runtime_error("Cannot parse model data"); - } - - return _load_robot_from_urdf(doc, base_link_name); - } - - RobotConfiguration LoadRobotFromURDF(const std::string &path, const std::string &base_link_name) { - using namespace tinyxml2; - - XMLDocument doc; - XMLError ok = doc.LoadFile(path.c_str()); - if (ok != XMLError::XML_SUCCESS) { - throw std::runtime_error("Load URDF failed"); - } - - return _load_robot_from_urdf(doc, base_link_name); - } -} diff --git a/cmvr-es/utils/dynamics/src/robot.tpp b/cmvr-es/utils/dynamics/src/robot.tpp deleted file mode 100644 index 517740d0..00000000 --- a/cmvr-es/utils/dynamics/src/robot.tpp +++ /dev/null @@ -1,828 +0,0 @@ -#pragma once - -#include "../include/robot.h" -using namespace cmvr::dyn; - - -template -Robot::Robot(const RobotConfiguration &robot_configuration) { - Build(robot_configuration); -} - -template -std::shared_ptr Robot::GetBase() { - return links_[0].links[0].link; -} - -template -std::vector Robot::GetLinkNames() const { - std::vector names; - for (const auto &[n, idx]: link_idx_) { - names.push_back(n); - } - return names; -} - -template -std::vector Robot::GetJointNames() const { - std::vector names; - for (const auto &[n, idx]: joint_idx_) { - names.push_back(n); - } - return names; -} - -template -std::shared_ptr Robot::GetLink(const std::string &name) const { - if (link_idx_.find(name) == link_idx_.end()) { - return nullptr; - } - const auto &idx = link_idx_.at(name); - return links_[idx.link_idx].links[idx.sub_link_idx].link; -} - -template -std::shared_ptr Robot::GetLink(std::shared_ptr > state, int index) const { - if (index >= static_cast(state->utr_link_map.size())) { - return nullptr; - } - const auto &idx = state->utr_link_map[index]; - return links_[idx.link_idx].links[idx.sub_link_idx].link; -} - -template -typename Robot::LinkIdx_ Robot::GetLinkIdx(const std::string& name) const { - auto it = link_idx_.find(name); - if (it == link_idx_.end()) { - throw std::runtime_error("Link name not found: " + name); - } - return it->second; -} - -template -template -std::shared_ptr> Robot::MakeState(const LinkContainer &link_names, const JointContainer &joint_names) { - static_assert(std::is_same_v || - std::is_same_v, - "LinkContainer value_type must be std::string"); - static_assert(std::is_same_v || - std::is_same_v, - "JointContainer value_type must be std::string"); - - std::vector flag; - flag.resize(n_joints_); - std::fill(flag.begin(), flag.end(), false); - - auto state = std::shared_ptr>(new State < DOF > (n_joints_)); - for (int i = 0; i < static_cast(joint_names.size()); i++) { - const auto &name = joint_names[i]; - - auto it = std::find_if(joints_.begin(), joints_.end(), - [name](const auto &j) { return j.joint->name_ == name; }); - if (it == joints_.end()) { - throw std::runtime_error("Cannot find the joint with name"); - } - state->utr_joint_map[i] = it - joints_.begin(); - state->joint_names[i] = it->joint->name_; - flag[state->utr_joint_map[i]] = true; - } - for (unsigned int i = joint_names.size(); i < n_joints_; i++) { - auto it = std::find(flag.begin(), flag.end(), false); - state->utr_joint_map[i] = it - flag.begin(); - state->joint_names[i] = (joints_.begin() + (it - flag.begin()))->joint->name_; - *it = true; - } - for (int i = 0; i < state->utr_joint_map.size(); i++) { - state->rtu_joint_map[state->utr_joint_map[i]] = i; - } - - std::unordered_map name_flag; - state->link_names.clear(); - state->utr_link_map.clear(); - for (const auto &name: link_names) { - auto it = link_idx_.find(name); - if (it == link_idx_.end()) { - throw std::runtime_error("The link with the given name does not exist"); - } - state->utr_link_map.push_back(it->second); - state->link_names.push_back(name); - name_flag[name] = true; - } - for (const auto &[k, v]: link_idx_) { - if (name_flag.find(k) == name_flag.end()) { - state->utr_link_map.push_back(v); - state->link_names.push_back(k); - } - } - for (auto i = 0; i < state->link_names.size(); i++) { - const auto &n = state->link_names[i]; - auto it = link_idx_.find(n); - if (it == link_idx_.end()) { - throw std::runtime_error("Fatal error; link names in state should be found in link idx"); - } - if (it->second.link_idx == 0 && it->second.sub_link_idx == 0) { - state->base_link_user_idx = i; - } - } - - return state; -} - -template -int Robot::GetDOF() const { - return n_joints_; -} - -template -int Robot::GetNumberOfJoints() const { - return n_joints_; -} - -template -void Robot::ComputeForwardKinematics(std::shared_ptr> state) { - for (auto i = 0; i < n_joints_; i++) { - state->E[i] = math::SE3::Exp(joints_[i].S, state->q(i)); - - int parent_joint_idx = links_[joints_[i].parent_link_idx].parent_joint_idx; - state->T[i] = GetJointT(state, parent_joint_idx) * state->E[i]; - - state->S.col(i) = math::SE3::Ad(GetJointT(state, parent_joint_idx), joints_[i].S); - } -} - -template -void Robot::ComputeDiffForwardKinematics(std::shared_ptr> state) { - // In Body Frame - for (auto i = 0; i < n_joints_; i++) { - state->V.col(i) = joints_[i].S * state->qdot(i); - - int parent_joint_idx = links_[joints_[i].parent_link_idx].parent_joint_idx; - state->V.col(i) += math::SE3::InvAd(state->E[i], GetJointV(state, parent_joint_idx)); - } -} - -template -void Robot::Compute2ndDiffForwardKinematics(std::shared_ptr> state) { - for (auto i = 0; i < n_joints_; i++) { - state->Vdot.col(i) = joints_[i].S * state->qddot(i); - - int parent_joint_idx = links_[joints_[i].parent_link_idx].parent_joint_idx; - state->Vdot.col(i) += math::SE3::InvAd(state->E[i], GetJointVdot(state, parent_joint_idx)) + - math::SE3::ad(state->V.col(i), joints_[i].S * state->qdot(i)); - } -} - -template -void Robot::ComputeInverseDynamics(std::shared_ptr> state) { - for (int i = n_joints_ - 1; i >= 0; i--) { - Eigen::Vector F; - F.setZero(); - - for (const auto &child_joint_idx: links_[joints_[i].child_link_idx].child_joint_idx) { - F += math::SE3::InvAd(state->E[child_joint_idx]).transpose() * state->F.col(child_joint_idx); - } - F += links_[joints_[i].child_link_idx].J * state->Vdot.col(i) - - math::SE3::adTranspose(state->V.col(i), links_[joints_[i].child_link_idx].J * state->V.col(i)); - - state->F.col(i) = F; - state->tau(i) = joints_[i].S.dot(F); - } -} - -template -Eigen::Vector Robot::ComputeGravityTerm(std::shared_ptr> state) { - Eigen::Vector gravity_term; - gravity_term.resize(n_joints_); - std::vector > Vdot{}, Fs{}; - Vdot.resize(n_joints_); - Fs.resize(n_joints_); - - // Calculate Vdot - for (auto i = 0; i < n_joints_; i++) { - int parent_joint_idx = links_[joints_[i].parent_link_idx].parent_joint_idx; - if (parent_joint_idx < 0) { - Vdot[i] = math::SE3::InvAd(state->E[i], state->Vdot0); - } else { - Vdot[i] = math::SE3::InvAd(state->E[i], Vdot[parent_joint_idx]); - } - } - - // - for (int i = n_joints_ - 1; i >= 0; i--) { - Eigen::Vector F; - F.setZero(); - - for (const auto &child_joint_idx: links_[joints_[i].child_link_idx].child_joint_idx) { - F += math::SE3::InvAd(state->E[child_joint_idx]).transpose() * Fs[child_joint_idx]; - } - F += links_[joints_[i].child_link_idx].J * Vdot[i]; - - Fs[i] = F; - gravity_term(state->rtu_joint_map[i]) = joints_[i].S.dot(F); - } - - return gravity_term; -} - -template -Eigen::Matrix Robot::ComputeMassMatrix(std::shared_ptr> state) { - Eigen::Matrix M(n_joints_, n_joints_); - M.setZero(); - std::vector > J; - J.resize(n_joints_); - - for (auto i = 0; i < n_joints_; i++) { - int parent_joint_idx = links_[joints_[i].parent_link_idx].parent_joint_idx; - if (parent_joint_idx < 0) { - J[i].setZero(); - J[i].col(state->rtu_joint_map[i]) = joints_[i].S; - } else { - J[i] = math::SE3::Ad(state->E[i].inverse()) * J[parent_joint_idx]; - J[i].col(state->rtu_joint_map[i]) = joints_[i].S; - } - } - - for (auto i = 0; i < n_joints_; i++) { - M += J[i].transpose() * links_[joints_[i].child_link_idx].J * J[i]; - } - - return M; -} - -template -Eigen::Matrix Robot::ComputeReflectiveInertia(std::shared_ptr> state, unsigned int from, unsigned int to) { - if (from >= state->utr_link_map.size() || to >= state->utr_link_map.size()) { - throw std::runtime_error("Out of range state link"); - } - const auto &J = ComputeBodyJacobian(state, from, to); - Eigen::Matrix m_inv = ComputeMassMatrix(state).completeOrthogonalDecomposition(). - pseudoInverse(); - return (J * m_inv * J.transpose()).completeOrthogonalDecomposition().pseudoInverse(); -} - -template -cmvr::math::SE3::MatrixType Robot::ComputeTransformation(std::shared_ptr> state, unsigned int from, unsigned int to) { - if (from >= state->utr_link_map.size() || to >= state->utr_link_map.size()) { - throw std::runtime_error("Out of range state link"); - } - return GetTransformation(state, state->utr_link_map[from], state->utr_link_map[to]); -} - -template -cmvr::math::se3v::MatrixType Robot::ComputeBodyVelocity(std::shared_ptr> state, unsigned int from, unsigned int to) { - if (from >= state->utr_link_map.size() || to >= state->utr_link_map.size()) { - throw std::runtime_error("Out of range state link"); - } - math::SE3::MatrixType T_from_to = ComputeTransformation(state, from, to); - math::se3v::MatrixType V_from = GetLinkV(state, state->utr_link_map[from]); - math::se3v::MatrixType V_to = GetLinkV(state, state->utr_link_map[to]); - return V_to - math::SE3::InvAd(T_from_to, V_from); -} - -template -Eigen::Matrix Robot::ComputeSpaceJacobian(std::shared_ptr> state, unsigned int from, unsigned int to) { - return GetSpaceJacobian(state, state->utr_link_map[from], state->utr_link_map[to]); -} - -template -Eigen::Matrix Robot::ComputeBodyJacobian(std::shared_ptr> state, unsigned int from, unsigned int to) { - math::SE3::MatrixType T = ComputeTransformation(state, from, to); - return math::SE3::InvAd(T) * ComputeSpaceJacobian(state, from, to); -} - -template -double Robot::ComputeMass(std::shared_ptr> state, unsigned int target_link) { - const auto &idx = state->utr_link_map[target_link]; - return Inertial::GetMass(links_[idx.link_idx].links[idx.sub_link_idx].link->I_); -} - -template -Eigen::Vector3d Robot::ComputeCenterOfMass(std::shared_ptr> state, unsigned int ref_link, unsigned int target_link) { - const auto &idx = state->utr_link_map[target_link]; - return math::SE3::Multiply(ComputeTransformation(state, ref_link, target_link), - Inertial::GetCOM(links_[idx.link_idx].links[idx.sub_link_idx].link->I_)); -} - -template -Eigen::Vector3d Robot::ComputeCenterOfMass(std::shared_ptr> state, unsigned int ref_link, const std::vector &target_links) { - Eigen::Vector3d com{Eigen::Vector3d::Zero()}; - double mass = 0; - for (auto target_link: target_links) { - const auto &idx = state->utr_link_map[target_link]; - const auto &p = Inertial::GetCOM(links_[idx.link_idx].links[idx.sub_link_idx].link->I_); - double m = Inertial::GetMass(links_[idx.link_idx].links[idx.sub_link_idx].link->I_); - com += math::SE3::Multiply(ComputeTransformation(state, ref_link, target_link), p) * m; - mass += m; - } - return com / mass; -} - -template -Eigen::Matrix Robot::ComputeCenterOfMassJacobian(std::shared_ptr> state, - unsigned int ref_link, - unsigned int target_link) { - using namespace math; - const auto &idx = state->utr_link_map[target_link]; - return SE3::GetRotation(ComputeTransformation(state, ref_link, target_link)) * - (SE3::InvAd(SE3::T(Inertial::GetCOM(links_[idx.link_idx].links[idx.sub_link_idx].link->I_))) * - ComputeBodyJacobian(state, ref_link, target_link)) - .block(3, 0, 3, n_joints_); -} - -template -Inertial::MatrixType Robot::ComputeTotalInertial(std::shared_ptr> state, unsigned int ref_link) { - Inertial::MatrixType I{Inertial::MatrixType::Zero()}; - for (const auto &link: links_) { - I += Inertial::Transform(GetJointT(state, link.parent_joint_idx), link.J); - } - - return Inertial::Transform(math::SE3::Inverse(GetLinkT(state, state->utr_link_map[ref_link])), I); -} - -template -Eigen::Vector3d Robot::ComputeCenterOfMass(std::shared_ptr> state, unsigned int ref_link) { - Eigen::Vector3d sum{Eigen::Vector3d::Zero()}; - double mass = 0; - for (const auto &link: links_) { - sum += Inertial::GetMass(link.J) * - math::SE3::Multiply(GetJointT(state, link.parent_joint_idx), Inertial::GetCOM(link.J)); - mass += Inertial::GetMass(link.J); - } - return math::SE3::Multiply(math::SE3::Inverse(GetLinkT(state, state->utr_link_map[ref_link])), sum / mass); -} - -template -Eigen::Matrix Robot::ComputeCenterOfMassJacobian(std::shared_ptr> state, - unsigned int ref_link) { - using namespace math; - Eigen::Matrix J; - J.resize(3, n_joints_); - J.setZero(); - double mass = 0; - for (auto i = 0; i < links_.size(); i++) { - // const auto& link = links_[i]; - double m = Inertial::GetMass(links_[i].I); - J += m * SE3::GetRotation(GetTransformation(state, state->utr_link_map[ref_link], {i, 0})) * - (SE3::InvAd(SE3::T(Inertial::GetCOM(links_[i].I))) * - GetBodyJacobian(state, state->utr_link_map[ref_link], {i, 0})) - .block(3, 0, 3, n_joints_); - mass += m; - } - return J / mass; -} - -template -std::vector Robot::DetectCollisionsOrNearestLinks(std::shared_ptr> state, int collision_threshold) { - std::vector dis; - int n = state->GetLinkNames().size(); - for (int i = 0; i < n; i++) { - auto link1 = GetLink(state, i); - if (!link1) { - throw std::runtime_error("Index error"); - } - auto link1_T = ComputeTransformation(state, 0, i); - for (int j = i + 1; j < n; j++) { - auto link2 = GetLink(state, j); - if (!link2) { - throw std::runtime_error("Index error"); - } - auto link2_T = ComputeTransformation(state, 0, j); - - for (auto link1_col: link1->GetCollisions()) { - for (auto link2_col: link2->GetCollisions()) { - for (auto link1_geom: link1_col->GetGeoms()) { - for (auto link2_geom: link2_col->GetGeoms()) { - if (link1_geom->GetType() == GeomType::kCapsule && // - link2_geom->GetType() == GeomType::kCapsule && // - link1_geom->Filter(*link2_geom)) { - auto collision_result = - link1_geom->ComputeMinimumDistance( - link1_T * link1_col->GetOrigin(), *link2_geom, // - link2_T * link2_col->GetOrigin()); - if (collision_result.has_value()) { - auto v = collision_result.value(); - v.link1 = link1->GetName(); - v.link2 = link2->GetName(); - dis.push_back(v); - } - } - } - } - } - } - } - } - - std::sort(dis.begin(), dis.end(), - [](const CollisionResult &r1, const CollisionResult &r2) { return r1.distance < r2.distance; }); - - std::vector rv; - for (const auto &d: dis) { - if (d.distance > 0 && static_cast(rv.size()) >= collision_threshold) { - break; - } - rv.push_back(d); - } - return rv; -} - -// TODO: ComputeBodyJacobianDot -// TODO: ComputeBodyAcceleration -// TODO: InverseDiffDynamics - -template -Eigen::Vector Robot::GetLimitQLower(const std::shared_ptr> &state) { - return GetJointProperty(state, [](auto j) { return j->GetLimitQLower(); }); -} - -template -Eigen::Vector Robot::GetLimitQUpper(const std::shared_ptr> &state) { - return GetJointProperty(state, [](auto j) { return j->GetLimitQUpper(); }); -} - -template -Eigen::Vector Robot::GetLimitQdotLower(const std::shared_ptr> &state) { - return GetJointProperty(state, [](auto j) { return j->GetLimitQdotLower(); }); -} - -template -Eigen::Vector Robot::GetLimitQdotUpper(const std::shared_ptr> &state) { - return GetJointProperty(state, [](auto j) { return j->GetLimitQdotUpper(); }); -} - -template -Eigen::Vector Robot::GetLimitQddotLower(const std::shared_ptr> &state) { - return GetJointProperty(state, [](auto j) { return j->GetLimitQddotLower(); }); -} - -template -Eigen::Vector Robot::GetLimitQddotUpper(const std::shared_ptr> &state) { - return GetJointProperty(state, [](auto j) { return j->GetLimitQddotUpper(); }); -} - -template -Eigen::Vector Robot::GetLimitTorque(const std::shared_ptr> &state) { - return GetJointProperty(state, [](auto j) { return j->GetLimitTorque(); }); -} - -template -Eigen::Vector Robot::GetJointProperty(const std::shared_ptr> &state, - const std::function)> &getter) { - Eigen::Vector prop; - prop.resize(n_joints_); - for (auto i = 0; i < n_joints_; i++) { - prop(state->rtu_joint_map[i]) = getter(joints_[i].joint); - } - return prop; -} - -template -void Robot::ComputeMobilityInverseDiffKinematics(std::shared_ptr> state, // - const Eigen::Vector2d &linear_velocity, // (m/s) - double angular_velocity // (rad/s) -) { - math::se2v::MatrixType S; - S(0) = angular_velocity; - S.tail<2>() = linear_velocity; - ComputeMobilityInverseDiffKinematics(state, S); -} - -template -void Robot::ComputeMobilityInverseDiffKinematics(std::shared_ptr> state, // - const math::se2v::MatrixType &body_velocity // w, x, y -) { - math::se3v::MatrixType S{math::se3v::MatrixType::Zero()}; - S.block<3, 1>(2, 0) = body_velocity; - - S = math::SE3::InvAd(mobile_base_->T, S); - - switch (mobile_base_->type) { - case MobileBaseType::None: { - break; - } - case MobileBaseType::Differential: { - auto mb = std::static_pointer_cast(mobile_base_); - double v_right = S(3) + mb->wheel_base / 2 * S(2); - double v_left = S(3) - mb->wheel_base / 2 * S(2); - state->qdot(mb->right_wheel_idx) = -v_right / mb->wheel_radius; - state->qdot(mb->left_wheel_idx) = -v_left / mb->wheel_radius; - break; - } - case MobileBaseType::Mecanum: { - double v_x = S(3); - double v_y = S(4); - double w = S(2); - // std::cout<<"V_x: "<< v_x<(mobile_base_); - double w_1 = 1 / mb->wheel_radius * (v_x - v_y - (mb->L_x + mb->L_y) * w); - double w_2 = 1 / mb->wheel_radius * (v_x + v_y + (mb->L_x + mb->L_y) * w); - double w_3 = 1 / mb->wheel_radius * (v_x + v_y - (mb->L_x + mb->L_y) * w); - double w_4 = 1 / mb->wheel_radius * (v_x - v_y + (mb->L_x + mb->L_y) * w); - state->qdot(mb->fl_wheel_idx) = w_1; - state->qdot(mb->fr_wheel_idx) = w_2; - state->qdot(mb->rl_wheel_idx) = w_3; - state->qdot(mb->rr_wheel_idx) = w_4; - - // std::cout<<"mb->fl_wheel_idx: "<fl_wheel_idx<fr_wheel_idx: "<fr_wheel_idx<rl_wheel_idx: "<rl_wheel_idx<rr_wheel_idx: "<rr_wheel_idx< -cmvr::math::se2v::MatrixType Robot::ComputeMobilityDiffKinematics( // - std::shared_ptr> state // -) { - math::se3v::MatrixType S{math::se3v::MatrixType::Zero()}; - - switch (mobile_base_->type) { - case MobileBaseType::None: { - break; - } - case MobileBaseType::Differential: { - auto mb = std::static_pointer_cast(mobile_base_); - double w_r = -state->qdot(mb->right_wheel_idx); - double w_l = -state->qdot(mb->left_wheel_idx); - S.block<3, 1>(2, 0) = math::se2v::MatrixType{ - (w_r - w_l) * mb->wheel_radius / mb->wheel_base, - (w_r + w_l) * mb->wheel_radius / 2, 0 - }; - break; - } - case MobileBaseType::Mecanum: { - auto mb = std::static_pointer_cast(mobile_base_); - double w_1 = state->qdot(mb->fl_wheel_idx); - double w_2 = state->qdot(mb->fr_wheel_idx); - double w_3 = state->qdot(mb->rl_wheel_idx); - double w_4 = state->qdot(mb->rr_wheel_idx); - double v_x = mb->wheel_radius / 4 * (w_1 + w_2 + w_3 + w_4); - double v_y = mb->wheel_radius / 4 * (-w_1 + w_2 + w_3 - w_4); - double w = mb->wheel_radius / (4 * (mb->L_x + mb->L_y)) * (-w_1 + w_2 - w_3 + w_4); - S.block<3, 1>(2, 0) = math::se2v::MatrixType{w, v_x, v_y}; - break; - } - } - S = math::SE3::Ad(mobile_base_->T, S); - return S.block<3, 1>(2, 0); -} - -template -int Robot::CountJoints(const std::shared_ptr &base_link, bool include_fixed) { - int n_joints = 0; - std::queue > que; - que.push(base_link); - while (!que.empty()) { - std::shared_ptr link = que.front(); - que.pop(); - - for (const auto &joint: link->GetChildJointList()) { - que.push(joint->GetChildLink()); - - if (joint->IsFixed() && !include_fixed); - else { - n_joints++; - } - } - } - return n_joints; -} - -template -void Robot::Build(const RobotConfiguration &rc) { - const auto &base = rc.base_link; - int n_joints = CountJoints(base); - if constexpr (DOF < 0) { - n_links_ = n_joints + 1; - n_joints_ = n_joints; - - links_.resize(n_links_); - joints_.resize(n_joints_); - } else { - if (n_joints != DOF) { - throw std::runtime_error("DOF does not match the number of joints"); - } - n_links_ = DOF + 1; - n_joints_ = DOF; - } - - struct QueueItem { - int parent_joint_idx{}; - int parent_link_idx{}; - std::shared_ptr link; - int depth{}; - bool merge_parent_link{}; - math::SE3::MatrixType M_wrt_base; - math::SE3::MatrixType M_wrt_p; - }; - - std::queue que; - int link_idx = 0; - int joint_idx = 0; - { - { - QueueItem item; - item.parent_joint_idx = -1; - item.parent_link_idx = link_idx++; - item.link = base; - item.depth = 0; - item.merge_parent_link = false; - item.M_wrt_base = math::SE3::Identity(); - item.M_wrt_p = math::SE3::Identity(); - que.push(item); - } - - while (!que.empty()) { - auto e = que.front(); - que.pop(); - - if (!e.merge_parent_link) { - links_[e.parent_link_idx].SetBaseLink(e.link, e.M_wrt_base); - links_[e.parent_link_idx].depth = e.depth; - links_[e.parent_link_idx].parent_joint_idx = e.parent_joint_idx; - link_idx_[e.link->name_] = {e.parent_link_idx, 0}; - } else { - int sub_link_idx = links_[e.parent_link_idx].AddLink(e.link, e.M_wrt_p); - link_idx_[e.link->name_] = {e.parent_link_idx, sub_link_idx}; - } - - for (const auto &joint: e.link->child_joints_) { - if (!joint->fixed_) { - math::SE3::MatrixType M_wrt_base = e.M_wrt_base * joint->T_pj_; - { - Joint_ j; - j.joint = joint; - j.S = math::SE3::Ad(M_wrt_base, joint->S_); - j.parent_link_idx = e.parent_link_idx; - j.child_link_idx = link_idx; - joints_[joint_idx] = j; - } - joint_idx_[joint->name_] = joint_idx; - links_[e.parent_link_idx].child_joint_idx.push_back(joint_idx); - { - QueueItem item; - item.parent_joint_idx = joint_idx++; - item.parent_link_idx = link_idx++; - item.link = joint->child_link_; - item.depth = e.depth + 1; - item.merge_parent_link = false; - item.M_wrt_base = M_wrt_base; - item.M_wrt_p = math::SE3::Identity(); - que.push(item); - } - } else { - QueueItem item; - item.parent_joint_idx = e.parent_joint_idx; - item.parent_link_idx = e.parent_link_idx; - item.link = joint->child_link_; - item.depth = e.depth; - item.merge_parent_link = true; - item.M_wrt_base = e.M_wrt_base * joint->T_pj_ * joint->T_jc_; - item.M_wrt_p = e.M_wrt_p * joint->T_pj_ * joint->T_jc_; - que.push(item); - } - } - } - } - - if (rc.mobile_base) { - switch (rc.mobile_base->type) { - case MobileBaseType::None: { - auto mb = std::make_shared(); - *std::static_pointer_cast(mb) = *rc.mobile_base; - mobile_base_ = mb; - break; - } - case MobileBaseType::Differential: { - auto mb = std::make_shared(); - *std::static_pointer_cast(mb) = *rc.mobile_base; - if (rc.mobile_base->joints.size() != 2) { - throw std::runtime_error("Differential type mobile should have two joints."); - } - if (joint_idx_.find(rc.mobile_base->joints[0]) == joint_idx_.end()) { - throw std::runtime_error("Right wheel has invalid parameter."); - } - if (joint_idx_.find(rc.mobile_base->joints[1]) == joint_idx_.end()) { - throw std::runtime_error("Left wheel has invalid parameter."); - } - if (rc.mobile_base->params.size() != 2) { - throw std::runtime_error("Differential type mobile should have two parameters."); - } - mb->right_wheel_idx = joint_idx_[rc.mobile_base->joints[0]]; - mb->left_wheel_idx = joint_idx_[rc.mobile_base->joints[1]]; - mb->wheel_base = rc.mobile_base->params[0]; - mb->wheel_radius = rc.mobile_base->params[1]; - mobile_base_ = mb; - break; - } - case MobileBaseType::Mecanum: - auto mb = std::make_shared(); - *std::static_pointer_cast(mb) = *rc.mobile_base; - if (rc.mobile_base->joints.size() != 4) { - throw std::runtime_error( - "Mecanum type mobile should have four joints. (front-right, front-left, rear-right, rear-left)"); - } - if (joint_idx_.find(rc.mobile_base->joints[0]) == joint_idx_.end()) { - throw std::runtime_error("Front-right wheel has invalid parameter."); - } - if (joint_idx_.find(rc.mobile_base->joints[1]) == joint_idx_.end()) { - throw std::runtime_error("Front-left wheel has invalid parameter."); - } - if (joint_idx_.find(rc.mobile_base->joints[2]) == joint_idx_.end()) { - throw std::runtime_error("Rear-right wheel has invalid parameter."); - } - if (joint_idx_.find(rc.mobile_base->joints[3]) == joint_idx_.end()) { - throw std::runtime_error("Rear-left wheel has invalid parameter."); - } - if (rc.mobile_base->params.size() != 3) { - throw std::runtime_error( - "Mecanum type mobile should have three parameters. (Lx, Ly, wheel radius)"); - } - mb->fr_wheel_idx = joint_idx_[rc.mobile_base->joints[0]]; - mb->fl_wheel_idx = joint_idx_[rc.mobile_base->joints[1]]; - mb->rr_wheel_idx = joint_idx_[rc.mobile_base->joints[2]]; - mb->rl_wheel_idx = joint_idx_[rc.mobile_base->joints[3]]; - mb->L_x = rc.mobile_base->params[0]; - mb->L_y = rc.mobile_base->params[1]; - mb->wheel_radius = rc.mobile_base->params[2]; - mobile_base_ = mb; - break; - } - } -} - -template -cmvr::math::SE3::MatrixType Robot::GetLinkT(std::shared_ptr> state, const LinkIdx_ &idx) { - return GetJointT(state, links_[idx.link_idx].parent_joint_idx) * - links_[idx.link_idx].links[idx.sub_link_idx].M_wrt_base; -} - -template -cmvr::math::se3v::MatrixType Robot::GetLinkV(std::shared_ptr> state, const LinkIdx_ &idx) { - return math::SE3::InvAd(links_[idx.link_idx].links[idx.sub_link_idx].M_wrt_base, - GetJointV(state, links_[idx.link_idx].parent_joint_idx)); -} - -template -cmvr::math::SE3::MatrixType Robot::GetJointT(std::shared_ptr> state, int joint_idx) { - if (joint_idx < 0) - return math::SE3::Identity(); - return state->T[joint_idx]; -} - -template -cmvr::math::se3v::MatrixType Robot::GetJointV(std::shared_ptr> state, int joint_idx) { - if (joint_idx < 0) - return state->V0; - return state->V.col(joint_idx); -} - -template -cmvr::math::se3v::MatrixType Robot::GetJointVdot(std::shared_ptr> state, int joint_idx) { - if (joint_idx < 0) - return state->Vdot0; - return state->Vdot.col(joint_idx); -} - -template -cmvr::math::SE3::MatrixType Robot::GetTransformation(std::shared_ptr> state, const LinkIdx_ &from, const LinkIdx_ &to) { - return math::SE3::Inverse(GetLinkT(state, from)) * GetLinkT(state, to); -} - -template -Eigen::Matrix Robot::GetSpaceJacobian(std::shared_ptr> state, const LinkIdx_ &from, const LinkIdx_ &to) { - Eigen::Matrix J; - J.resize(6, n_joints_); - J.setZero(); - - // TODO: Need to optimize - - int from_link_idx = from.link_idx; - int to_link_idx = to.link_idx; - while (from_link_idx != to_link_idx) { - if (links_[from_link_idx].depth > links_[to_link_idx].depth) { - int pj = links_[from_link_idx].parent_joint_idx; - J.col(state->rtu_joint_map[pj]) = -state->S.col(pj); - from_link_idx = joints_[pj].parent_link_idx; - } else { - int pj = links_[to_link_idx].parent_joint_idx; - J.col(state->rtu_joint_map[pj]) = state->S.col(pj); - to_link_idx = joints_[pj].parent_link_idx; - } - } - - return math::SE3::InvAd(GetLinkT(state, from)) * J; -} - -template -Eigen::Matrix Robot::GetBodyJacobian(std::shared_ptr> state, const LinkIdx_ &from, - const LinkIdx_ &to) { - math::SE3::MatrixType T = GetTransformation(state, from, to); - return math::SE3::InvAd(T) * GetSpaceJacobian(state, from, to); -} \ No newline at end of file diff --git a/cmvr-es/utils/math/include/constants.h b/cmvr-es/utils/math/include/constants.h deleted file mode 100644 index 9644e0b1..00000000 --- a/cmvr-es/utils/math/include/constants.h +++ /dev/null @@ -1,52 +0,0 @@ -// -// Created by xtkuang on 2025/7/7. -// - -#ifndef CONSTANCE_H -#define CONSTANCE_H - -#pragma once - -#include - -namespace cmvr::math { - - constexpr double kPi = 3.1415926535897932384626433832795; ///< \pi - constexpr double kPiHalf = 1.5707963267948966192313216916398; ///< \frac{\pi}{2} - constexpr double kPiDouble = 6.283185307179586476925286766559; ///< \pi*2 - constexpr double kInvPiDouble = 0.15915494309189533576888376337251; ///< \frac{1}{\pi*2} - constexpr double kPiDividedBySqrt2 = 2.2214414690791831235079404950303; ///< \frac{\pi}{\sqrt{2}} - constexpr double kPiSquare = 9.8696044010893586188344909998762; ///< \pi^2 - - constexpr double kRad2Deg = 57.295779513082320876798154814105; ///< \frac{180}{\pi} - constexpr double kDeg2Rad = 0.01745329251994329576923690768489; /// \frac{\pi}{180} - - static const Eigen::Matrix kAffineConstant = {0, 0, 0, 1}; - - static const double kDoubleEpsilon = std::numeric_limits::epsilon(); - - /// Calculate sine and cosine simultaneously - inline void fsincos(double theta, ///< Angle in radians - double& sine, ///< Variable for storing a sine value - double& cosine ///< Variable for storing a sine value - ) { - using std::sin; - using std::sqrt; - - theta -= (int)(theta * kInvPiDouble) * kPiDouble; - if (theta < 0) - theta += kPiDouble; - - sine = sin(theta); - if (theta < kPiHalf) { - cosine = sqrt(1 - sine * sine); - return; - } else if (theta < kPi + kPiHalf) { - cosine = -sqrt(1 - sine * sine); - return; - } - cosine = sqrt(1 - sine * sine); - } -} - -#endif //CONSTANCE_H \ No newline at end of file diff --git a/cmvr-es/utils/math/include/liegroup.h b/cmvr-es/utils/math/include/liegroup.h deleted file mode 100644 index 4526401f..00000000 --- a/cmvr-es/utils/math/include/liegroup.h +++ /dev/null @@ -1,15 +0,0 @@ -// -// Created by xtkuang on 2025/7/7. -// - -#ifndef LIEGROUP_H -#define LIEGROUP_H - -#pragma once - -#include "se2.h" - -#include "se3.h" -#include "so3.h" - -#endif //LIEGROUP_H \ No newline at end of file diff --git a/cmvr-es/utils/math/include/qp_solver.h b/cmvr-es/utils/math/include/qp_solver.h deleted file mode 100644 index c71bc898..00000000 --- a/cmvr-es/utils/math/include/qp_solver.h +++ /dev/null @@ -1,126 +0,0 @@ -// -// Created by xtkuang on 2025/7/7. -// - -#ifndef QP_SOLVER_H -#define QP_SOLVER_H - -#pragma once - -#include -#include -#include - -#include -#include - -namespace cmvr::math { - -class QPSolverException : public std::exception { -public: - static constexpr unsigned int kStatusOffset = 100; - - explicit QPSolverException(int error_code); - - const char* what() const noexcept override; - - int code() const noexcept; - - static std::string GenerateMessage(int code); - -private: - int error_code_; - std::string message_; -}; - -class QPSolverImpl { -public: - QPSolverImpl() = default; - ~QPSolverImpl() = default; - - void SetupImpl(int n_var, int n_const, double time_limit); - void InitFunctionImpl(); - void AddCostFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &b); - void SetCostFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &b); - void SetConstraintsFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub); - void SetPrimalVariableImpl(const Eigen::VectorXd &pv); - void ResetIsFirstImpl(); - Eigen::VectorXd SolveImpl(); - Eigen::MatrixXd GetACostImpl() const; - Eigen::VectorXd GetBCostImpl() const; - Eigen::MatrixXd GetAConstImpl() const; - Eigen::VectorXd GetLowerBoundImpl() const; - Eigen::VectorXd GetUpperBoundImpl() const; - -private: - OsqpEigen::Solver solver_; - int n_var_{}; - int n_const_{}; - int err_code_{}; - - int is_first_{}; - - int n_hessian_element_{}; - - Eigen::Matrix A_cost_; - Eigen::Matrix b_cost_; - - Eigen::Matrix A_const_; - Eigen::Matrix lb_; - Eigen::Matrix ub_; - - Eigen::SparseMatrix hessian_; - Eigen::Matrix gradient_; - - Eigen::SparseMatrix linearMatrix_; - Eigen::Matrix lowerBound_; - Eigen::Matrix upperBound_; - - Eigen::Matrix primal_variable_for_warmstart_; -}; - - -class QPSolver { -public: - QPSolver(); - - ~QPSolver(); - - void Setup(int n_var, int n_const, double time_limit = 2e-3); - - void InitFunction(); - - void AddCostFunction(const Eigen::MatrixXd& A, const Eigen::VectorXd& b); - - void SetCostFunction(const Eigen::MatrixXd& A, const Eigen::VectorXd& b); - - void SetConstraintsFunction(const Eigen::MatrixXd& A, const Eigen::VectorXd& lb, const Eigen::VectorXd& ub); - - void SetPrimalVariable(const Eigen::VectorXd& pv); - - void ResetIsFirst(); - - /** - * Solve the QP problem - * @return Solution - * @throw QPSolverException - */ - Eigen::VectorXd Solve(); - - Eigen::MatrixXd GetACost() const; // NOLINT - - Eigen::VectorXd GetBCost() const; // NOLINT - - Eigen::MatrixXd GetAConst() const; // NOLINT - - Eigen::VectorXd GetLowerBound() const; // NOLINT - - Eigen::VectorXd GetUpperBound() const; // NOLINT - -private: - std::unique_ptr impl_; -}; - -} - -#endif //QP_SOLVER_H \ No newline at end of file diff --git a/cmvr-es/utils/math/include/se2.h b/cmvr-es/utils/math/include/se2.h deleted file mode 100644 index 8ca48cc4..00000000 --- a/cmvr-es/utils/math/include/se2.h +++ /dev/null @@ -1,42 +0,0 @@ -// -// Created by xtkuang on 2025/7/7. -// - -#ifndef SE2_H -#define SE2_H - - -#pragma once - -#include -#include "constants.h" - -namespace cmvr::math { - - class SE2; - class se2v; - - class se2v { - public: - using MatrixType = Eigen::Vector3d; - - private: - se2v() = default; - }; - - class SE2 { - public: - using MatrixType = Eigen::Matrix3d; - - static MatrixType Identity(); - - static MatrixType T(double angle, const Eigen::Vector& translation = {0, 0}); - - static MatrixType Exp(const se2v::MatrixType& s, double angle = 1.0); - - static se2v::MatrixType Log(const MatrixType& T); - }; - -} // namespace rb::math - -#endif //SE2_H \ No newline at end of file diff --git a/cmvr-es/utils/math/include/se3.h b/cmvr-es/utils/math/include/se3.h deleted file mode 100644 index f2fb6fc6..00000000 --- a/cmvr-es/utils/math/include/se3.h +++ /dev/null @@ -1,90 +0,0 @@ -// -// Created by xtkuang on 2025/7/7. -// - -#ifndef SE3_H -#define SE3_H - -#pragma once - -#include -#include - -#include "so3.h" - -namespace cmvr::math { - -class SE3; -class se3; -class se3v; - -class se3 { - public: - using MatrixType = Eigen::Matrix4d; - - private: - se3() = default; -}; - -class se3v { - public: - using MatrixType = Eigen::Vector; - - private: - se3v() = default; -}; - -class SE3 { - public: - using MatrixType = Eigen::Matrix4d; - - static MatrixType Identity(); - - static MatrixType T(const Eigen::Vector3d& p); - - static MatrixType T(const SO3::MatrixType& R, const Eigen::Vector3d& p = Eigen::Vector3d::Zero()); - - static MatrixType Inverse(const MatrixType& T); - - static SO3::MatrixType GetRotation(const MatrixType& T); - - static Eigen::Vector3d GetPosition(const MatrixType& T); - - static MatrixType Exp(const se3v::MatrixType& S, double angle = 1.); - - static MatrixType Exp(so3v::MatrixType w, Eigen::Vector3d v, double angle = 1.); - - static se3v::MatrixType Log(const MatrixType& T); - - static typename Eigen::Matrix Ad(const MatrixType& T); - - static se3v::MatrixType Ad(const MatrixType& T, const se3v::MatrixType& S); - - static Eigen::Matrix InvAd(const MatrixType& T); - - static se3v::MatrixType InvAd(const MatrixType& T, const se3v::MatrixType& S); - - static typename Eigen::Matrix ad(const se3v::MatrixType& S); - - static Eigen::Matrix adTranspose(const se3v::MatrixType& S); - - static se3v::MatrixType ad(const se3v::MatrixType& S1, const se3v::MatrixType& S2); - - static se3v::MatrixType adTranspose(const se3v::MatrixType& S1, const se3v::MatrixType& S2); - - static Eigen::Vector3d Multiply(const MatrixType& T, const Eigen::Vector3d& p); - - template >> - static std::optional Average(const Container& matrices, double eps, int max_iter = -1); - - static se3v::MatrixType Vec(const se3::MatrixType& s); - - static se3::MatrixType Hat(const se3v::MatrixType& v); - - private: - SE3() = default; -}; - -} // namespace rb::math - -#endif //SE3_H \ No newline at end of file diff --git a/cmvr-es/utils/math/include/so3.h b/cmvr-es/utils/math/include/so3.h deleted file mode 100644 index 68f6fd4b..00000000 --- a/cmvr-es/utils/math/include/so3.h +++ /dev/null @@ -1,121 +0,0 @@ -// -// Created by xtkuang on 2025/7/7. -// - -#ifndef SO3_H -#define SO3_H - -#pragma once - -#include -#include -#include -#include - -#include "constants.h" - -namespace cmvr::math { - enum class EulerAngleType { ZYX, ZYZ }; - - class SO3; - class so3; - class so3v; - - class so3 { - public: - using MatrixType = Eigen::Matrix3d; - - private: - so3() = default; - }; - - class so3v { - public: - using MatrixType = Eigen::Vector3d; - - private: - so3v() = default; - }; - - class SO3 { - public: - using MatrixType = Eigen::Matrix3d; - - static MatrixType Identity(); - - static MatrixType Inverse(const MatrixType &R); - - static MatrixType Exp(so3v::MatrixType w, double angle = 1.0); - - static so3v::MatrixType Log(const MatrixType &R); - - static MatrixType RotX(double angle); - - static MatrixType RotY(double angle); - - static MatrixType RotZ(double angle); - - static MatrixType FromEulerAngle(const Eigen::Vector3d &angles, EulerAngleType type); - - static Eigen::Vector3d ToEulerAngle(const MatrixType &R, EulerAngleType type); - - static MatrixType FromQuaternion(const Eigen::Quaterniond &q); - - static Eigen::Quaterniond ToQuaternion(const MatrixType &R); - - /** - * Calculate rotation from roll, pitch and yqw angles - * @param angles [0] roll [1] pitch [2] yaw - * @return - */ - static MatrixType FromRPY(const Eigen::Vector3d &angles); - - /** - * - * @param R - * @return [0] roll [1] pitch [2] yaw - */ - static Eigen::Vector3d ToRPY(const MatrixType &R); - - static MatrixType Projection(const MatrixType &m); - - template > > - static std::optional Average(const Container &matrices, double eps, int max_iter = -1) { - if (matrices.size() == 0) { - return Identity(); - } - - SO3::MatrixType avg = *matrices.begin(); - for (int i = 0; max_iter < 0 || i < max_iter; i++) { - Eigen::Vector3d w = Eigen::Vector3d::Zero(); - - for (const auto &m: matrices) { - w += Log(m * avg.inverse()); - } - w /= matrices.size(); - avg = SO3::Exp(w) * avg; - if (w.norm() < eps) { - return avg; - } - } - - return std::nullopt; - } - - static Eigen::Vector3d GetX(const MatrixType &R); - - static Eigen::Vector3d GetY(const MatrixType &R); - - static Eigen::Vector3d GetZ(const MatrixType &R); - - static so3v::MatrixType Vec(const so3::MatrixType &r); - - static so3::MatrixType Hat(const so3v::MatrixType &w); - - private: - SO3() = default; - }; -} - -#endif //SO3_H diff --git a/cmvr-es/utils/math/include/velocity_estimator.h b/cmvr-es/utils/math/include/velocity_estimator.h deleted file mode 100644 index 5cfbf581..00000000 --- a/cmvr-es/utils/math/include/velocity_estimator.h +++ /dev/null @@ -1,124 +0,0 @@ -// -// Created by xtkuang on 2025/7/7. -// - -#ifndef VELOCITY_ESTIMATOR_H -#define VELOCITY_ESTIMATOR_H - -#pragma once - -#include -#include - -#include "Eigen/Dense" - -namespace cmvr { - class VelocityFilterEstimator { - public: - explicit VelocityFilterEstimator(double dt, int avg_count = 5) - : avg_count_(avg_count), valid_(false), velocity_(0), dt_(dt), sum_velocity_(0) { - } - - void Update(double position, bool valid) { - if (valid && valid_) { - double v = (position - prev_position_) / dt_; - velocities_.push(v); - sum_velocity_ += v; - while (velocities_.size() > avg_count_) { - sum_velocity_ -= velocities_.front(); - velocities_.pop(); - } - } else { - while (!velocities_.empty()) { - velocities_.pop(); - } - sum_velocity_ = 0; - } - prev_position_ = position; - valid_ = valid; - } - - double GetVelocity() { - if (velocities_.empty()) { - return 0.; - } - return sum_velocity_ / (double) velocities_.size(); - } - - private: - unsigned int avg_count_; - std::queue velocities_; - double sum_velocity_; - - bool valid_; - double prev_position_{0.}; - - double velocity_; - double dt_; - }; - - class VelocityEstimator { - public: - VelocityEstimator() { - x_.setZero(); - P_.setIdentity(); - F_.setZero(); - Q_.setZero(); - H_ << 1, 0; - R_.setZero(); - } - - VelocityEstimator(double initial_position, double initial_velocity, double position_variance, - double velocity_variance, double measurement_variance) - : VelocityEstimator() { - SetInitialPosition(initial_position); - SetInitialVelocity(initial_velocity); - SetPositionVariance(position_variance); - SetVelocityVariance(velocity_variance); - SetMeasurementVariance(measurement_variance); - } - - void SetInitialPosition(double initial_position) { x_(0) = initial_position; } - - void SetInitialVelocity(double initial_velocity) { x_(1) = initial_velocity; } - - void SetPositionVariance(double position_variance) { Q_(0, 0) = position_variance; } - - void SetVelocityVariance(double velocity_variance) { Q_(1, 1) = velocity_variance; } - - void SetMeasurementVariance(double measurement_variance) { R_(0, 0) = measurement_variance; } - - void Predict(double dt) { - F_(0, 0) = 1; - F_(0, 1) = dt; - F_(1, 0) = 0; - F_(1, 1) = 1; - - x_ = F_ * x_; - P_ = F_ * P_ * F_.transpose() + Q_; - } - - void Update(double measured_position) { - Eigen::Vector y; - y(0) = measured_position - H_ * x_; - - Eigen::Matrix S = H_ * P_ * H_.transpose() + R_; - Eigen::Matrix K = P_ * H_.transpose() * S.inverse(); - - x_ = x_ + K * y; - P_ = (Eigen::Matrix2d::Identity() - K * H_) * P_; - } - - double GetVelocity() { return x_(1); } - - private: - Eigen::Vector2d x_; - Eigen::Matrix2d P_; - Eigen::Matrix2d F_; - Eigen::Matrix2d Q_; - Eigen::Matrix H_; - Eigen::Matrix R_; - }; -} - -#endif //VELOCITY_ESTIMATOR_H diff --git a/cmvr-es/utils/math/src/qp_solver.cpp b/cmvr-es/utils/math/src/qp_solver.cpp deleted file mode 100644 index 2f14abd5..00000000 --- a/cmvr-es/utils/math/src/qp_solver.cpp +++ /dev/null @@ -1,287 +0,0 @@ -// -// Created by xtkuang on 2025/7/7. -// - -#include "../include/qp_solver.h" -#include "OsqpEigen/OsqpEigen.h" - -using namespace cmvr::math; - -QPSolverException::QPSolverException(int error_code) : error_code_(error_code), message_(GenerateMessage(error_code)) { -} - -const char *QPSolverException::what() const noexcept { - return message_.c_str(); -} - -int QPSolverException::code() const noexcept { - return error_code_; -} - -std::string QPSolverException::GenerateMessage(int code) { - static const std::unordered_map error_messages = { - {OSQP_DATA_VALIDATION_ERROR, "Data validation error"}, - {OSQP_SETTINGS_VALIDATION_ERROR, "Settings validation error"}, -#ifdef OSQP_EIGEN_OSQP_IS_V1 - {OSQP_ALGEBRA_LOAD_ERROR, "Linear system solver load error"}, -#else - {OSQP_LINSYS_SOLVER_LOAD_ERROR, "Linear system solver load error"}, -#endif - {OSQP_LINSYS_SOLVER_INIT_ERROR, "Linear system solver initialization error"}, - {OSQP_NONCVX_ERROR, "Non-convex error"}, - {OSQP_MEM_ALLOC_ERROR, "Memory allocation error"}, - {OSQP_WORKSPACE_NOT_INIT_ERROR, "Workspace not initialized"}, - - {QPSolverException::kStatusOffset + OSQP_SOLVED, "Solved"}, - {QPSolverException::kStatusOffset + OSQP_SOLVED_INACCURATE, "Solved inaccurate"}, - {QPSolverException::kStatusOffset + OSQP_PRIMAL_INFEASIBLE, "Primal infeasible"}, - {QPSolverException::kStatusOffset + OSQP_PRIMAL_INFEASIBLE_INACCURATE, "Primal infeasible inaccurate"}, - {QPSolverException::kStatusOffset + OSQP_DUAL_INFEASIBLE, "Dual infeasible"}, - {QPSolverException::kStatusOffset + OSQP_DUAL_INFEASIBLE_INACCURATE, "Dual infeasible inaccurate"}, - {QPSolverException::kStatusOffset + OSQP_MAX_ITER_REACHED, "Maximum iterations reached"}, - {QPSolverException::kStatusOffset + OSQP_TIME_LIMIT_REACHED, "Run time limit reached"}, - {QPSolverException::kStatusOffset + OSQP_NON_CVX, "Problem non convex"}, - {QPSolverException::kStatusOffset + OSQP_SIGINT, "Interrupted"}, - {QPSolverException::kStatusOffset + OSQP_UNSOLVED, "Unsolved"}, - - {-1, "General solver error or matrix update failure"}, - }; - - auto it = error_messages.find(code); - if (it != error_messages.end()) { - return it->second; - } - return "Unknown solver error (code: " + std::to_string(code) + ")"; -} - - -void QPSolverImpl::SetupImpl(int n_var, int n_const, double time_limit = 2e-3) { - n_var_ = n_var; - n_const_ = n_const; - - solver_.settings()->setWarmStart(true); - solver_.settings()->setVerbosity(false); - solver_.settings()->setTimeLimit(time_limit); - - solver_.data()->setNumberOfVariables(n_var); - solver_.data()->setNumberOfConstraints(n_const); - - primal_variable_for_warmstart_.resize(n_var_, 1); - primal_variable_for_warmstart_.setZero(); - - is_first_ = true; - - n_hessian_element_ = 0; -} - -void QPSolverImpl::InitFunctionImpl() { - A_cost_.resize(0, n_var_); - b_cost_.resize(0, 1); - - A_const_.resize(0, n_var_); - lb_.resize(0, 1); - ub_.resize(0, 1); - - A_cost_.setZero(); - b_cost_.setZero(); - - A_const_.setZero(); - lb_.setZero(); - ub_.setZero(); -} - -void QPSolverImpl::AddCostFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &b) { - if (A.rows() != b.rows()) { - std::cerr << "OSQP Solver Add cost function failed: Size issue Ax = b (A.rows(): " << A.rows() - << ", b.rows(): " << b.rows() << ")" << std::endl; - return; - } - - if (b.cols() != 1) { - std::cerr << "OSQP Solver Add cost function failed: Size issue: b (Nx1)" << std::endl; - return; - } - - int n_rows_old = (int) A_cost_.rows(); - int n_rows_add = (int) A.rows(); - - if (n_rows_old == -1) { - A_cost_.resize(n_rows_add, Eigen::NoChange); - b_cost_.resize(n_rows_add, Eigen::NoChange); - A_cost_.bottomRows(n_rows_add) = A; - b_cost_.bottomRows(n_rows_add) = b; - } else { - A_cost_.conservativeResize(n_rows_old + n_rows_add, Eigen::NoChange); - b_cost_.conservativeResize(n_rows_old + n_rows_add, Eigen::NoChange); - A_cost_.bottomRows(n_rows_add) = A; - b_cost_.bottomRows(n_rows_add) = b; - } -} - -void QPSolverImpl::SetCostFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &b) { - // Ax = b - - hessian_ = (A.transpose() * A).sparseView(); - gradient_ = -b.transpose() * A; -} - -void QPSolverImpl::SetConstraintsFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &lb, - const Eigen::VectorXd &ub) { - // lb <= Ax <= ub - - linearMatrix_ = A.sparseView(); - lowerBound_ = lb; - upperBound_ = ub; -} - -void QPSolverImpl::SetPrimalVariableImpl(const Eigen::VectorXd &pv) { primal_variable_for_warmstart_ = pv; } - -void QPSolverImpl::ResetIsFirstImpl() { is_first_ = true; } - -Eigen::VectorXd QPSolverImpl::SolveImpl() { - Eigen::MatrixXd ret; - - bool need_init = false; - - if (!is_first_) { - if (n_hessian_element_ == hessian_.nonZeros()) { - if (!solver_.updateHessianMatrix(hessian_)) { - need_init = true; - } - if (!solver_.updateGradient(gradient_)) { - need_init = true; - } - if (!solver_.updateLinearConstraintsMatrix(linearMatrix_)) { - need_init = true; - } - if (!solver_.updateBounds(lowerBound_, upperBound_)) { - need_init = true; - } - } else { - need_init = true; - } - } else { - need_init = true; - } - - if (need_init) { - solver_.data()->clearHessianMatrix(); - solver_.data()->clearLinearConstraintsMatrix(); - solver_.clearSolver(); - - n_hessian_element_ = (int) hessian_.nonZeros(); - - if (!solver_.data()->setHessianMatrix(hessian_)) { - throw QPSolverException(-1); - } - if (!solver_.data()->setGradient(gradient_)) { - throw QPSolverException(-1); - } - if (!solver_.data()->setLinearConstraintsMatrix(linearMatrix_)) { - throw QPSolverException(-1); - } - if (!solver_.data()->setLowerBound(lowerBound_)) { - throw QPSolverException(-1); - } - if (!solver_.data()->setUpperBound(upperBound_)) { - throw QPSolverException(-1); - } - - if (!solver_.initSolver()) { - throw QPSolverException(-1); - } - - is_first_ = false; - } - - solver_.setPrimalVariable(primal_variable_for_warmstart_); - - const auto result = solver_.solveProblem(); - if (result != OsqpEigen::ErrorExitFlag::NoError) { - throw QPSolverException(static_cast(result)); - } - if (solver_.getStatus() != OsqpEigen::Status::Solved) { - throw QPSolverException((int) QPSolverException::kStatusOffset + static_cast(solver_.getStatus())); - } - - return solver_.getSolution(); -} - -Eigen::MatrixXd QPSolverImpl::GetACostImpl() const { // NOLINT - return A_cost_; -} - -Eigen::VectorXd QPSolverImpl::GetBCostImpl() const { // NOLINT - return b_cost_; -} - -Eigen::MatrixXd QPSolverImpl::GetAConstImpl() const { // NOLINT - return A_const_; -} - -Eigen::VectorXd QPSolverImpl::GetLowerBoundImpl() const { // NOLINT - return lb_; -} - -Eigen::VectorXd QPSolverImpl::GetUpperBoundImpl() const { // NOLINT - return ub_; -} - - -QPSolver::QPSolver() { - impl_ = std::make_unique(); -} - -QPSolver::~QPSolver() = default; - -void QPSolver::Setup(int n_var, int n_const, double time_limit) { - impl_->SetupImpl(n_var, n_const, time_limit); -} - -void QPSolver::InitFunction() { - impl_->InitFunctionImpl(); -} - -void QPSolver::AddCostFunction(const Eigen::MatrixXd &A, const Eigen::VectorXd &b) { - impl_->AddCostFunctionImpl(A, b); -} - -void QPSolver::SetCostFunction(const Eigen::MatrixXd &A, const Eigen::VectorXd &b) { - impl_->SetCostFunctionImpl(A, b); -} - -void QPSolver::SetConstraintsFunction(const Eigen::MatrixXd &A, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub) { - impl_->SetConstraintsFunctionImpl(A, lb, ub); -} - -void QPSolver::SetPrimalVariable(const Eigen::VectorXd &pv) { - impl_->SetPrimalVariableImpl(pv); -} - -void QPSolver::ResetIsFirst() { - impl_->ResetIsFirstImpl(); -} - -Eigen::VectorXd QPSolver::Solve() { - return impl_->SolveImpl(); -} - -Eigen::MatrixXd QPSolver::GetACost() const { - return impl_->GetACostImpl(); -} - -Eigen::VectorXd QPSolver::GetBCost() const { - return impl_->GetBCostImpl(); -} - -Eigen::MatrixXd QPSolver::GetAConst() const { - return impl_->GetACostImpl(); -} - -Eigen::VectorXd QPSolver::GetLowerBound() const { - return impl_->GetLowerBoundImpl(); -} - -Eigen::VectorXd QPSolver::GetUpperBound() const { - return impl_->GetUpperBoundImpl(); -} diff --git a/cmvr-es/utils/math/src/se2.cpp b/cmvr-es/utils/math/src/se2.cpp deleted file mode 100644 index b8b535ac..00000000 --- a/cmvr-es/utils/math/src/se2.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// -// Created by xtkuang on 2025/7/7. -// - -#include "../include/se2.h" - -using namespace cmvr::math; - -SE2::MatrixType SE2::Identity() { - return MatrixType::Identity(); -} - -SE2::MatrixType SE2::T(double angle, const Eigen::Vector& translation) { - double s, c; - fsincos(angle, s, c); - - MatrixType T; - T(0, 0) = c; - T(0, 1) = -s; - T(1, 0) = s; - T(1, 1) = c; - T(0, 2) = translation(0); - T(1, 2) = translation(1); - T(2, 0) = 0; - T(2, 1) = 0; - T(2, 2) = 1; - - return T; -} - -SE2::MatrixType SE2::Exp(const se2v::MatrixType& s, double angle) { - double a = s[0] * angle; - Eigen::Vector t = s.tail<2>() * angle; - return T(a, t); -} - -se2v::MatrixType SE2::Log(const MatrixType& T) { - se2v::MatrixType S; - S(0) = std::atan2(T(1, 0), T(1, 1)); - S.tail<2>() = T.block<2, 1>(0, 2); - return S; -} diff --git a/cmvr-es/utils/math/src/se3.cpp b/cmvr-es/utils/math/src/se3.cpp deleted file mode 100644 index 6dcc7978..00000000 --- a/cmvr-es/utils/math/src/se3.cpp +++ /dev/null @@ -1,422 +0,0 @@ -// -// Created by xtkuang on 2025/7/7. -// - -#include "../include/se3.h" - - -using namespace cmvr::math; - -SE3::MatrixType SE3::Identity() { - return MatrixType::Identity(); -} - -SE3::MatrixType SE3::T(const Eigen::Vector3d &p) { - MatrixType T = Identity(); - T.block<3, 1>(0, 3) = p; - return T; -} - -SE3::MatrixType SE3::T(const SO3::MatrixType &R, const Eigen::Vector3d &p) { - MatrixType T; - T.block<3, 3>(0, 0) = R; - T.block<3, 1>(0, 3) = p; - T.block<1, 4>(3, 0) = kAffineConstant; - return T; -} - -SE3::MatrixType SE3::Inverse(const MatrixType &T) { - MatrixType invT; - invT.block<3, 3>(0, 0) = SO3::Inverse(GetRotation(T)); - invT.block<3, 1>(0, 3) = -invT.block<3, 3>(0, 0) * GetPosition(T); - invT.block<1, 4>(3, 0) = kAffineConstant; - return invT; -} - -SO3::MatrixType SE3::GetRotation(const MatrixType &T) { - return T.block<3, 3>(0, 0); -} - -Eigen::Vector3d SE3::GetPosition(const MatrixType &T) { - return T.block<3, 1>(0, 3); -} - -SE3::MatrixType SE3::Exp(const se3v::MatrixType &S, double angle) { - return Exp(S.head<3>(), S.tail<3>(), angle); -} - -SE3::MatrixType SE3::Exp(so3v::MatrixType w, Eigen::Vector3d v, double angle) { - w *= angle; - v *= angle; - - using std::sqrt; - - double sq0 = w(0) * w(0), sq1 = w(1) * w(1), sq2 = w(2) * w(2); - double theta = sqrt(sq0 + sq1 + sq2); - double st_t, ct_t, vt_t; - - if (theta < kDoubleEpsilon) { - st_t = 1.0 - theta * theta / 6.0; - ct_t = 0.5 - theta * theta / 24.0; - vt_t = (w(0) * v(0) + w(1) * v(1) + w(2) * v(2)) * (1.0 - theta * theta / 20.0) / 6.0; - } else { - double s, c; - fsincos(theta, s, c); - double itheta = 1.0 / theta; - st_t = s * itheta; - itheta *= itheta; - ct_t = (1.0 - c) * itheta; - vt_t = (w(0) * v(0) + w(1) * v(1) + w(2) * v(2)) * (1.0 - st_t) * itheta; - } - - MatrixType T; - - T(0, 0) = 1.0 - ct_t * (sq1 + sq2); - T(0, 1) = ct_t * w(0) * w(1) - st_t * w(2); - T(0, 2) = ct_t * w(0) * w(2) + st_t * w(1); - T(1, 0) = ct_t * w(0) * w(1) + st_t * w(2); - T(1, 1) = 1.0 - ct_t * (sq0 + sq2); - T(1, 2) = ct_t * w(1) * w(2) - st_t * w(0); - T(2, 0) = ct_t * w(0) * w(2) - st_t * w(1); - T(2, 1) = ct_t * w(1) * w(2) + st_t * w(0); - T(2, 2) = 1.0 - ct_t * (sq0 + sq1); - T(0, 3) = st_t * v(0) + vt_t * w(0) + ct_t * (w(1) * v(2) - w(2) * v(1)); - T(1, 3) = st_t * v(1) + vt_t * w(1) + ct_t * (w(2) * v(0) - w(0) * v(2)); - T(2, 3) = st_t * v(2) + vt_t * w(2) + ct_t * (w(0) * v(1) - w(1) * v(0)); - T.block<1, 4>(3, 0) = kAffineConstant; - - return T; -} - -se3v::MatrixType SE3::Log(const MatrixType &T) { - using std::cos; - using std::sin; - - se3v::MatrixType S; - - double theta = 0.5 * (T(0, 0) + T(1, 1) + T(2, 2) - 1.0), t_st; - - if (theta < kDoubleEpsilon - 1.0) { - double w[3]; - if (T(0, 0) > 1.0 - kDoubleEpsilon) { - w[0] = kPi; - w[1] = w[2] = 0.0; - } else if (T(1, 1) > 1.0 - kDoubleEpsilon) { - w[1] = kPi; - w[0] = w[2] = 0.0; - } else if (T(2, 2) > 1.0 - kDoubleEpsilon) { - w[2] = kPi; - w[0] = w[1] = 0.0; - } else { - w[0] = kPiDividedBySqrt2 * sqrt((T(1, 0) * T(1, 0) + T(2, 0) * T(2, 0)) / (1.0 - T(0, 0))); - w[1] = kPiDividedBySqrt2 * sqrt((T(0, 1) * T(0, 1) + T(2, 1) * T(2, 1)) / (1.0 - T(1, 1))); - w[2] = kPiDividedBySqrt2 * sqrt((T(0, 2) * T(0, 2) + T(1, 2) * T(1, 2)) / (1.0 - T(2, 2))); - } - double w2[] = {w[0] * w[0], w[1] * w[1], w[2] * w[2]}, w3[] = {w2[0] * w[0], w2[1] * w[1], w2[2] * w[2]}, - v[] = {w[1] * w[2], w[2] * w[0], w[0] * w[1]}; - double id = 0.25 * kPiSquare / - (w2[0] * w2[0] + w2[1] * w2[1] + w2[2] * w2[2] + 2.0 * ( - w2[0] * w2[1] + w2[1] * w2[2] + w2[2] * w2[0])); - double p[3] = {T(0, 3) * id, T(1, 3) * id, T(2, 3) * id}; - - S(0) = w[0]; - S(1) = w[1]; - S(2) = w[2]; - S(3) = 2.0 * (2.0 * w2[0] * p[0] + (w3[2] + v[1] * w[0] + v[0] * w[1] + 2.0 * v[2]) * p[1] + - (2.0 * v[1] - w3[1] - w[0] * v[2] - w[2] * v[0]) * p[2]); - S(4) = 2.0 * ((2.0 * v[2] - w3[2] - v[0] * w[1] - v[1] * w[0]) * p[0] + 2.0 * w2[1] * p[1] + - (w3[0] + w[2] * v[1] + v[2] * w[1] + 2.0 * v[0]) * p[2]); - S(5) = 2.0 * ((w[0] * v[2] + w[2] * v[0] + 2.0 * v[1] + w3[1]) * p[0] + - (2.0 * v[0] - w3[0] - v[2] * w[1] - w[2] * v[1]) * p[1] + 2.0 * w2[2] * p[2]); - } else { - if (theta > 1.0) { - theta = 1.0; - } else if (theta < -1.0) { - theta = -1.0; - } - - theta = acos(theta); - if (theta < kDoubleEpsilon) - t_st = 3.0 / (6.0 - theta * theta); - else - t_st = theta / (2.0 * sin(theta)); - double stct, st = sin(theta), w[] = {T(2, 1) - T(1, 2), T(0, 2) - T(2, 0), T(1, 0) - T(0, 1)}, - w2[] = {w[0] * w[0], w[1] * w[1], w[2] * w[2]}, w3[] = {w[1] * w[2], w[2] * w[0], w[0] * w[1]}; - w[0] = t_st * w[0]; - w[1] = t_st * w[1]; - w[2] = t_st * w[2]; - - if (theta < kDoubleEpsilon) - stct = theta / 48.0; - else - stct = (2.0 * st - theta * (1.0 + cos(theta))) / (8.0 * st * st * st); - - S(0) = w[0]; - S(1) = w[1]; - S(2) = w[2]; - S(3) = (1.0 - stct * (w2[1] + w2[2])) * T(0, 3) + (0.5 * w[2] + stct * w3[2]) * T(1, 3) + - (stct * w3[1] - 0.5 * w[1]) * T(2, 3); - S(4) = (stct * w3[2] - 0.5 * w[2]) * T(0, 3) + (1.0 - stct * (w2[0] + w2[2])) * T(1, 3) + - (0.5 * w[0] + stct * w3[0]) * T(2, 3); - S(5) = (0.5 * w[1] + stct * w3[1]) * T(0, 3) + (stct * w3[0] - 0.5 * w[0]) * T(1, 3) + - (1.0 - stct * (w2[1] + w2[0])) * T(2, 3); - } - - return S; -} - -Eigen::Matrix SE3::Ad(const MatrixType &T) { - Eigen::Matrix m; - - m(0, 0) = T(0, 0); - m(0, 1) = T(0, 1); - m(0, 2) = T(0, 2); - m(0, 3) = 0; - m(0, 4) = 0; - m(0, 5) = 0; - - m(1, 0) = T(1, 0); - m(1, 1) = T(1, 1); - m(1, 2) = T(1, 2); - m(1, 3) = 0; - m(1, 4) = 0; - m(1, 5) = 0; - - m(2, 0) = T(2, 0); - m(2, 1) = T(2, 1); - m(2, 2) = T(2, 2); - m(2, 3) = 0; - m(2, 4) = 0; - m(2, 5) = 0; - - m(3, 0) = -T(2, 3) * T(1, 0) + T(1, 3) * T(2, 0); - m(3, 1) = -T(2, 3) * T(1, 1) + T(1, 3) * T(2, 1); - m(3, 2) = -T(2, 3) * T(1, 2) + T(1, 3) * T(2, 2); - m(3, 3) = T(0, 0); - m(3, 4) = T(0, 1); - m(3, 5) = T(0, 2); - - m(4, 0) = T(2, 3) * T(0, 0) - T(0, 3) * T(2, 0); - m(4, 1) = T(2, 3) * T(0, 1) - T(0, 3) * T(2, 1); - m(4, 2) = T(2, 3) * T(0, 2) - T(0, 3) * T(2, 2); - m(4, 3) = T(1, 0); - m(4, 4) = T(1, 1); - m(4, 5) = T(1, 2); - - m(5, 0) = -T(1, 3) * T(0, 0) + T(0, 3) * T(1, 0); - m(5, 1) = -T(1, 3) * T(0, 1) + T(0, 3) * T(1, 1); - m(5, 2) = -T(1, 3) * T(0, 2) + T(0, 3) * T(1, 2); - m(5, 3) = T(2, 0); - m(5, 4) = T(2, 1); - m(5, 5) = T(2, 2); - - return m; -} - -se3v::MatrixType SE3::Ad(const MatrixType &T, const se3v::MatrixType &S) { - double Rw1, Rw2, Rw3; - Rw1 = T(0, 0) * S(0) + T(0, 1) * S(1) + T(0, 2) * S(2); - Rw2 = T(1, 0) * S(0) + T(1, 1) * S(1) + T(1, 2) * S(2); - Rw3 = T(2, 0) * S(0) + T(2, 1) * S(1) + T(2, 2) * S(2); - se3v::MatrixType AdT_S; - AdT_S(0) = Rw1; - AdT_S(1) = Rw2; - AdT_S(2) = Rw3; - AdT_S(3) = -T(2, 3) * Rw2 + T(1, 3) * Rw3 + T(0, 0) * S(3) + T(0, 1) * S(4) + T(0, 2) * S(5); - AdT_S(4) = T(2, 3) * Rw1 - T(0, 3) * Rw3 + T(1, 0) * S(3) + T(1, 1) * S(4) + T(1, 2) * S(5); - AdT_S(5) = -T(1, 3) * Rw1 + T(0, 3) * Rw2 + T(2, 0) * S(3) + T(2, 1) * S(4) + T(2, 2) * S(5); - return AdT_S; -} - -Eigen::Matrix SE3::InvAd(const MatrixType &T) { - typename Eigen::Matrix m; - - m(0, 0) = T(0, 0); - m(0, 1) = T(1, 0); - m(0, 2) = T(2, 0); - m(0, 3) = 0; - m(0, 4) = 0; - m(0, 5) = 0; - - m(1, 0) = T(0, 1); - m(1, 1) = T(1, 1); - m(1, 2) = T(2, 1); - m(1, 3) = 0; - m(1, 4) = 0; - m(1, 5) = 0; - - m(2, 0) = T(0, 2); - m(2, 1) = T(1, 2); - m(2, 2) = T(2, 2); - m(2, 3) = 0; - m(2, 4) = 0; - m(2, 5) = 0; - - m(3, 0) = -T(1, 0) * T(2, 3) + T(2, 0) * T(1, 3); - m(3, 1) = T(0, 0) * T(2, 3) - T(2, 0) * T(0, 3); - m(3, 2) = -T(0, 0) * T(1, 3) + T(1, 0) * T(0, 3); - m(3, 3) = T(0, 0); - m(3, 4) = T(1, 0); - m(3, 5) = T(2, 0); - - m(4, 0) = -T(1, 1) * T(2, 3) + T(2, 1) * T(1, 3); - m(4, 1) = T(0, 1) * T(2, 3) - T(2, 1) * T(0, 3); - m(4, 2) = -T(0, 1) * T(1, 3) + T(1, 1) * T(0, 3); - m(4, 3) = T(0, 1); - m(4, 4) = T(1, 1); - m(4, 5) = T(2, 1); - - m(5, 0) = -T(1, 2) * T(2, 3) + T(2, 2) * T(1, 3); - m(5, 1) = T(0, 2) * T(2, 3) - T(2, 2) * T(0, 3); - m(5, 2) = -T(0, 2) * T(1, 3) + T(1, 2) * T(0, 3); - m(5, 3) = T(0, 2); - m(5, 4) = T(1, 2); - m(5, 5) = T(2, 2); - - return m; -} - -se3v::MatrixType SE3::InvAd(const MatrixType &T, const se3v::MatrixType &S) { - return Ad(T.inverse(), S); -} - -Eigen::Matrix SE3::ad(const se3v::MatrixType &S) { - Eigen::Matrix m; - - m(0, 0) = 0; - m(0, 1) = -S(2); - m(0, 2) = S(1); - m(0, 3) = 0; - m(0, 4) = 0; - m(0, 5) = 0; - - m(1, 0) = S(2); - m(1, 1) = 0; - m(1, 2) = -S(0); - m(1, 3) = 0; - m(1, 4) = 0; - m(1, 5) = 0; - - m(2, 0) = -S(1); - m(2, 1) = S(0); - m(2, 2) = 0; - m(2, 3) = 0; - m(2, 4) = 0; - m(2, 5) = 0; - - m(3, 0) = 0; - m(3, 1) = -S(5); - m(3, 2) = S(4); - m(3, 3) = 0; - m(3, 4) = -S(2); - m(3, 5) = S(1); - - m(4, 0) = S(5); - m(4, 1) = 0; - m(4, 2) = -S(3); - m(4, 3) = S(2); - m(4, 4) = 0; - m(4, 5) = -S(0); - - m(5, 0) = -S(4); - m(5, 1) = S(3); - m(5, 2) = 0; - m(5, 3) = -S(1); - m(5, 4) = S(0); - m(5, 5) = 0; - - return m; -} - -Eigen::Matrix SE3::adTranspose(const se3v::MatrixType &S) { - Eigen::Matrix m; - - m(0, 0) = 0; - m(0, 1) = S(2); - m(0, 2) = -S(1); - m(0, 3) = 0; - m(0, 4) = S(5); - m(0, 5) = -S(4); - - m(1, 0) = -S(2); - m(1, 1) = 0; - m(1, 2) = S(0); - m(1, 3) = -S(5); - m(1, 4) = 0; - m(1, 5) = S(3); - - m(2, 0) = S(1); - m(2, 1) = -S(0); - m(2, 2) = 0; - m(2, 3) = S(4); - m(2, 4) = -S(3); - m(2, 5) = 0; - - m(3, 0) = 0; - m(3, 1) = 0; - m(3, 2) = 0; - m(3, 3) = 0; - m(3, 4) = S(2); - m(3, 5) = -S(1); - - m(4, 0) = 0; - m(4, 1) = 0; - m(4, 2) = 0; - m(4, 3) = -S(2); - m(4, 4) = 0; - m(4, 5) = S(0); - - m(5, 0) = 0; - m(5, 1) = 0; - m(5, 2) = 0; - m(5, 3) = S(1); - m(5, 4) = -S(0); - m(5, 5) = 0; - - return m; -} - -se3v::MatrixType SE3::ad(const se3v::MatrixType &S1, const se3v::MatrixType &S2) { - se3v::MatrixType S; - - S(0) = -S1(2) * S2(1) + S1(1) * S2(2); - S(1) = S1(2) * S2(0) - S1(0) * S2(2); - S(2) = -S1(1) * S2(0) + S1(0) * S2(1); - S(3) = -S1(5) * S2(1) + S1(4) * S2(2) - S1(2) * S2(4) + S1(1) * S2(5); - S(4) = S1(5) * S2(0) - S1(3) * S2(2) + S1(2) * S2(3) - S1(0) * S2(5); - S(5) = -S1(4) * S2(0) + S1(3) * S2(1) - S1(1) * S2(3) + S1(0) * S2(4); - - return S; -} - -se3v::MatrixType SE3::adTranspose(const se3v::MatrixType &S1, const se3v::MatrixType &S2) { - se3v::MatrixType S; - - S(0) = S1(2) * S2(1) - S1(1) * S2(2) + S1(5) * S2(4) - S1(4) * S2(5); - S(1) = -S1(2) * S2(0) + S1(0) * S2(2) - S1(5) * S2(3) + S1(3) * S2(5); - S(2) = S1(1) * S2(0) - S1(0) * S2(1) + S1(4) * S2(3) - S1(3) * S2(4); - S(3) = S1(2) * S2(4) - S1(1) * S2(5); - S(4) = -S1(2) * S2(3) + S1(0) * S2(5); - S(5) = S1(1) * S2(3) - S1(0) * S2(4); - - return S; -} - -Eigen::Vector3d SE3::Multiply(const MatrixType &T, const Eigen::Vector3d &p) { - return T.block<3, 3>(0, 0) * p + T.block<3, 1>(0, 3); -} - -se3v::MatrixType SE3::Vec(const se3::MatrixType &s) { - se3v::MatrixType v; - v.head<3>() = SO3::Vec(s.block<3, 3>(0, 0)); - v.tail<3>() = s.block<3, 1>(0, 3); - return v; -} - -se3::MatrixType SE3::Hat(const se3v::MatrixType &v) { - se3::MatrixType s; - s.setZero(); - s.block<3, 3>(0, 0) = SO3::Hat(v.head<3>()); - s.block<3, 1>(0, 3) = v.tail<3>(); - return s; -} diff --git a/cmvr-es/utils/math/src/so3.cpp b/cmvr-es/utils/math/src/so3.cpp deleted file mode 100644 index ea0bdd0d..00000000 --- a/cmvr-es/utils/math/src/so3.cpp +++ /dev/null @@ -1,184 +0,0 @@ -// -// Created by xtkuang on 2025/7/7. -// - -#include "../include/so3.h" - -using namespace cmvr::math; - -SO3::MatrixType SO3::Identity() { - return MatrixType::Identity(); -} - -SO3::MatrixType SO3::Inverse(const MatrixType &R) { - return R.transpose(); -} - -SO3::MatrixType SO3::Exp(so3v::MatrixType w, double angle) { - w *= angle; - - using std::cos; - using std::sin; - using std::sqrt; - - double sq0 = w(0) * w(0), sq1 = w(1) * w(1), sq2 = w(2) * w(2); - double theta = sqrt(sq0 + sq1 + sq2); - double st_t, ct_t; - - if (theta < kDoubleEpsilon) { - st_t = 1.0 - theta * theta / 6.0; - ct_t = 0.5 - theta * theta / 24.0; - } else { - double s, c; - fsincos(theta, s, c); - double itheta = 1.0 / theta; - st_t = s * itheta; - itheta *= itheta; - ct_t = (1.0 - c) * itheta; - } - - MatrixType m; - - m(0, 0) = 1.0 - ct_t * (sq1 + sq2); - m(0, 1) = ct_t * w(0) * w(1) - st_t * w(2); - m(0, 2) = ct_t * w(0) * w(2) + st_t * w(1); - m(1, 0) = ct_t * w(0) * w(1) + st_t * w(2); - m(1, 1) = 1.0 - ct_t * (sq0 + sq2); - m(1, 2) = ct_t * w(1) * w(2) - st_t * w(0); - m(2, 0) = ct_t * w(0) * w(2) - st_t * w(1); - m(2, 1) = ct_t * w(1) * w(2) + st_t * w(0); - m(2, 2) = 1.0 - ct_t * (sq0 + sq1); - - return m; -} - -so3v::MatrixType SO3::Log(const MatrixType &R) { - using std::acos; - using std::sin; - using std::sqrt; - - double theta = 0.5 * (R(0, 0) + R(1, 1) + R(2, 2) - 1.0), t_st; - - if (theta < kDoubleEpsilon - 1.0) { - if (R(0, 0) > 1.0 - kDoubleEpsilon) - return {kPi, 0.0, 0.0}; - else if (R(1, 1) > 1.0 - kDoubleEpsilon) - return {0.0, kPi, 0.0}; - else if (R(2, 2) > 1.0 - kDoubleEpsilon) - return {0.0, 0.0, kPi}; - - return { - (kPiDividedBySqrt2 * sqrt((R(1, 0) * R(1, 0) + R(2, 0) * R(2, 0)) / (1.0 - R(0, 0)))), - (kPiDividedBySqrt2 * sqrt((R(0, 1) * R(0, 1) + R(2, 1) * R(2, 1)) / (1.0 - R(1, 1)))), - (kPiDividedBySqrt2 * sqrt((R(0, 2) * R(0, 2) + R(1, 2) * R(1, 2)) / (1.0 - R(2, 2)))) - }; - } - theta = acos(theta); - if (theta < kDoubleEpsilon) - t_st = 3.0 / (6.0 - theta * theta); - else - t_st = theta / (2.0 * sin(theta)); - return {t_st * (R(2, 1) - R(1, 2)), t_st * (R(0, 2) - R(2, 0)), t_st * (R(1, 0) - R(0, 1))}; -} - -SO3::MatrixType SO3::RotX(double angle) { - return Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX()).toRotationMatrix(); -} - -SO3::MatrixType SO3::RotY(double angle) { - return Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitY()).toRotationMatrix(); -} - -SO3::MatrixType SO3::RotZ(double angle) { - return Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()).toRotationMatrix(); -} - -SO3::MatrixType SO3::FromEulerAngle(const Eigen::Vector3d &angles, EulerAngleType type) { - switch (type) { - case EulerAngleType::ZYX: - return RotZ(angles[0]) * RotY(angles[1]) * RotX(angles[2]); - case EulerAngleType::ZYZ: - return RotZ(angles[0]) * RotY(angles[1]) * RotZ(angles[2]); - } - assert(false); - return Identity(); -} - -Eigen::Vector3d SO3::ToEulerAngle(const SO3::MatrixType &R, EulerAngleType type) { - using std::atan2; - - switch (type) { - case EulerAngleType::ZYX: - return { - atan2(R(1, 0), R(0, 0)), atan2(-R(2, 0), sqrt(R(0, 0) * R(0, 0) + R(1, 0) * R(1, 0))), - atan2(R(2, 1), R(2, 2)) - }; - case EulerAngleType::ZYZ: - return { - atan2(R(1, 2), R(0, 2)), atan2(sqrt(R(0, 2) * R(0, 2) + R(1, 2) * R(1, 2)), R(2, 2)), - atan2(R(2, 1), -R(2, 0)) - }; - } - assert(false); - return Eigen::Vector3d::Zero(); -} - -SO3::MatrixType SO3::FromQuaternion(const Eigen::Quaterniond &q) { - return q.toRotationMatrix(); -} - -Eigen::Quaterniond SO3::ToQuaternion(const SO3::MatrixType &R) { - return Eigen::Quaterniond(R); -} - -SO3::MatrixType SO3::FromRPY(const Eigen::Vector3d &angles) { - double r = angles(0), p = angles(1), y = angles(2); - return FromEulerAngle({y, p, r}, EulerAngleType::ZYX); -} - -Eigen::Vector3d SO3::ToRPY(const MatrixType &R) { - const auto angles = ToEulerAngle(R, EulerAngleType::ZYX); - return {angles[2], angles[1], angles[0]}; -} - -SO3::MatrixType SO3::Projection(const MatrixType &m) { - Eigen::JacobiSVD svd(m, Eigen::ComputeFullU | Eigen::ComputeFullV); - if (m.determinant() > 0) { - return svd.matrixV() * svd.matrixU().transpose(); - } - return svd.matrixV() * Eigen::DiagonalMatrix(1, 1, -1) * svd.matrixU().transpose(); -} - -Eigen::Vector3d SO3::GetX(const MatrixType &R) { - return R.template block<3, 1>(0, 0); -} - -Eigen::Vector3d SO3::GetY(const MatrixType &R) { - return R.template block<3, 1>(0, 1); -} - -Eigen::Vector3d SO3::GetZ(const MatrixType &R) { - return R.template block<3, 1>(0, 2); -} - -so3v::MatrixType SO3::Vec(const so3::MatrixType &r) { - so3v::MatrixType w; - w(0) = -r(1, 2); - w(1) = r(0, 2); - w(2) = -r(0, 1); - return w; -} - -so3::MatrixType SO3::Hat(const so3v::MatrixType &w) { - so3::MatrixType m; - m(0, 0) = 0; - m(0, 1) = -w(2); - m(0, 2) = w(1); - m(1, 0) = w(2); - m(1, 1) = 0; - m(1, 2) = -w(0); - m(2, 0) = -w(1); - m(2, 1) = w(0); - m(2, 2) = 0; - return m; -} diff --git a/data/ik_movel_scurve.csv b/data/ik_movel_scurve.csv index 61bd7747..6f474ed4 100644 --- a/data/ik_movel_scurve.csv +++ b/data/ik_movel_scurve.csv @@ -1,547 +1,547 @@ t,q1,q2,q3,q4,q5,q6,q7 0,0.25,1,1.5707963267949,1.5707963267949,-1.5707963267949,0,0 -0.002,0.25,1,1.5707963267949,1.5707963267949,-1.5707963267949,0,0 -0.004,0.25000047042195,0.999999643059012,1.57079655522309,1.57079724068573,-1.57079596985391,-1.30973721143836e-06,4.82598248142038e-07 -0.006,0.250002352119835,0.999998215295479,1.57079746893306,1.57080089623876,-1.57079454209152,-6.54868491622439e-06,2.41299125801426e-06 -0.008,0.250006586030861,0.999995002831306,1.57079952475507,1.57080912114056,-1.57079132964004,-1.83363069831887e-05,6.75637568596772e-06 -0.01,0.250014113346931,0.999989291798997,1.57080317944813,1.57082374281808,-1.57078561867083,-3.92920384698955e-05,1.44779486252094e-05 -0.012,0.25002587578704,0.999980368352981,1.57080888962368,1.57084658816104,-1.570776695436,-7.20352548050359e-05,2.65429080714911e-05 -0.014,0.250042815970618,0.999967518685138,1.57081711164106,1.57087948314138,-1.57076384632562,-0.000119185229528684,4.39164535541204e-05 -0.016,0.250065877891993,0.999950029044509,1.57082830147452,1.57092425232965,-1.5707463579404,-0.000183361080548634,6.75637869166764e-05 -0.018,0.250096007496088,0.999927185761162,1.57084291455164,1.57098271830821,-1.5707235171799,-0.000267181704862094,9.84501132239856e-05 -0.02,0.250134153355467,0.999898275274187,1.570861405563,1.57105670098111,-1.57069461134612,-0.000373265701722758,0.000137540641784734 -0.022,0.250181267448461,0.999862584163762,1.57088422824305,1.57114801678116,-1.57065892826264,-0.00050423128435177,0.00018580058735287 -0.024,0.250238306037392,0.999819399187298,1.57091183512237,1.57125847777549,-1.57061575640899,-0.000662696180567743,0.000244195171776534 -0.026,0.250306230644415,0.999768007319777,1.57094467725243,1.5713898906728,-1.57056438507031,-0.000851277523241454,0.000313689626812957 -0.028,0.250386009127344,0.999707695798416,1.57098320390151,1.57154405572967,-1.5705041045026,-0.0010725917297323,0.000395249197883043 -0.03,0.250478616872607,0.999637752170651,1.57102786221273,1.57172276553403,-1.57043420611487,-0.00132925436411946,0.000489839144531641 -0.032,0.250585038074371,0.999557464346465,1.57107909683867,1.57192780370559,-1.57035398266535,-0.00162387999348339,0.000598424744928135 -0.034,0.250706267118671,0.999466120654551,1.57113734954332,1.57216094348961,-1.57026272847384,-0.00195908203141728,0.00072197130036424 -0.036,0.250843310069524,0.999363009902274,1.57120305877169,1.57242394624786,-1.5701597396497,-0.00233747256989253,0.000861444140564823 -0.038,0.25099718625673,0.999247421439378,1.57127665918684,1.57271855984787,-1.57004431433571,-0.00276166219960709,0.0010178086300883 -0.04,0.251168929965533,0.999118645225347,1.57135858117314,1.57304651695034,-1.56991575296774,-0.00323425981877845,0.0011920301760154 -0.042,0.251359592228325,0.998975971900352,1.57144925030474,1.57340953319513,-1.5697733585504,-0.00375787243033101,0.00138507423714958 -0.044,0.251570242718595,0.998818692859658,1.5715490867781,1.57380930528595,-1.56961643694885,-0.00433510492741253,0.00159790633497738 -0.046,0.251801971747353,0.998646100331404,1.57165850480715,1.57424750897407,-1.56944429719675,-0.0049685598671558,0.00183149206666271 -0.048,0.252055499984317,0.998457778919316,1.57177772745776,1.57472505784035,-1.56925654240594,-0.00565976745880769,0.00208640259738799 -0.05,0.252330161960637,0.998254355835669,1.57190631321648,1.57524020644808,-1.56905381705993,-0.00640643167306516,0.00236179878779658 -0.052,0.252625171204899,0.998036551188815,1.57204376048817,1.57579097026807,-1.56883685873587,-0.00720591555188081,0.00265671598977199 -0.054,0.252939742373786,0.997805085101871,1.57218956708213,1.57637536373045,-1.56860640519761,-0.00805558200416776,0.00297018961259649 -0.056,0.253273089233422,0.997560677628197,1.57234323076536,1.57699140228548,-1.56836319408827,-0.00895279403244094,0.0033012551216505 -0.058,0.253624422740584,0.997304048671724,1.57250424979362,1.57763710435995,-1.56810796263798,-0.00989491494798889,0.00364894803594106 -0.06,0.253992949223688,0.997035917912167,1.57267212342084,1.5783104932092,-1.56784144738653,-0.0108793085746444,0.00401230392437078 -0.062,0.254377868663495,0.996757004735117,1.57284635238723,1.57900959866477,-1.56756438392099,-0.0119033394412278,0.00439035840069514 -0.064,0.25477837307352,0.996468028167002,1.5730264393861,1.57973245877761,-1.56727750662821,-0.0129643729627431,0.00478214711714979 -0.066,0.25519364498011,0.996169706814893,1.57321188950975,1.58047712135703,-1.56698154846202,-0.0140597756104065,0.00518670575676088 -0.068,0.25562285600223,0.995862758811123,1.57340221067413,1.58124164540536,-1.56667724072532,-0.0151869150705921,0.00560307002438021 -0.07,0.256065165530977,0.995547901762665,1.57359691402226,1.58202410244853,-1.56636531286683,-0.0163431603927744,0.00603027563651297 -0.0720000000000001,0.256519719508883,0.995225852705231,1.57379551430601,1.58282257776265,-1.56604649229253,-0.0175258821265462,0.00646735831002896 -0.0740000000000001,0.256985649309076,0.994897328062009,1.57399753024604,1.5836351714967,-1.56572150419184,-0.0187324524477844,0.00691335374986928 -0.0760000000000001,0.257462070714391,0.994563043606989,1.57420248486925,1.58445999969163,-1.56539107137852,-0.0199602452740281,0.00736729763587812 -0.0780000000000001,0.257948082996526,0.994223714432794,1.57440990582331,1.58529519519592,-1.56505591414624,-0.0212066363691233,0.00782822560890514 -0.0800000000000001,0.258442768095387,0.993880054922939,1.57461932566765,1.58613890847775,-1.56471675013891,-0.0224690034371797,0.00829517325633661 -0.0820000000000001,0.25894518989871,0.993532778728442,1.57483028214013,1.58698930833416,-1.56437429423587,-0.0237447262058671,0.00876717609722456 -0.0840000000000001,0.259454393622146,0.993182598748703,1.57504231839865,1.58784458249719,-1.56402925845179,-0.0250311864990688,0.00924326956719108 -0.0860000000000001,0.259969405289927,0.992830227116568,1.57525498323697,1.58870293813725,-1.56368235185161,-0.0263257682988902,0.00972248900329144 -0.0880000000000001,0.260489231316283,0.9924763751875,1.57546783127372,1.58956260226394,-1.56333428048042,-0.0276258577970037,0.0102038696290235 -0.0900000000000001,0.261012858187775,0.992121753532774,1.57568042311382,1.5904218220245,-1.56298574730837,-0.0289288434352961,0.010686446539673 -0.0920000000000001,0.261539293116413,0.991767044398554,1.57589234193789,1.59127893145011,-1.56263742514831,-0.0302322171314552,0.0111692921788712 -0.0940000000000001,0.26206826540447,0.991412432143538,1.57610347297143,1.59213348235848,-1.56228949602713,-0.0315352999553912,0.0116521558609447 -0.0960000000000001,0.262599775628729,0.991057916464163,1.57631381254385,1.59298547524946,-1.56194196032547,-0.0328380914940666,0.0121350381449214 -0.0980000000000001,0.26313382436409,0.990703497061623,1.5765233569802,1.59383491061208,-1.5615948184307,-0.0341405913192268,0.0126179395848887 -0.1,0.263670412183494,0.990349173641918,1.57673210260126,1.59468178892469,-1.56124807073686,-0.0354427989874131,0.0131008607300459 -0.102,0.26420953965784,0.989994945915897,1.57694004572354,1.59552611065498,-1.56090171764465,-0.0367447140399751,0.0135838021247549 -0.104,0.26475120735591,0.989640813599312,1.57714718265939,1.59636787626012,-1.56055575956143,-0.0380463360030837,0.0140667643085924 -0.106,0.26529541584429,0.989286776412861,1.57735350971706,1.59720708618682,-1.56021019690123,-0.0393476643877437,0.0145497478164011 -0.108,0.265842165687293,0.988932834082236,1.57755902320072,1.59804374087143,-1.55986503008467,-0.0406486986898065,0.0150327531783412 -0.11,0.266391457446878,0.988578986338176,1.57776371941057,1.59887784074,-1.55952025953902,-0.0419494383899835,0.0155157809199423 -0.112,0.26694329168258,0.988225232916509,1.57796759464288,1.59970938620838,-1.55917588569813,-0.0432498829538583,0.0159988315621542 -0.114,0.267497668951425,0.987871573558206,1.57817064519004,1.60053837768231,-1.55883190900246,-0.0445500318319001,0.0164819056213988 -0.116,0.268054589807858,0.987518008009427,1.57837286734064,1.60136481555748,-1.55848832989906,-0.0458498844594769,0.0169650036096216 -0.118,0.268614054803667,0.987164536021568,1.57857425737952,1.60218870021964,-1.55814514884153,-0.0471494402568679,0.0174481260343425 -0.12,0.269176064487903,0.986811157351316,1.57877481158786,1.60301003204465,-1.55780236629004,-0.0484486986292776,0.017931273398708 -0.122,0.26974061940681,0.986457871760693,1.57897452624321,1.60382881139859,-1.55745998271133,-0.0497476589668481,0.0184144462015419 -0.124,0.270307720103742,0.986104679017106,1.57917339761955,1.60464503863785,-1.55711799857864,-0.0510463206446732,0.0188976449373969 -0.126,0.270877367119095,0.985751578893398,1.5793714219874,1.60545871410915,-1.55677641437178,-0.0523446830228112,0.019380870096606 -0.128,0.271449560990227,0.9853985711679,1.57956859561384,1.6062698381497,-1.55643523057704,-0.0536427454462986,0.0198641221653337 -0.13,0.272024302251384,0.985045655624473,1.57976491476258,1.60707841108723,-1.55609444768725,-0.0549405072451638,0.0203474016256274 -0.132,0.272601591433626,0.984692832052567,1.57996037569404,1.60788443324009,-1.55575406620173,-0.0562379677344403,0.0208307089554687 -0.134,0.273181429064756,0.984340100247267,1.5801549746654,1.6086879049173,-1.55541408662628,-0.0575351262141809,0.0213140446288245 -0.136,0.273763815669239,0.983987460009341,1.58034870793068,1.60948882641869,-1.5550745094732,-0.0588319819694711,0.0217974091156985 -0.138,0.274348751768133,0.983634911145294,1.58054157174079,1.61028719803492,-1.55473533526125,-0.0601285342704432,0.0222808028821822 -0.14,0.274936237879016,0.983282453467419,1.58073356234361,1.61108302004757,-1.55439656451566,-0.0614247823722904,0.0227642263905066 -0.142,0.275526274515908,0.982930086793845,1.58092467598404,1.61187629272925,-1.55405819776811,-0.0627207255152808,0.0232476800990929 -0.144,0.276118862189203,0.982577810948587,1.58111490890407,1.61266701634365,-1.55372023555674,-0.0640163629247713,0.0237311644626041 -0.146,0.276714001405596,0.982225625761601,1.58130425734287,1.61345519114562,-1.55338267842611,-0.0653116938112225,0.024214679931996 -0.148,0.277311692668004,0.981873531068833,1.58149271753681,1.61424081738124,-1.55304552692725,-0.0666067173702129,0.0246982269545687 -0.15,0.277911936475503,0.981521526712269,1.58168028571956,1.61502389528794,-1.55270878161758,-0.0679014327824531,0.0251818059740175 -0.152,0.278514733323249,0.981169612539986,1.58186695812217,1.61580442509453,-1.55237244306095,-0.0691958392138011,0.0256654174304842 -0.154,0.27912008370241,0.980817788406207,1.5820527309731,1.61658240702127,-1.55203651182763,-0.0704899358152766,0.0261490617606084 -0.156,0.279727988100092,0.98046605417135,1.58223760049831,1.61735784128,-1.55170098849429,-0.0717837217230759,0.0266327393975787 -0.158,0.280338446999268,0.980114409702077,1.58242156292131,1.61813072807417,-1.551365873644,-0.0730771960585872,0.0271164507711838 -0.16,0.280951460878711,0.979762854871352,1.58260461446327,1.61890106759892,-1.55103116786622,-0.0743703579284058,0.0276001963078635 -0.162,0.281567030212918,0.979411389558486,1.58278675134304,1.61966886004119,-1.5506968717568,-0.075663206424349,0.0280839764307602 -0.164,0.282185155472041,0.979060013649194,1.58296796977723,1.62043410557973,-1.55036298591798,-0.0769557406234719,0.02856779155977 -0.166,0.28280583712182,0.978708727035644,1.58314826598031,1.62119680438525,-1.55002951095837,-0.0782479595880829,0.0290516421115937 -0.168,0.283429075623506,0.978357529616511,1.58332763616464,1.62195695662045,-1.54969644749294,-0.0795398623657591,0.0295355284997882 -0.17,0.284054871433801,0.978006421297028,1.58350607654057,1.62271456244007,-1.54936379614306,-0.0808314479893627,0.0300194511348173 -0.172,0.284683225004777,0.977655401989038,1.58368358331648,1.62346962199104,-1.54903155753642,-0.0821227154770566,0.0305034104241034 -0.174,0.285314136783819,0.977304471611047,1.58386015269888,1.62422213541247,-1.5486997323071,-0.0834136638323206,0.030987406772078 -0.176,0.285947607213544,0.976953630088275,1.58403578089245,1.62497210283579,-1.54836832109552,-0.0847042920439679,0.0314714405802334 -0.178,0.286583636731742,0.976602877352713,1.58421046410015,1.62571952438478,-1.54803732454846,-0.0859945990861612,0.0319555122471737 -0.18,0.287222225771302,0.976252213343167,1.58438419852324,1.62646440017566,-1.54770674331902,-0.0872845839184297,0.0324396221686657 -0.182,0.287863374760144,0.97590163800532,1.5845569803614,1.62720673031715,-1.54737657806668,-0.0885742454856857,0.0329237707376901 -0.184,0.288507084121155,0.975551151291778,1.58472880581277,1.62794651491056,-1.54704682945723,-0.0898635827182415,0.0334079583444932 -0.186,0.289153354272115,0.975200753162125,1.58489967107404,1.62868375404986,-1.54671749816281,-0.0911525945318267,0.033892185376637 -0.188,0.289802185625634,0.974850443582977,1.58506957234051,1.62941844782171,-1.54638858486189,-0.0924412798276051,0.0343764522190514 -0.19,0.290453578589085,0.974500222528032,1.58523850580615,1.6301505963056,-1.54606009023927,-0.0937296374921925,0.0348607592540846 -0.192,0.291107533564534,0.974150089978124,1.58540646766371,1.63088019957388,-1.54573201498608,-0.0950176663976742,0.0353451068615543 -0.194,0.291764050948672,0.973800045921278,1.58557345410477,1.6316072576918,-1.54540435979976,-0.0963053654016227,0.0358294954187993 -0.196,0.292423131132757,0.973450090352759,1.5857394613198,1.63233177071767,-1.54507712538411,-0.0975927333471159,0.03631392530073 -0.198,0.293084774502535,0.973100223275128,1.58590448549825,1.63305373870282,-1.54475031244922,-0.0988797690627548,0.0367983968798798 -0.2,0.293748981438185,0.972750444698293,1.58606852282864,1.63377316169178,-1.5444239217115,-0.100166471362683,0.0372829105264564 -0.202,0.294415752314247,0.972400754639564,1.58623156949858,1.63449003972224,-1.54409795389372,-0.101452839046602,0.0377674666083925 -0.204,0.295085087499557,0.972051153123704,1.5863936216949,1.63520437282521,-1.54377240972491,-0.102738870899796,0.038252065491397 -0.206,0.295756987357185,0.971701640182982,1.5865546756037,1.63591616102504,-1.54344728994046,-0.104024565693144,0.0387367075390062 -0.208,0.296431452244367,0.971352215857228,1.58671472741044,1.6366254043395,-1.54312259528206,-0.105309922183144,0.0392213931126348 -0.21,0.297108482512437,0.971002880193886,1.58687377329997,1.63733210277985,-1.54279832649772,-0.106594939111928,0.0397061225716272 -0.212,0.297788078506771,0.970653633248063,1.58703180945667,1.63803625635091,-1.54247448434176,-0.107879615207285,0.0401908962733081 -0.214,0.298470240566714,0.970304475082586,1.58718883206446,1.63873786505112,-1.54215106957482,-0.10916394918268,0.0406757145730338 -0.216,0.299154969025521,0.969955405768056,1.58734483730694,1.63943692887261,-1.54182808296385,-0.110447939737272,0.0411605778242434 -0.218,0.29984226421029,0.969606425382896,1.58749982136741,1.64013344780127,-1.54150552528214,-0.111731585555937,0.0416454863785097 -0.22,0.3005321264419,0.969257534013409,1.58765378042898,1.64082742181683,-1.54118339730925,-0.113014885309286,0.04213044058559 -0.222,0.301224556034948,0.968908731753829,1.58780671067464,1.6415188508929,-1.54086169983111,-0.114297837653685,0.0426154407934776 -0.224,0.301919553297684,0.968560018706372,1.58795860828732,1.64220773499704,-1.54054043363992,-0.115580441231278,0.0431004873484523 -0.226,0.302617118531949,0.968211394981292,1.58810946944998,1.64289407409085,-1.54021959953422,-0.116862694670006,0.0435855805951317 -0.228,0.303317252033112,0.967862860696935,1.58825929034571,1.64357786813002,-1.53989919831888,-0.118144596583632,0.0440707208765224 -0.23,0.304019954090006,0.967514415979785,1.58840806715775,1.6442591170644,-1.53957923080508,-0.119426145571755,0.0445559085340702 -0.232,0.304725224984871,0.967166060964527,1.58855579606963,1.64493782083805,-1.53925969781032,-0.120707340219838,0.0450411439077118 -0.234,0.305433064993284,0.96681779579409,1.5887024732652,1.64561397938933,-1.53894060015842,-0.121988179099229,0.0455264273359255 -0.236,0.306143474384104,0.966469620619707,1.58884809492874,1.64628759265094,-1.53862193867954,-0.123268660767179,0.0460117591557821 -0.238,0.306856453419405,0.966121535600963,1.58899265724502,1.64695866055001,-1.53830371421017,-0.124548783766869,0.0464971397029957 -0.24,0.30757200235442,0.965773540905851,1.58913615639938,1.64762718300816,-1.53798592759311,-0.12582854662743,0.0469825693119748 -0.242,0.308290121437475,0.965425636710822,1.58927858857782,1.64829315994152,-1.53766857967752,-0.127107947863965,0.047468048315873 -0.244,0.309010810909931,0.965077823200839,1.58941994996707,1.64895659126086,-1.53735167131887,-0.128386985977575,0.0479535770466399 -0.246,0.309734071006122,0.96473010056943,1.58956023675469,1.64961747687163,-1.53703520337899,-0.129665659455379,0.0484391558350722 -0.248,0.310459901953295,0.964382469018738,1.58969944512909,1.65027581667399,-1.53671917672604,-0.130943966770539,0.048924785010864 -0.25,0.31118830397155,0.964034928759578,1.58983757127968,1.65093161056293,-1.53640359223453,-0.132221906382283,0.0494104649026581 -0.252,0.311919277273779,0.963687480011482,1.58997461139693,1.65158485842826,-1.53608845078532,-0.13349947673593,0.0498961958380963 -0.254,0.312652822065607,0.963340123002759,1.59011056167242,1.65223556015477,-1.53577375326561,-0.134776676262912,0.0503819781438705 -0.256,0.313388938545334,0.962992857970542,1.59024541829896,1.65288371562221,-1.53545950056896,-0.1360535033808,0.0508678121457733 -0.258,0.31412762690387,0.962645685160841,1.59037917747063,1.65352932470537,-1.53514569359531,-0.137329956493328,0.0513536981687488 -0.26,0.314868887324684,0.962298604828597,1.59051183538291,1.65417238727418,-1.53483233325094,-0.13860603399042,0.051839636536943 -0.262,0.315612719983738,0.961951617237731,1.59064338823272,1.65481290319372,-1.53451942044849,-0.139881734248211,0.0523256275737547 -0.264,0.316359125049433,0.961604722661196,1.59077383221852,1.65545087232433,-1.534206956107,-0.141157055629075,0.0528116716018859 -0.266,0.317108102682548,0.961257921381031,1.59090316354039,1.65608629452163,-1.53389494115187,-0.142431996481649,0.0532977689433926 -0.268,0.317859653036183,0.960911213688412,1.59103137840012,1.6567191696366,-1.53358337651488,-0.143706555140862,0.0537839199197351 -0.27,0.3186137762557,0.9605645998837,1.59115847300127,1.65734949751566,-1.53327226313419,-0.144980729927958,0.0542701248518287 -0.272,0.319370472478667,0.960218080276496,1.59128444354927,1.65797727800068,-1.53296160195437,-0.146254519150522,0.0547563840600941 -0.274,0.320129741834796,0.95987165518569,1.5914092862515,1.6586025109291,-1.53265139392637,-0.147527921102509,0.0552426978645079 -0.276,0.320891584445894,0.959525324939513,1.59153299731737,1.65922519613393,-1.53234164000754,-0.14880093406427,0.0557290665846529 -0.278,0.321656000425796,0.959179089875588,1.59165557295841,1.65984533344388,-1.53203234116164,-0.150073556302579,0.0562154905397685 -0.28,0.322422989880316,0.958832950340979,1.59177700938833,1.66046292268334,-1.53172349835885,-0.151345786070659,0.0567019700488009 -0.282,0.323192552907185,0.958486906692245,1.59189730282316,1.66107796367251,-1.53141511257575,-0.15261762160821,0.0571885054304539 -0.284,0.323964689595999,0.958140959295485,1.59201644948126,1.66169045622742,-1.53110718479536,-0.153889061141441,0.0576750970032382 -0.286,0.324739400028158,0.957795108526396,1.59213444558344,1.66230040016,-1.53079971600712,-0.155160102883092,0.0581617450855226 -0.288,0.325516684276815,0.957449354770314,1.59225128735308,1.66290779527813,-1.5304927072069,-0.156430745032465,0.0586484499955835 -0.29,0.326296542406816,0.957103698422271,1.59236697101614,1.66351264138571,-1.53018615939702,-0.157700985775455,0.059135212051655 -0.292,0.327078974474646,0.956758139887043,1.5924814928013,1.66411493828273,-1.52988007358625,-0.158970823284575,0.0596220315719796 -0.294,0.327863980528374,0.956412679579197,1.59259484894003,1.66471468576528,-1.5295744507898,-0.160240255718988,0.0601089088748571 -0.296,0.328651560607598,0.956067317923144,1.59270703566668,1.66531188362568,-1.52926929202935,-0.161509281224534,0.0605958442786957 -0.298,0.329441714743389,0.955722055353186,1.59281804921854,1.66590653165247,-1.52896459833304,-0.162777897933763,0.061082838102061 -0.3,0.330234442958236,0.955376892313565,1.59292788583597,1.6664986296305,-1.52866037073551,-0.164046103965962,0.0615698906637263 -0.302,0.331029745265994,0.955031829258514,1.59303654176244,1.66708817734099,-1.52835661027784,-0.165313897427187,0.0620570022827223 -0.304,0.331827621671827,0.954686866652304,1.59314401324465,1.66767517456159,-1.52805331800762,-0.166581276410293,0.0625441732783867 -0.306,0.332628072172154,0.954342004969292,1.5932502965326,1.6682596210664,-1.52775049497894,-0.167848238994966,0.063031403970414 -0.308,0.333431096754597,0.953997244693971,1.59335538787968,1.66884151662608,-1.52744814225239,-0.16911478324775,0.0635186946789048 -0.31,0.334236695397927,0.953652586321016,1.59345928354276,1.66942086100786,-1.52714626089506,-0.170380907222085,0.0640060457244159 -0.312,0.335044868072007,0.953308030355334,1.59356197978227,1.66999765397563,-1.52684485198055,-0.171646608958332,0.0644934574280089 -0.314,0.335855614737744,0.95296357731211,1.5936634728623,1.67057189528999,-1.52654391658902,-0.172911886483809,0.0649809301113006 -0.316,0.336668935347032,0.952619227716857,1.59376375905067,1.67114358470826,-1.52624345580712,-0.174176737812823,0.0654684640965115 -0.318,0.337484829842701,0.952274982105459,1.59386283461903,1.67171272198463,-1.52594347072807,-0.175441160946701,0.0659560597065153 -0.32,0.338303298158464,0.951930841024221,1.59396069584294,1.6722793068701,-1.52564396245162,-0.176705153873823,0.0664437172648883 -0.322,0.339124340218865,0.951586805029915,1.59405733900198,1.67284333911265,-1.52534493208409,-0.177968714569655,0.0669314370959582 -0.324,0.339947955939224,0.951242874689827,1.5941527603798,1.67340481845719,-1.52504638073836,-0.179231840996784,0.0674192195248532 -0.326,0.34077414522559,0.950899050581803,1.59424695626424,1.6739637446457,-1.52474830953388,-0.18049453110495,0.0679070648775512 -0.328,0.341602907974683,0.950555333294294,1.59433992294741,1.67452011741722,-1.52445071959667,-0.18175678283108,0.0683949734809281 -0.33,0.342434244073849,0.950211723426402,1.59443165672576,1.67507393650796,-1.52415361205937,-0.183018594099322,0.0688829456628071 -0.332,0.343268153401002,0.949868221587929,1.59452215390022,1.6756252016513,-1.52385698806118,-0.184279962821081,0.069370981752007 -0.334,0.344104635824578,0.949524828399416,1.59461141077621,1.67617391257789,-1.52356084874794,-0.18554088689505,0.0698590820783912 -0.336,0.344943691203482,0.949181544492193,1.59469942366383,1.67672006901567,-1.52326519527207,-0.186801364207251,0.0703472469729158 -0.338,0.345785319387035,0.948838370508422,1.59478618887784,1.67726367068995,-1.52297002879264,-0.188061392631064,0.0708354767676782 -0.34,0.346629520214928,0.948495307101141,1.59487170273784,1.67780471732343,-1.52267535047535,-0.189320970027268,0.0713237717959655 -0.342,0.347476293517167,0.948152354934307,1.59495596156831,1.6783432086363,-1.52238116149254,-0.190580094244071,0.0718121323923024 -0.344,0.348325639114026,0.947809514682845,1.59503896169872,1.67887914434624,-1.52208746302317,-0.191838763117154,0.0723005588924998 -0.346,0.349177556815994,0.947466787032685,1.59512069946362,1.67941252416852,-1.52179425625291,-0.193096974469698,0.0727890516337023 -0.348,0.350032046423729,0.947124172680809,1.59520117120271,1.67994334781601,-1.52150154237405,-0.194354726112429,0.0732776109544367 -0.35,0.350889107728003,0.946781672335295,1.59528037326097,1.68047161499928,-1.5212093225856,-0.19561201584365,0.0737662371946591 -0.352,0.351748740509659,0.946439286715355,1.5953583019887,1.68099732542658,-1.52091759809321,-0.19686884144928,0.0742549306958032 -0.354,0.352610944539556,0.946097016551382,1.59543495374166,1.68152047880399,-1.52062637010927,-0.198125200702894,0.0747436918008276 -0.356,0.353475719578523,0.94575486258499,1.59551032488114,1.68204107483538,-1.52033563985285,-0.199381091365754,0.0752325208542634 -0.358,0.354343065377309,0.945412825569056,1.59558441177404,1.68255911322251,-1.52004540854973,-0.200636511186854,0.0757214182022613 -0.36,0.355212981676535,0.945070906267762,1.59565721079299,1.68307459366507,-1.51975567743242,-0.201891457902957,0.0762103841926393 -0.362,0.356085468206646,0.944729105456634,1.5957287183164,1.68358751586073,-1.51946644774018,-0.203145929238629,0.0766994191749294 -0.364,0.356960524687859,0.944387423922587,1.59579893072861,1.6840978795052,-1.51917772071898,-0.204399922906287,0.0771885235004247 -0.366,0.35783815083012,0.944045862463961,1.59586784441992,1.68460568429225,-1.51888949762157,-0.205653436606227,0.0776776975222266 -0.368,0.358718346333053,0.943704421890561,1.59593545578674,1.6851109299138,-1.51860177970743,-0.206906468026673,0.0781669415952909 -0.37,0.359601110885912,0.943363103023703,1.59600176123164,1.68561361605995,-1.51831456824285,-0.208159014843814,0.0786562560764751 -0.372,0.360486444167536,0.943021906696245,1.59606675716345,1.68611374241903,-1.51802786450087,-0.209411074721842,0.0791456413245844 -0.374,0.361374345846298,0.942680833752633,1.59613043999738,1.68661130867764,-1.51774166976134,-0.210662645312993,0.0796350977004183 -0.376,0.36226481558006,0.942339885048933,1.59619280615507,1.68710631452072,-1.51745598531088,-0.211913724257591,0.0801246255668166 -0.378,0.363157853016126,0.941999061452876,1.59625385206475,1.68759875963159,-1.51717081244295,-0.213164309184085,0.0806142252887057 -0.38,0.364053457791195,0.941658363843892,1.59631357416123,1.688088643692,-1.51688615245782,-0.214414397709093,0.0811038972331446 -0.382,0.364951629531313,0.941317793113149,1.59637196888611,1.68857596638216,-1.51660200666258,-0.215663987437443,0.08159364176937 -0.384,0.365852367851828,0.940977350163588,1.59642903268779,1.68906072738083,-1.51631837637116,-0.216913075962212,0.082083459268843 -0.386,0.366755672357344,0.940637035909964,1.5964847620216,1.68954292636532,-1.51603526290435,-0.218161660864773,0.0825733501052936 -0.388,0.367661542641671,0.940296851278877,1.59653915334987,1.69002256301158,-1.5157526675898,-0.219409739714834,0.0830633146547665 -0.39,0.368569978287785,0.939956797208814,1.59659220314207,1.69049963699421,-1.515470591762,-0.220657310070483,0.0835533532956665 -0.392,0.369480978867779,0.939616874650179,1.59664390787484,1.69097414798655,-1.51518903676233,-0.221904369478227,0.0840434664088029 -0.394,0.370394543942817,0.939277084565332,1.59669426403216,1.69144609566067,-1.51490800393909,-0.223150915473041,0.0845336543774348 -0.396,0.371310673063087,0.938937427928622,1.59674326810536,1.6919154796875,-1.51462749464742,-0.224396945578406,0.0850239175873156 -0.398,0.372229365767761,0.938597905726425,1.59679091659329,1.69238229973678,-1.51434751024941,-0.225642457306357,0.0855142564267376 -0.4,0.373150621584946,0.938258518957171,1.59683720600238,1.69284655547717,-1.51406805211405,-0.226887448157526,0.0860046712865763 -0.402,0.374074440031639,0.937919268631384,1.59688213284672,1.6933082465763,-1.51378912161725,-0.228131915621184,0.0864951625603347 -0.404,0.375000820613682,0.937580155771713,1.59692569364819,1.69376737270078,-1.51351072014188,-0.229375857175288,0.086985730644187 -0.406,0.375929762825722,0.937241181412965,1.59696788493653,1.69422393351626,-1.51323284907771,-0.230619270286527,0.0874763759370227 -0.408,0.376861266151158,0.936902346602134,1.59700870324946,1.69467792868748,-1.51295550982152,-0.231862152410366,0.0879670988404902 -0.41,0.377795330062107,0.936563652398441,1.59704814513274,1.69512935787833,-1.51267870377702,-0.233104500991088,0.0884578997590401 -0.412,0.37873195401935,0.936225099873356,1.59708620714031,1.69557822075187,-1.51240243235489,-0.234346313461847,0.0889487790999687 -0.414,0.379671137472297,0.935886690110636,1.59712288583434,1.69602451697037,-1.51212669697282,-0.235587587244708,0.0894397372734609 -0.416,0.380612879858934,0.935548424206352,1.59715817778535,1.6964682461954,-1.51185149905546,-0.236828319750696,0.089930774692633 -0.418,0.38155718060579,0.935210303268921,1.59719207957232,1.69690940808782,-1.5115768400345,-0.238068508379842,0.0904218917735756 -0.42,0.382504039127882,0.934872328419132,1.59722458778275,1.69734800230787,-1.51130272134862,-0.239308150521231,0.090913088935396 -0.422,0.383453454828683,0.934534500790182,1.5972556990128,1.69778402851517,-1.51102914444351,-0.240547243553046,0.0914043666002606 -0.424,0.38440542710007,0.934196821527698,1.59728540986734,1.69821748636881,-1.51075611077192,-0.24178578484262,0.0918957251934366 -0.426,0.385359955322286,0.933859291789768,1.59731371696009,1.69864837552736,-1.51048362179361,-0.243023771746481,0.0923871651433344 -0.428,0.386317038863895,0.933521912746971,1.59734061691368,1.69907669564892,-1.51021167897542,-0.244261201610398,0.0928786868815489 -0.43,0.387276677081741,0.933184685582401,1.59736610635976,1.69950244639119,-1.50994028379122,-0.245498071769434,0.0933702908429011 -0.432,0.388238869320904,0.932847611491693,1.59739018193913,1.69992562741147,-1.50966943772197,-0.246734379547992,0.0938619774654791 -0.434,0.389203614914658,0.932510691683054,1.59741284030178,1.70034623836673,-1.50939914225568,-0.247970122259861,0.0943537471906794 -0.436,0.390170913184431,0.932173927377284,1.59743407810701,1.70076427891365,-1.50912939888748,-0.249205297208272,0.0948456004632477 -0.438,0.391140763439758,0.931837319807807,1.59745389202356,1.70117974870866,-1.50886020911956,-0.25043990168594,0.0953375377313192 -0.44,0.392113164978246,0.931500870220689,1.59747227872965,1.70159264740799,-1.50859157446123,-0.251673932975118,0.0958295594464589 -0.442,0.393088117085527,0.931164579874666,1.59748923491312,1.70200297466769,-1.50832349642891,-0.252907388347646,0.0963216660637022 -0.444,0.394065619035216,0.930828450041171,1.59750475727152,1.70241073014369,-1.50805597654615,-0.254140265064998,0.0968138580415941 -0.446,0.395045670088874,0.930492482004351,1.59751884251218,1.70281591349184,-1.50778901634361,-0.25537256037834,0.0973061358422294 -0.448,0.396028269495965,0.930156677061093,1.59753148735235,1.70321852436796,-1.50752261735909,-0.25660427152857,0.0977984999312916 -0.45,0.397013416493813,0.929821036521048,1.59754268851926,1.70361856242784,-1.50725678113756,-0.257835395746379,0.0982909507780924 -0.452,0.398001110307561,0.929485561706649,1.59755244275026,1.70401602732735,-1.5069915092311,-0.259065930252294,0.0987834888556103 -0.454,0.398991350150136,0.929150253953137,1.59756074679286,1.70441091872241,-1.506726803199,-0.260295872256736,0.0992761146405295 -0.456,0.399984135222199,0.928815114608577,1.59756759740488,1.70480323626908,-1.50646266460768,-0.261525218960066,0.099768828613278 -0.458,0.400979464712113,0.928480145033881,1.59757299135453,1.70519297962359,-1.50619909503075,-0.262753967552641,0.100261631258066 -0.46,0.401977337795896,0.928145346602827,1.59757692542049,1.70558014844233,-1.50593609604903,-0.263982115214864,0.100754523062923 -0.462,0.402977753637188,0.927810720702078,1.59757939639205,1.70596474238199,-1.5056736692505,-0.265209659117237,0.101247504519738 -0.464,0.403980711387202,0.927476268731201,1.59758040106917,1.70634676109951,-1.50541181623034,-0.266436596420413,0.101740576124292 -0.466,0.404986210184694,0.927141992102685,1.59757993626258,1.70672620425215,-1.50515053859098,-0.267662924275251,0.1022337383763 -0.468,0.405994249155913,0.926807892241959,1.5975779987939,1.70710307149753,-1.50488983794201,-0.268888639822866,0.102726991779444 -0.47,0.40700482741457,0.926473970587407,1.59757458549574,1.7074773624937,-1.50462971590028,-0.270113740194684,0.103220336841414 -0.472,0.408017944061793,0.926140228590386,1.59756969321177,1.7078490768991,-1.50437017408986,-0.271338222512494,0.103713774073937 -0.474,0.409033598186093,0.925806667715241,1.59756331879685,1.70821821437268,-1.50411121414204,-0.272562083888507,0.104207303992821 -0.476,0.410051788863317,0.925473289439324,1.59755545911709,1.7085847745739,-1.50385283769538,-0.273785321425403,0.104700927117986 -0.478,0.411072515156616,0.925140095253,1.59754611105,1.70894875716276,-1.50359504639568,-0.275007932216388,0.1051946439735 -0.48,0.412095776116403,0.924807086659672,1.59753527148455,1.70931016179987,-1.50333784189597,-0.276229913345252,0.105688455087614 -0.482,0.413121570780314,0.924474265175787,1.59752293732127,1.70966898814644,-1.50308122585658,-0.277451261886419,0.106182360992797 -0.484,0.414149898173172,0.924141632330849,1.59750910547238,1.71002523586438,-1.50282519994509,-0.278671974905004,0.106676362225771 -0.486,0.415180757306943,0.923809189667436,1.59749377286185,1.71037890461627,-1.50256976583636,-0.279892049456867,0.107170459327546 -0.488,0.416214147180705,0.92347693874121,1.59747693642552,1.71072999406544,-1.50231492521251,-0.28111148258867,0.107664652843451 -0.49,0.417250066780603,0.923144881120924,1.59745859311121,1.71107850387601,-1.50206067976296,-0.282330271337933,0.108158943323168 -0.492,0.418288515079815,0.92281301838844,1.59743873987878,1.71142443371289,-1.50180703118444,-0.283548412733088,0.10865333132077 -0.494,0.419329491038513,0.922481352138732,1.59741737370027,1.71176778324186,-1.50155398118094,-0.284765903793536,0.109147817394746 -0.496,0.420372993603824,0.922149883979901,1.59739449155999,1.71210855212956,-1.50130153146376,-0.285982741529702,0.10964240210804 -0.498,0.421419021709794,0.921818615533179,1.59737009045458,1.71244674004356,-1.50104968375153,-0.287198922943095,0.110137086028081 -0.5,0.422467574277348,0.921487548432941,1.59734416739316,1.71278234665239,-1.50079843977016,-0.288414445026361,0.110631869726815 -0.502,0.423518650214258,0.92115668432671,1.59731671939742,1.71311537162556,-1.50054780125289,-0.289629304763342,0.111126753780736 -0.504,0.424572248415098,0.920826024875166,1.5972877435017,1.71344581463362,-1.50029776994026,-0.290843499129133,0.111621738770918 -0.506,0.425628367761213,0.920495571752151,1.59725723675307,1.71377367534816,-1.50004834758017,-0.292057025090136,0.112116825283045 -0.508,0.426687007120678,0.920165326644675,1.5972251962115,1.71409895344188,-1.4997995359278,-0.293269879604124,0.112612013907443 -0.51,0.427748165348264,0.919835291252921,1.59719161894989,1.7144216485886,-1.49955133674569,-0.294482059620294,0.113107305239111 -0.512,0.428811841285401,0.91950546729025,1.59715650205419,1.71474176046331,-1.49930375180371,-0.295693562079326,0.113602699877745 -0.514,0.429878033760137,0.919175856483205,1.59711984262351,1.71505928874217,-1.49905678287905,-0.296904383913441,0.114098198427777 -0.516,0.430946741587108,0.918846460571513,1.59708163777022,1.7153742331026,-1.49881043175625,-0.298114522046459,0.114593801498395 -0.518,0.432017963567498,0.918517281308088,1.59704188462004,1.71568659322327,-1.4985647002272,-0.29932397339386,0.115089509703578 -0.52,0.433091698489001,0.918188320459035,1.59700058031211,1.71599636878415,-1.49831959009112,-0.300532734862839,0.115585323662122 -0.522,0.434167945125789,0.917859579803647,1.59695772199916,1.71630355946653,-1.49807510315458,-0.301740803352369,0.116081243997667 -0.524,0.435246702238473,0.917531061134412,1.59691330684755,1.71660816495307,-1.49783124123149,-0.302948175753255,0.116577271338728 -0.526,0.436327968574069,0.917202766257005,1.59686733203737,1.71691018492781,-1.49758800614312,-0.304154848948199,0.117073406318718 -0.528,0.437411742865961,0.916874696990294,1.59681979476258,1.71720961907622,-1.4973453997181,-0.305360819811854,0.11756964957598 -0.53,0.438498023833864,0.916546855166338,1.59677069223108,1.71750646708523,-1.49710342379238,-0.306566085210889,0.118066001753808 -0.532,0.439586810183794,0.916219242630379,1.59672002166479,1.71780072864327,-1.49686208020928,-0.307770642004043,0.118562463500479 -0.534,0.440678100608024,0.915891861240847,1.5966677802998,1.71809240344026,-1.4966213708195,-0.308974487042191,0.119059035469273 -0.536,0.441771893785058,0.915564712869354,1.59661396538643,1.71838149116769,-1.49638129748104,-0.3101776171684,0.119555718318503 -0.538,0.442868188379589,0.915237799400688,1.59655857418932,1.71866799151864,-1.49614186205929,-0.311380029217989,0.120052512711538 -0.54,0.443966983042466,0.91491112273281,1.59650160398758,1.71895190418777,-1.49590306642699,-0.312581720018594,0.120549419316826 -0.542,0.445068276410662,0.91458468477685,1.59644305207483,1.71923322887141,-1.49566491246421,-0.313782686390222,0.121046438807922 -0.544,0.446172067107234,0.914258487457099,1.59638291575934,1.71951196526756,-1.49542740205841,-0.314982925145319,0.12154357186351 -0.546,0.447278353741294,0.913932532711005,1.59632119236409,1.7197881130759,-1.49519053710435,-0.316182433088826,0.122040819167425 -0.548,0.448387134907968,0.913606822489161,1.59625787922691,1.72006167199787,-1.49495431950419,-0.31738120701824,0.122538181408677 -0.55,0.449498409188369,0.913281358755302,1.59619297370056,1.72033264173665,-1.4947187511674,-0.31857924372368,0.123035659281475 -0.552,0.450612175149558,0.912956143486296,1.59612647315281,1.72060102199721,-1.49448383401081,-0.319776539987942,0.123533253485245 -0.554,0.451728431344509,0.91263117867213,1.59605837496658,1.72086681248635,-1.4942495699586,-0.320973092586567,0.124030964724659 -0.556,0.452847176312079,0.912306466315906,1.59598867653998,1.72113001291272,-1.49401596094226,-0.322168898287895,0.124528793709649 -0.558,0.453968408576972,0.911982008433825,1.59591737528648,1.72139062298681,-1.49378300890066,-0.323363953853137,0.125026741155429 -0.56,0.455092126649704,0.91165780705518,1.59584446863492,1.72164864242107,-1.49355071577996,-0.324558256036426,0.125524807782522 -0.562,0.456218329026572,0.911333864222342,1.5957699540297,1.72190407092982,-1.49331908353368,-0.325751801584889,0.126022994316769 -0.564,0.457347014189618,0.911010181990748,1.59569382893082,1.72215690822939,-1.49308811412265,-0.326944587238701,0.12652130148936 -0.566,0.458478180606598,0.910686762428884,1.59561609081397,1.72240715403807,-1.49285780951502,-0.328136609731153,0.127019730036845 -0.568,0.459611826730947,0.910363607618278,1.59553673717068,1.72265480807615,-1.49262817168628,-0.329327865788713,0.127518280701155 -0.57,0.460747951001747,0.910040719653478,1.59545576550837,1.72289987006599,-1.4923992026192,-0.330518352131086,0.128016954229621 -0.572,0.461886551843691,0.909718100642041,1.59537317335046,1.723142339732,-1.49217090430386,-0.331708065471283,0.128515751374993 -0.574,0.463027627667057,0.909395752704516,1.59528895823648,1.72338221680068,-1.49194327873766,-0.332897002515675,0.129014672895453 -0.576,0.464171176867667,0.909073677974424,1.59520311772215,1.72361950100065,-1.49171632792528,-0.334085159964064,0.129513719554636 -0.578,0.46531719782686,0.908751878598245,1.59511564937949,1.72385419206267,-1.49149005387869,-0.335272534509743,0.130012892121643 -0.58,0.466465688911458,0.908430356735398,1.59502655079689,1.72408628971968,-1.49126445861714,-0.336459122839557,0.13051219137106 -0.582,0.467616648473732,0.908109114558218,1.59493581957926,1.7243157937068,-1.49103954416714,-0.337644921633971,0.131011618082973 -0.584,0.468770074851372,0.907788154251945,1.59484345334806,1.72454270376138,-1.49081531256249,-0.338829927567129,0.131511173042979 -0.586,0.469925966367455,0.907467478014693,1.59474944974146,1.72476701962301,-1.49059176584422,-0.34001413730692,0.132010857042206 -0.588,0.471084321330411,0.907147088057438,1.59465380641438,1.72498874103356,-1.49036890606064,-0.341197547515043,0.132510670877323 -0.59,0.472245138033991,0.906826986603992,1.59455652103862,1.72520786773716,-1.49014673526726,-0.342380154847064,0.133010615350555 -0.592,0.473408414757241,0.906507175890978,1.59445759130295,1.72542439948029,-1.48992525552685,-0.34356195595249,0.133510691269697 -0.594,0.47457414976446,0.906187658167812,1.59435701491321,1.72563833601175,-1.4897044689094,-0.344742947474824,0.134010899448123 -0.596,0.47574234130518,0.905868435696676,1.59425478959237,1.72584967708271,-1.48948437749209,-0.345923126051633,0.134511240704803 -0.598,0.476912987614126,0.905549510752494,1.5941509130807,1.72605842244674,-1.48926498335933,-0.347102488314612,0.13501171586431 -0.6,0.478086086911188,0.905230885622905,1.59404538313576,1.72626457185978,-1.48904628860268,-0.348281030889647,0.135512325756834 -0.602,0.479261637401393,0.904912562608241,1.59393819753261,1.72646812508025,-1.48882829532091,-0.349458750396881,0.136013071218192 -0.604,0.480439637274868,0.904594544021494,1.59382935406381,1.72666908186899,-1.48861100561995,-0.350635643450775,0.136513953089834 -0.606,0.481620084706813,0.904276832188293,1.59371885053956,1.72686744198932,-1.48839442161287,-0.351811706660177,0.13701497221886 -0.608,0.482802977857471,0.903959429446877,1.59360668478781,1.72706320520708,-1.48817854541989,-0.352986936628383,0.137516129458023 -0.61,0.483988314872094,0.90364233814806,1.5934928546543,1.72725637129059,-1.48796337916836,-0.3541613299532,0.138017425665739 -0.612,0.485176093880918,0.903325560655206,1.5933773580027,1.72744694001076,-1.48774892499274,-0.355334883227016,0.138518861706095 -0.614,0.486366312999127,0.903009099344198,1.59326019271468,1.72763491114102,-1.4875351850346,-0.356507593036861,0.139020438448859 -0.616,0.487558970326826,0.902692956603405,1.59314135669003,1.7278202844574,-1.48732216144258,-0.357679455964469,0.139522156769481 -0.618,0.488754063949011,0.902377134833652,1.59302084784673,1.72800305973853,-1.4871098563724,-0.358850468586351,0.140024017549106 -0.62,0.489951591935539,0.902061636448187,1.59289866412103,1.72818323676566,-1.48689827198684,-0.36002062747385,0.140526021674575 -0.622,0.491151552341097,0.901746463872645,1.59277480346758,1.72836081532269,-1.4866874104557,-0.361189929193213,0.141028170038431 -0.624,0.492353943205176,0.901431619545019,1.59264926385951,1.72853579519617,-1.48647727395584,-0.362358370305652,0.141530463538927 -0.626,0.493558762552036,0.901117105915622,1.59252204328849,1.72870817617534,-1.48626786467109,-0.363525947367411,0.142032903080027 -0.628,0.494766008390683,0.900802925447048,1.59239313976488,1.72887795805214,-1.48605918479229,-0.364692656929829,0.142535489571409 -0.63,0.495975678714836,0.900489080614146,1.59226255131776,1.72904514062123,-1.48585123651724,-0.365858495539406,0.143038223928472 -0.632,0.497187771502897,0.900175573903971,1.59213027599509,1.72920972368,-1.48564402205071,-0.36702345973787,0.143541107072334 -0.634,0.498402284717928,0.899862407815755,1.59199631186372,1.72937170702861,-1.48543754360439,-0.368187546062236,0.144044139929837 -0.636,0.499619216307615,0.899549584860865,1.59186065700957,1.72953109046998,-1.48523180339688,-0.369350751044879,0.144547323433548 -0.638,0.500838564204245,0.899237107562765,1.59172330953764,1.72968787380984,-1.4850268036537,-0.370513071213592,0.145050658521758 -0.64,0.502060326324677,0.898924978456973,1.59158426757215,1.72984205685669,-1.48482254660723,-0.371674503091655,0.145554146138485 -0.642,0.503284500570311,0.898613200091027,1.59144352925663,1.72999363942192,-1.4846190344967,-0.372835043197899,0.146057787233472 -0.644,0.504511084827061,0.898301775024437,1.59130109275397,1.73014262131969,-1.48441626956818,-0.373994688046771,0.146561582762187 -0.646,0.50574007696533,0.897990705828645,1.59115695624656,1.73028900236709,-1.48421425407455,-0.3751534341484,0.147065533685819 -0.648,0.506971474839979,0.897679995086986,1.59101111793635,1.73043278238403,-1.4840129902755,-0.37631127800866,0.147569640971281 -0.65,0.508205276290301,0.89736964539464,1.59086357604494,1.73057396119335,-1.48381248043744,-0.377468216129236,0.148073905591203 -0.652,0.509441479139993,0.897059659358588,1.59071432881369,1.73071253862077,-1.48361272683357,-0.37862424500769,0.148578328523931 -0.654,0.51068008119713,0.896750039597571,1.59056337450377,1.73084851449496,-1.4834137317438,-0.379779361137527,0.149082910753522 -0.656,0.511921080254135,0.896440788742039,1.59041071139629,1.73098188864751,-1.4832154974547,-0.380933561008256,0.149587653269741 -0.658,0.513164474087757,0.896131909434109,1.59025633779236,1.73111266091297,-1.48301802625956,-0.382086841105458,0.150092557068056 -0.66,0.514410260459039,0.895823404327517,1.59010025201319,1.73124083112886,-1.48282132045828,-0.383239197910851,0.15059762314963 -0.662,0.515658437113297,0.89551527608757,1.58994245240019,1.73136639913569,-1.48262538235739,-0.384390627902356,0.151102852521318 -0.664,0.516909001780089,0.895207527391094,1.58978293731502,1.73148936477694,-1.48243021427003,-0.385541127554157,0.151608246195661 -0.666,0.51816195217319,0.894900160926391,1.58962170513971,1.73160972789914,-1.48223581851586,-0.386690693336772,0.152113805190873 -0.668,0.519417285990568,0.894593179393185,1.58945875427673,1.7317274883518,-1.48204219742113,-0.387839321717115,0.15261953053084 -0.67,0.520675000914355,0.894286585502574,1.58929408314909,1.73184264598752,-1.48184935331856,-0.38898700915856,0.153125423245108 -0.672,0.521935094610826,0.893980381976974,1.5891276902004,1.7319552006619,-1.48165728854736,-0.390133752121008,0.153631484368872 -0.674,0.523197564730369,0.893674571550073,1.58895957389499,1.73206515223364,-1.48146600545321,-0.391279547060951,0.154137714942972 -0.676,0.524462408907462,0.893369156966773,1.58878973271797,1.73217250056449,-1.48127550638819,-0.392424390431536,0.154644116013877 -0.678,0.525729624760646,0.893064140983141,1.58861816517533,1.73227724551931,-1.48108579371077,-0.393568278682629,0.155150688633677 -0.68,0.526999209892505,0.892759526366351,1.58844486979399,1.73237938696605,-1.4808968697858,-0.394711208260885,0.155657433860071 -0.682,0.528271161889635,0.892455315894632,1.58826984512195,1.73247892477576,-1.48070873698442,-0.395853175609803,0.156164352756354 -0.684,0.529545478322624,0.892151512357212,1.58809308972831,1.73257585882264,-1.48052139768411,-0.396994177169802,0.156671446391406 -0.686,0.530822156746026,0.891848118554258,1.58791460220337,1.73267018898401,-1.48033485426858,-0.398134209378273,0.157178715839674 -0.688000000000001,0.532101194698337,0.891545137296824,1.58773438115874,1.73276191514032,-1.48014910912778,-0.399273268669656,0.157686162181166 -0.690000000000001,0.533382589701972,0.891242571406793,1.58755242522738,1.7328510371752,-1.47996416465785,-0.400411351475495,0.158193786501428 -0.692000000000001,0.53466633926324,0.890940423716815,1.58736873306373,1.73293755497542,-1.4797800232611,-0.401548454224505,0.158701589891533 -0.694000000000001,0.535952440872321,0.89063869707025,1.58718330334374,1.73302146843096,-1.47959668734595,-0.40268457334264,0.159209573448066 -0.696000000000001,0.537240892003244,0.890337394321109,1.586996134765,1.73310277743496,-1.47941415932693,-0.403819705253151,0.159717738273103 -0.698000000000001,0.538531690113862,0.890036518333992,1.58680722604678,1.73318148188375,-1.4792324416246,-0.404953846376654,0.1602260854742 -0.700000000000001,0.539824832645832,0.88973607198403,1.58661657593013,1.73325758167688,-1.47905153666555,-0.406086993131194,0.160734616164367 -0.702000000000001,0.541120317024588,0.889436058156818,1.58642418317797,1.73333107671711,-1.47887144688234,-0.407219141932305,0.161243331462059 -0.704000000000001,0.542418140659324,0.889136479748358,1.58623004657515,1.7334019669104,-1.47869217471346,-0.40835028919308,0.161752232491148 -0.706000000000001,0.543718300942968,0.888837339664992,1.58603416492852,1.73347025216597,-1.47851372260333,-0.409480431324229,0.16226132038091 -0.708000000000001,0.54502079525216,0.888538640823339,1.58583653706706,1.73353593239626,-1.47833609300219,-0.410609564734145,0.162770596266001 -0.710000000000001,0.546325620947233,0.888240386150234,1.5856371618419,1.73359900751695,-1.47815928836615,-0.411737685828969,0.163280061286435 -0.712000000000001,0.547632775372191,0.887942578582658,1.58543603812642,1.73365947744698,-1.47798331115705,-0.412864791012652,0.163789716587566 -0.714000000000001,0.548942255854684,0.887645221067672,1.58523316481633,1.73371734210856,-1.47780816384249,-0.413990876687015,0.164299563320064 -0.716000000000001,0.550254059705993,0.887348316562356,1.58502854082976,1.73377260142713,-1.47763384889577,-0.415115939251818,0.16480960263989 -0.718000000000001,0.551568184221004,0.887051868033737,1.58482216510729,1.73382525533145,-1.47746036879584,-0.416239975104821,0.165319835708274 -0.720000000000001,0.552884626678192,0.886755878458723,1.5846140366121,1.73387530375352,-1.47728772602725,-0.417362980641844,0.165830263691691 -0.722000000000001,0.554203384339596,0.886460350824033,1.58440415432997,1.73392274662865,-1.47711592308013,-0.418484952256834,0.166340887761837 -0.724000000000001,0.555524454450804,0.886165288126131,1.58419251726939,1.73396758389542,-1.47694496245012,-0.419605886341925,0.166851709095599 -0.726000000000001,0.556847834240929,0.885870693371153,1.58397912446164,1.73400981549571,-1.47677484663834,-0.420725779287503,0.167362728875035 -0.728000000000001,0.558173520922593,0.885576569574838,1.58376397496086,1.73404944137472,-1.47660557815134,-0.421844627482264,0.16787394828734 -0.730000000000001,0.559501511691904,0.885282919762457,1.58354706784411,1.73408646148093,-1.47643715950105,-0.422962427313284,0.168385368524824 -0.732000000000001,0.560831803728442,0.88498974696874,1.58332840221145,1.73412087576615,-1.47626959320474,-0.424079175166072,0.168896990784883 -0.734000000000001,0.562164394195236,0.884697054237806,1.58310797718603,1.73415268418549,-1.47610288178497,-0.42519486742464,0.169408816269966 -0.736000000000001,0.563499280238748,0.884404844623089,1.58288579191413,1.73418188669737,-1.47593702776951,-0.426309500471559,0.169920846187549 -0.738000000000001,0.564836458988852,0.884113121187264,1.58266184556526,1.73420848326357,-1.47577203369137,-0.427423070688025,0.170433081750106 -0.740000000000001,0.566175927558821,0.883821887002171,1.5824361373322,1.73423247384916,-1.47560790208866,-0.42853557445392,0.170945524175074 -0.742000000000001,0.567517683045306,0.883531145148745,1.58220866643111,1.73425385842254,-1.47544463550459,-0.429647008147871,0.171458174684822 -0.744000000000001,0.56886172252832,0.883240898716936,1.58197943210157,1.73427263695546,-1.47528223648741,-0.430757368147312,0.171971034506622 -0.746000000000001,0.570208043071218,0.882951150805634,1.58174843360665,1.73428880942298,-1.47512070759036,-0.431866650828549,0.172484104872614 -0.748000000000001,0.571556641720685,0.882661904522594,1.58151567023301,1.73430237580353,-1.47496005137162,-0.432974852566815,0.172997387019772 -0.750000000000001,0.572907515506717,0.882373162984358,1.58128114129091,1.73431333607884,-1.47480027039421,-0.434081969736338,0.173510882189869 -0.752000000000001,0.574260661442607,0.882084929316175,1.58104484611434,1.734321690234,-1.47464136722603,-0.435187998710394,0.174024591629444 -0.754000000000001,0.575616076524924,0.881797206651926,1.58080678406105,1.73432743825743,-1.47448334443972,-0.436292935861373,0.174538516589767 -0.756000000000001,0.576973757733505,0.881509998134041,1.58056695451263,1.73433058014092,-1.47432620461263,-0.437396777560839,0.175052658326798 -0.758000000000001,0.578333702031433,0.881223306913424,1.58032535687455,1.73433111587956,-1.4741699503268,-0.438499520179586,0.175567018101155 -0.760000000000001,0.579695906365026,0.880937136149367,1.58008199057628,1.73432904547181,-1.47401458416884,-0.439601160087703,0.176081597178075 -0.762000000000001,0.581060367663824,0.880651489009475,1.57983685507131,1.73432436891946,-1.47386010872993,-0.440701693654633,0.176596396827374 -0.764000000000001,0.582427082840568,0.88036636866958,1.5795899498372,1.73431708622767,-1.47370652660573,-0.441801117249229,0.177111418323409 -0.766000000000001,0.583796048791196,0.880081778313663,1.5793412743757,1.73430719740491,-1.47355384039632,-0.442899427239818,0.177626662945039 -0.768000000000001,0.585167262394818,0.879797721133767,1.57909082821277,1.73429470246301,-1.47340205270617,-0.443996619994258,0.178142131975585 -0.770000000000001,0.586540720513715,0.879514200329917,1.57883861089866,1.73427960141713,-1.47325116614405,-0.445092691879997,0.178657826702786 -0.772000000000001,0.587916419993315,0.879231219110036,1.57858462200795,1.73426189428578,-1.47310118332296,-0.446187639264133,0.179173748418762 -0.774000000000001,0.589294357662189,0.878948780689859,1.57832886113965,1.73424158109082,-1.47295210686012,-0.447281458513473,0.179689898419965 -0.776000000000001,0.590674530332033,0.878666888292853,1.57807132791721,1.73421866185741,-1.47280393937686,-0.44837414599459,0.180206278007144 -0.778000000000001,0.592056934797661,0.878385545150123,1.57781202198864,1.73419313661408,-1.47265668349856,-0.449465698073883,0.180722888485294 -0.780000000000001,0.593441567836988,0.878104754500334,1.5775509430265,1.73416500539268,-1.47251034185463,-0.450556111117633,0.181239731163617 -0.782000000000001,0.594828426211026,0.877824519589624,1.57728809072802,1.73413426822839,-1.47236491707839,-0.451645381492064,0.181756807355473 -0.784000000000001,0.596217506663866,0.87754484367151,1.57702346481513,1.7341009251597,-1.47222041180703,-0.452733505563398,0.182274118378338 -0.786000000000001,0.597608805922672,0.87726573000681,1.57675706503451,1.73406497622846,-1.47207682868156,-0.453820479697916,0.182791665553752 -0.788000000000001,0.599002320697671,0.876987181863549,1.57648889115765,1.73402642147981,-1.47193417034672,-0.45490630026201,0.183309450207279 -0.790000000000001,0.600398047682141,0.876709202516871,1.57621894298094,1.73398526096222,-1.47179243945091,-0.455990963622245,0.183827473668453 -0.792000000000001,0.601795983552404,0.876431795248953,1.57594722032567,1.73394149472746,-1.47165163864617,-0.457074466145414,0.184345737270735 -0.794000000000001,0.603196124967817,0.876154963348912,1.57567372303813,1.73389512283062,-1.47151177058802,-0.458156804198595,0.184864242351456 -0.796000000000001,0.604598468570761,0.875878710112718,1.57539845098963,1.73384614533009,-1.4713728379355,-0.459237974149207,0.185382990251778 -0.798000000000001,0.606003010986638,0.875603038843101,1.57512140407657,1.73379456228755,-1.471234843351,-0.460317972365066,0.185901982316635 -0.800000000000001,0.607409748823857,0.875327952849463,1.57484258222051,1.73374037376799,-1.47109778950029,-0.461396795214443,0.186421219894683 -0.802000000000001,0.608818678673833,0.875053455447783,1.57456198536819,1.73368357983966,-1.47096167905234,-0.462474439066119,0.186940704338253 -0.804000000000001,0.610229797110975,0.874779549960529,1.57427961349158,1.73362418057413,-1.47082651467933,-0.463550900289437,0.187460437003294 -0.806000000000001,0.611643100692684,0.874506239716561,1.57399546658798,1.73356217604621,-1.47069229905656,-0.464626175254363,0.187980419249321 -0.808000000000001,0.613058585959342,0.874233528051042,1.57370954468001,1.73349756633399,-1.47055903486235,-0.465700260331539,0.188500652439361 -0.810000000000001,0.614476249434314,0.873961418305345,1.57342184781567,1.73343035151884,-1.47042672477799,-0.466773151892335,0.189021137939902 -0.812000000000001,0.615896087623933,0.873689913826956,1.57313237606844,1.73336053168536,-1.47029537148767,-0.467844846308908,0.189541877120831 -0.814000000000001,0.617318097017503,0.873419017969381,1.57284112953726,1.73328810692141,-1.47016497767838,-0.468915339954255,0.190062871355384 -0.816000000000001,0.618742274087295,0.873148734092055,1.57254810834662,1.73321307731809,-1.47003554603984,-0.469984629202264,0.190584122020088 -0.818000000000001,0.620168615288535,0.872879065560241,1.57225331264657,1.73313544296973,-1.46990707926447,-0.471052710427774,0.191105630494703 -0.820000000000001,0.62159711705941,0.872610015744938,1.57195674261282,1.7330552039739,-1.46977958004723,-0.472119580006623,0.191627398162166 -0.822000000000001,0.623027775821061,0.872341588022787,1.57165839844673,1.73297236043136,-1.46965305108561,-0.473185234315705,0.192149426408529 -0.824000000000001,0.624460587977579,0.872073785775971,1.57135828037537,1.7328869124461,-1.46952749507954,-0.47424966973302,0.192671716622905 -0.826000000000001,0.625895549916005,0.871806612392117,1.5710563886516,1.7327988601253,-1.46940291473127,-0.475312882637732,0.193194270197405 -0.828000000000001,0.627332658006332,0.871540071264207,1.57075272355404,1.73270820357932,-1.46927931274535,-0.476374869410216,0.193717088527078 -0.830000000000001,0.628771908601495,0.871274165790472,1.57044728538719,1.73261494292173,-1.46915669182849,-0.477435626432114,0.19424017300985 -0.832000000000001,0.630213298037378,0.871008899374296,1.57014007448141,1.73251907826924,-1.46903505468952,-0.478495150086386,0.194763525046463 -0.834000000000001,0.631656822632812,0.870744275424124,1.56983109119298,1.73242060974174,-1.4689144040393,-0.479553436757363,0.195287146040414 -0.836000000000001,0.633102478689575,0.870480297353353,1.56952033590416,1.73231953746225,-1.46879474259064,-0.480610482830795,0.195811037397887 -0.838000000000001,0.634550262492392,0.870216968580243,1.56920780902321,1.73221586155694,-1.46867607305818,-0.48166628469391,0.196335200527698 -0.840000000000001,0.636000170308938,0.869954292527811,1.56889351098442,1.73210958215513,-1.46855839815836,-0.482720838735454,0.196859636841221 -0.842000000000001,0.637452198389838,0.869692272623734,1.56857744224815,1.73200069938921,-1.46844172060932,-0.483774141345754,0.197384347752331 -0.844000000000001,0.638906342968672,0.869430912300249,1.56825960330088,1.73188921339473,-1.46832604313078,-0.484826188916759,0.197909334677334 -0.846000000000001,0.640362600261975,0.869170214994053,1.56793999465524,1.73177512431028,-1.468211368444,-0.485876977842096,0.198434599034903 -0.848000000000001,0.641820966469242,0.868910184146201,1.56761861685004,1.73165843227758,-1.46809769927166,-0.486926504517117,0.19896014224601 -0.850000000000001,0.643281437772933,0.868650823202005,1.56729547045029,1.73153913744139,-1.4679850383378,-0.487974765338951,0.19948596573386 -0.852000000000001,0.644744010338476,0.868392135610934,1.56697055604728,1.73141723994954,-1.46787338836771,-0.48902175670655,0.200012070923822 -0.854000000000001,0.64620868031427,0.868134124826513,1.56664387425853,1.73129273995289,-1.46776275208785,-0.490067475020745,0.200538459243361 -0.856000000000001,0.647675443831696,0.867876794306219,1.56631542572793,1.73116563760535,-1.46765313222577,-0.491111916684285,0.201065132121969 -0.858000000000001,0.649144297005118,0.867620147511379,1.56598521112567,1.73103593306384,-1.46754453151001,-0.492155078101897,0.201592090991095 -0.860000000000001,0.65061523593189,0.867364187907071,1.56565323114831,1.73090362648827,-1.46743695267,-0.493196955680325,0.202119337284073 -0.862000000000001,0.652088256692368,0.867108918962015,1.56531948651884,1.73076871804157,-1.46733039843599,-0.494237545828382,0.202646872436052 -0.864000000000001,0.653563355349911,0.866854344148476,1.56498397798665,1.73063120788962,-1.46722487153896,-0.495276844957,0.203174697883929 -0.866000000000001,0.655040527950894,0.866600466942159,1.5646467063276,1.73049109620127,-1.46712037471049,-0.496314849479273,0.203702815066266 -0.868000000000001,0.656519770524717,0.866347290822102,1.56430767234401,1.73034838314831,-1.46701691068271,-0.497351555810508,0.20423122542323 -0.870000000000001,0.658001079083809,0.866094819270579,1.56396687686473,1.73020306890547,-1.46691448218819,-0.498386960368268,0.20475993039651 -0.872000000000001,0.659484449623645,0.865843055772988,1.56362432074513,1.73005515365041,-1.46681309195986,-0.499421059572424,0.205288931429246 -0.874000000000001,0.660969878122753,0.865592003817755,1.56328000486712,1.72990463756365,-1.46671274273087,-0.500453849845198,0.205818229965956 -0.876000000000001,0.662457360542723,0.865341666896222,1.56293393013922,1.72975152082862,-1.46661343723456,-0.501485327611209,0.20634782745246 -0.878000000000001,0.663946892828224,0.865092048502547,1.5625860974965,1.72959580363163,-1.4665151782043,-0.502515489297519,0.206877725335803 -0.880000000000001,0.665438470907013,0.864843152133601,1.56223650790071,1.72943748616182,-1.46641796837345,-0.50354433133368,0.207407925064179 -0.882000000000001,0.666932090689948,0.864594981288854,1.56188516234017,1.72927656861116,-1.46632181047522,-0.504571850151778,0.207938428086857 -0.884000000000001,0.668427748071,0.864347539470282,1.56153206182992,1.72911305117447,-1.46622670724262,-0.50559804218648,0.208469235854099 -0.886000000000001,0.669925438927274,0.864100830182251,1.56117720741164,1.72894693404934,-1.46613266140828,-0.506622903875073,0.209000349817084 -0.888000000000001,0.671425159119014,0.863854856931418,1.56082060015371,1.72877821743615,-1.46603967570445,-0.507646431657516,0.209531771427833 -0.890000000000001,0.672926904489625,0.863609623226621,1.56046224115124,1.72860690153805,-1.46594775286284,-0.508668621976478,0.210063502139123 -0.892000000000001,0.674430670865689,0.863365132578778,1.56010213152603,1.72843298656094,-1.46585689561452,-0.509689471277386,0.210595543404412 -0.894000000000001,0.675936454056978,0.863121388500776,1.55974027242666,1.72825647271344,-1.46576710668985,-0.510708976008465,0.211127896677758 -0.896000000000001,0.677444249856473,0.862878394507367,1.55937666502844,1.72807736020688,-1.46567838881836,-0.511727132620783,0.21166056341374 -0.898000000000001,0.678954054040382,0.862636154115063,1.55901131053346,1.7278956492553,-1.46559074472864,-0.512743937568295,0.212193545067371 -0.900000000000001,0.68046586236816,0.862394670842026,1.55864421017057,1.72771134007538,-1.46550417714826,-0.513759387307882,0.212726843094021 -0.902000000000001,0.681979670582525,0.862153948207967,1.55827536519544,1.72752443288647,-1.46541868880367,-0.514773478299397,0.213260458949336 -0.904000000000001,0.68349547440948,0.861913989734032,1.55790477689051,1.72733492791056,-1.46533428242004,-0.515786207005705,0.213794394089151 -0.906000000000001,0.685013269558333,0.861674798942701,1.55753244656506,1.72714282537223,-1.46525096072125,-0.516797569892724,0.214328649969407 -0.908000000000001,0.686533051721719,0.861436379357682,1.55715837555517,1.72694812549868,-1.4651687264297,-0.51780756342947,0.21486322804607 -0.910000000000001,0.68805481657562,0.861198734503797,1.55678256522375,1.72675082851966,-1.46508758226627,-0.518816184088091,0.215398129775046 -0.912000000000001,0.689578559779388,0.860961867906883,1.55640501696055,1.72655093466749,-1.46500753095015,-0.519823428343917,0.215933356612093 -0.914000000000001,0.691104276975769,0.86072578309368,1.55602573218213,1.72634844417699,-1.4649285751988,-0.520829292675493,0.216468910012739 -0.916000000000001,0.692631963790926,0.860490483591726,1.55564471233194,1.72614335728551,-1.46485071772781,-0.521833773564622,0.217004791432196 -0.918000000000001,0.694161615834466,0.860255972929249,1.55526195888024,1.7259356742329,-1.46477396125078,-0.522836867496405,0.217541002325271 -0.920000000000001,0.695693228699459,0.86002225463506,1.55487747332414,1.72572539526144,-1.46469830847925,-0.523838570959278,0.218077544146281 -0.922000000000001,0.697226797962472,0.859789332238447,1.55449125718762,1.72551252061588,-1.46462376212257,-0.524838880445057,0.218614418348965 -0.924000000000001,0.698762319183589,0.859557209269066,1.5541033120215,1.72529705054338,-1.46455032488778,-0.525837792448971,0.219151626386398 -0.926000000000001,0.70029978790644,0.859325889256835,1.55371363940345,1.72507898529349,-1.46447799947954,-0.526835303469702,0.219689169710901 -0.928000000000001,0.701839199658234,0.859095375731825,1.553322240938,1.72485832511816,-1.46440678859996,-0.527831410009425,0.220227049773952 -0.930000000000001,0.703380549949778,0.858865672224156,1.55292911825653,1.72463507027167,-1.46433669494857,-0.528826108573845,0.220765268026098 -0.932000000000001,0.704923834275515,0.858636782263888,1.55253427301724,1.72440922101062,-1.46426772122215,-0.529819395672234,0.221303825916866 -0.934000000000001,0.706469048113551,0.858408709380912,1.5521377069052,1.72418077759394,-1.46419987011463,-0.53081126781747,0.221842724894669 -0.936000000000001,0.708016186925684,0.858181457104846,1.5517394216323,1.72394974028283,-1.464133144317,-0.531801721526072,0.222381966406721 -0.938000000000001,0.709565246157439,0.857955028964927,1.55133941893729,1.72371610934073,-1.46406754651717,-0.532790753318239,0.222921551898944 -0.940000000000001,0.711116221238098,0.857729428489903,1.55093770058571,1.72347988503334,-1.4640030793999,-0.533778359717884,0.223461482815873 -0.942000000000001,0.712669107580733,0.857504659207926,1.55053426836993,1.72324106762854,-1.46393974564663,-0.534764537252671,0.224001760600571 -0.944000000000001,0.714223900582241,0.857280724646449,1.55012912410912,1.72299965739641,-1.46387754793543,-0.535749282454052,0.224542386694531 -0.946000000000001,0.715780595623377,0.857057628332111,1.54972226964928,1.72275565460918,-1.46381648894084,-0.536732591857301,0.225083362537588 -0.948000000000001,0.717339188068792,0.85683537379064,1.54931370686316,1.72250905954121,-1.46375657133378,-0.53771446200155,0.22562468956782 -0.950000000000001,0.718899673267065,0.85661396454674,1.54890343765031,1.72225987246896,-1.46369779778142,-0.538694889429823,0.226166369221463 -0.952000000000001,0.720462046550743,0.856393404123984,1.54849146393702,1.72200809367097,-1.46364017094711,-0.539673870689072,0.226708402932809 -0.954000000000001,0.722026303236377,0.856173696044713,1.54807778767637,1.72175372342785,-1.4635836934902,-0.540651402330209,0.227250792134119 -0.956000000000001,0.72359243862456,0.855954843829926,1.54766241084813,1.72149676202222,-1.46352836806596,-0.541627480908142,0.227793538255521 -0.958000000000001,0.725160447999967,0.855736850999173,1.54724533545882,1.72123720973869,-1.46347419732549,-0.542602102981809,0.228336642724924 -0.960000000000001,0.726730326631394,0.855519721070452,1.54682656354166,1.72097506686386,-1.46342118391554,-0.543575265114208,0.228880106967914 -0.962000000000001,0.7283020697718,0.8553034575601,1.54640609715655,1.72071033368628,-1.46336933047848,-0.544546963872433,0.229423932407666 -0.964000000000001,0.729875672658345,0.855088063982692,1.54598393839006,1.72044301049639,-1.46331863965209,-0.545517195827706,0.229968120464842 -0.966000000000001,0.731451130512437,0.85487354385093,1.54556008935541,1.72017309758653,-1.46326911406952,-0.546485957555409,0.230512672557499 -0.968000000000001,0.733028438539769,0.854659900675541,1.54513455219245,1.71990059525092,-1.46322075635914,-0.547453245635117,0.23105759010099 -0.970000000000001,0.734607591930368,0.854447137965172,1.54470732906763,1.7196255037856,-1.46317356914442,-0.548419056650628,0.231602874507869 -0.972000000000001,0.736188585858637,0.854235259226282,1.544278422174,1.71934782348839,-1.46312755504384,-0.549383387189995,0.232148527187791 -0.974000000000001,0.7377714154834,0.854024267963042,1.54384783373116,1.71906755465893,-1.46308271667073,-0.550346233845558,0.23269454954742 -0.976000000000001,0.739356075947947,0.853814167677228,1.54341556598525,1.71878469759857,-1.46303905663319,-0.551307593213976,0.233240942990322 -0.978000000000001,0.740942562380084,0.853604961868114,1.54298162120891,1.71849925261039,-1.46299657753396,-0.552267461896253,0.233787708916878 -0.980000000000001,0.74253086989218,0.853396654032375,1.5425460017013,1.71821121999916,-1.46295528197029,-0.553225836497772,0.234334848724176 -0.982000000000001,0.744120993581211,0.853189247663977,1.542108709788,1.7179206000713,-1.46291517253385,-0.554182713628324,0.234882363805919 -0.984000000000001,0.745712928528814,0.852982746254073,1.54166974782104,1.71762739313486,-1.46287625181059,-0.555138089902139,0.235430255552321 -0.986000000000001,0.747306669801336,0.852777153290908,1.54122911817886,1.71733159949949,-1.4628385223806,-0.556091961937912,0.235978525350012 -0.988000000000001,0.748902212449883,0.852572472259706,1.54078682326625,1.7170332194764,-1.46280198681805,-0.557044326358833,0.236527174581937 -0.990000000000001,0.750499551510374,0.852368706642572,1.54034286551436,1.71673225337834,-1.46276664769103,-0.557995179792617,0.237076204627253 -0.992000000000001,0.752098682003593,0.852165859918391,1.53989724738063,1.71642870151956,-1.46273250756142,-0.558944518871534,0.237625616861236 -0.994000000000001,0.753699598935238,0.851963935562724,1.53944997134879,1.71612256421578,-1.46269956898482,-0.55989234023243,0.238175412655175 -0.996000000000001,0.755302297295983,0.851762937047704,1.53900103992882,1.71581384178417,-1.46266783451038,-0.560838640516764,0.238725593376274 -0.998000000000001,0.756906772061525,0.85156286784194,1.53855045565687,1.7155025345433,-1.4626373066807,-0.561783416370628,0.239276160387549 -1,0.758513018192645,0.85136373141041,1.5380982210953,1.71518864281313,-1.46260798803172,-0.562726664444779,0.239827115047732 -1.002,0.760121030635263,0.851165531214365,1.53764433883257,1.71487216691494,-1.46257988109258,-0.56366838139466,0.240378458711165 -1.004,0.76172919481938,0.850968474709213,1.53718927241537,1.71455342542013,-1.46255302535324,-0.56460762397024,0.240929648748077 -1.006,0.763334277846886,0.850772968998806,1.53673395490576,1.71423306805792,-1.46252749178502,-0.565542516737705,0.241479597042579 -1.008,0.764933032822655,0.850579414161899,1.53627933296572,1.71391176514017,-1.46250334251824,-0.566471196624796,0.242027213011431 -1.01,0.766522199073563,0.850388203246982,1.53582636669269,1.71359020737919,-1.46248063087678,-0.567391812881454,0.242571403604126 -1.012,0.768098502436208,0.850199722302999,1.53537602938591,1.71326910560232,-1.4624594014561,-0.568302526976553,0.243111073314558 -1.014,0.769658655611943,0.850014350446767,1.53492930724412,1.71294919036331,-1.46243969024533,-0.569201512429837,0.243645124204465 -1.016,0.771199358587896,0.849832459967934,1.53448719899545,1.71263121145076,-1.46242152479363,-0.570086954578155,0.244172455938864 -1.018,0.772717299122628,0.849654416472253,1.53405071545996,1.71231593729341,-1.46240492442135,-0.570957050275173,0.24469196583363 -1.02,0.774209153295166,0.849480579063933,1.53362087904565,1.71200415426268,-1.46238990047625,-0.571810007523754,0.245202548915392 -1.022,0.775671586116195,0.849311300567766,1.53319872317847,1.71169666587236,-1.46237645663525,-0.572644045040266,0.245703097993917 -1.024,0.777101252200228,0.849146927791694,1.53278529166706,1.71139429187555,-1.46236458925177,-0.573457391750115,0.246192503747102 -1.026,0.778494796497713,0.848987801830413,1.53238163800274,1.711097867259,-1.46235428774923,-0.574248286213881,0.246669654818711 -1.028,0.779848855086078,0.848834258410554,1.53198882459542,1.71080824113483,-1.46234553506075,-0.575014975983496,0.247133437928979 -1.03,0.78116005601885,0.848686628277909,1.53160792194592,1.71052627552974,-1.4623383081153,-0.575755716887987,0.247582737998157 -1.032,0.782425020232096,0.848545237627096,1.53124000775516,1.71025284407179,-1.46233257837058,-0.576468772248378,0.248016438283103 -1.034,0.783640362507572,0.848410408573973,1.53088616597073,1.70998883057466,-1.46232831239258,-0.577152412021443,0.248433420526975 -1.036,0.7848026924921,0.848282459671025,1.53054748577114,1.70973512751965,-1.46232547248219,-0.577804911872101,0.248832565122065 -1.038,0.785908615772882,0.848161706465851,1.53022506048805,1.70949263443516,-1.46232401734862,-0.57842455217434,0.24921275128582 -1.04,0.78695473500859,0.848048462102792,1.52991998646666,1.70926225617395,-1.4623239028299,-0.579009616940665,0.249572857250063 -1.042,0.787937651116305,0.847943037967608,1.52963336186444,1.70904490108794,-1.46232508266035,-0.579558392680183,0.249911760463395 -1.044,0.788853964514537,0.847845744375028,1.52936628538811,1.70884147910071,-1.46232750928497,-0.580069167185563,0.250228337806779 -1.046,0.789700276422797,0.847756891298874,1.52911985496879,1.70865289967754,-1.4623311347206,-0.580540228249231,0.250521465822255 -1.048,0.790474531515875,0.847676635331512,1.52889476407133,1.70847977114713,-1.46233589722404,-0.580970607186103,0.250790469763278 -1.05,0.791179477875349,0.847604587113496,1.52869025955989,1.70832162230551,-1.4623416888769,-0.581361990782562,0.251036280427551 -1.052,0.791818305861189,0.84754031645905,1.52850544503333,1.7081778637077,-1.46234840719383,-0.58171628864015,0.251259974198561 -1.054,0.792394220156922,0.847483400874214,1.52833940991484,1.70804788416909,-1.4623559591194,-0.582035396968814,0.251462630091183 -1.056,0.792910438356313,0.847433424873286,1.52819123084624,1.70793105283471,-1.46236426018755,-0.58232119983247,0.251645329536891 -1.058,0.79337018963127,0.847389979322948,1.52805997300734,1.70782672114435,-1.46237323371983,-0.582575570339205,0.25180915617816 -1.06,0.793776713478906,0.847352660815312,1.52794469136149,1.7077342246939,-1.46238281006298,-0.582800371774751,0.251955195672332 -1.062,0.794133258545905,0.847321071071069,1.52784443182793,1.70765288499264,-1.46239292586635,-0.582997458678001,0.252084535505199 -1.064,0.7944430815285,0.847294816373726,1.52775823238202,1.7075820111171,-1.46240352339968,-0.583168677857466,0.252198264814529 -1.066,0.794709446146578,0.847273507035861,1.52768512408382,1.7075209012611,-1.46241454991172,-0.583315869347713,0.252297474223733 -1.068,0.794935621134677,0.847256582178173,1.52762398533234,1.70746886022957,-1.46242568999227,-0.583440857634691,0.252383050855421 -1.07,0.795124876564734,0.847242800968848,1.52757311565017,1.70742524724721,-1.46243558784952,-0.583545424258445,0.252455082111308 -1.072,0.79528048963571,0.847231324436233,1.52753114513836,1.70738937777344,-1.4624435007616,-0.583631368484945,0.252514124239224 -1.074,0.795405743378718,0.847221698745122,1.52749702203415,1.70736052646407,-1.46244927420063,-0.583700507250131,0.25256118338822 -1.076,0.795503924901395,0.847213705083353,1.5274698878575,1.70733794208708,-1.46245311274696,-0.583754667567952,0.252597540065297 -1.078,0.795578324099649,0.847207244726261,1.5274489817838,1.707320858976,-1.46245540452512,-0.583795680766422,0.252624614669369 -1.08,0.795632232709233,0.847202255186311,1.52743357092267,1.70730850546845,-1.46245659303338,-0.58382537835427,0.252643869376861 -1.082,0.795668943607795,0.847198653344747,1.52742290313086,1.70730010975261,-1.46245709017542,-0.583845589308781,0.252656741614279 -1.084,0.795691750307612,0.847196301672216,1.52741617913159,1.70729490350484,-1.46245722458809,-0.583858138579516,0.252664604579035 -1.086,0.79570394660047,0.847194993819697,1.52741254083593,1.70729212367106,-1.4624572196004,-0.583864846606945,0.25266875045968 -1.088,0.79570882633024,0.84719445593062,1.52741107280446,1.70729101272849,-1.4624571952486,-0.583867529655005,0.252670392078664 -1.09,0.795709683276719,0.847194360012352,1.52741081376797,1.70729081775968,-1.4624571887431,-0.583868000753386,0.252670678660393 +0.002,0.249999999999913,0.9999993223229,1.57079700176132,1.57079545652522,-1.5707956491178,8.70269716351503e-07,6.7496635452126e-07 +0.004,0.250000000005317,0.999996609828846,1.57079970340221,1.57079197314014,-1.57079293662376,4.35364647000001e-06,3.37661479353802e-06 +0.006,0.250000000069342,0.999990505439807,1.57080578333657,1.57078413376369,-1.57078683223476,1.21929275623507e-05,9.45663670180122e-06 +0.008,0.250000000390752,0.999979652177805,1.57081659299971,1.57077019531257,-1.57077597897283,2.6130902489352e-05,2.02667378256507e-05 +0.01,0.250000001469771,0.999962693225846,1.57083348349264,1.5707484142151,-1.57075902002099,4.79104038692621e-05,3.71586995448357e-05 +0.012,0.250000004321685,0.999938272011552,1.57085780537543,1.57071704602619,-1.57073459880692,7.92743770766243e-05,6.14844627211257e-05 +0.014,0.250000010748276,0.999905032313492,1.5708909084047,1.57067434493832,-1.57070135910937,0.000121965968571637,9.45962341467464e-05 +0.016,0.250000023667198,0.999861618390179,1.57093414121488,1.57061856318858,-1.57065794518726,0.000177728632909719,0.000137846615630581 +0.018,0.250000047499444,0.999806675131712,1.57098885094334,1.57054795036174,-1.57060300193173,0.000248306258410532,0.000192588755778474 +0.02,0.250000088615133,0.999738848234052,1.5710563827991,1.57046075258932,-1.57053517504088,0.000335443311459109,0.000260176524546053 +0.022,0.250000155837912,0.999656784395864,1.57113807957493,1.57035521164458,-1.57045311121753,0.00044088499940409,0.000341964710668075 +0.024,0.250000261008343,0.999559131537894,1.5712352811025,1.57022956393351,-1.57035545839001,0.000566377452009496,0.000439309242097627 +0.026,0.250000419606772,0.999444539044817,1.57134932365028,1.57008203938163,-1.57024086595608,0.000713667921406858,0.000553567429621316 +0.028,0.250000651436237,0.999311658029475,1.57148153926362,1.5699108602166,-1.57010798505013,0.000884505000484163,0.000686098233853168 +0.03,0.250000981366146,0.99915914161943,1.57163325504652,1.56971423964657,-1.56995546883394,0.00108063885963709,0.000838262555850229 +0.032,0.250001440137529,0.99898564526571,1.57180579238442,1.56949038043402,-1.56978197281106,0.00130382150179617,0.00101142355163674 +0.034,0.250002065230851,0.998789827073657,1.57200046610725,1.56923747336512,-1.56958615516518,0.00155580703563159,0.00120694697097161 +0.036,0.250002901797504,0.998570348155707,1.57221858359183,1.56895369561439,-1.56936667712271,0.00183835196682487,0.00142620152074542 +0.038,0.250004003656267,0.99832587300597,1.57246144380258,1.56863720900437,-1.56912220333997,0.00215321550728441,0.00167055925344866 +0.04,0.250005434356188,0.998055069896422,1.57273033626954,1.56828615816021,-1.56885140231523,0.00250215990216965,0.00194139598121259 +0.042,0.250007268307558,0.997756611294517,1.57302654000216,1.56789866855889,-1.56855294682614,0.00288695077457718,0.00224009171598722 +0.044,0.250009591982804,0.997429174301983,1.57335132233768,1.5674728444727,-1.56822551439295,0.00330935748773114,0.00256803113648916 +0.046,0.250012500708199,0.99707199281458,1.5737053908275,1.56700748555696,-1.56786833945785,0.00377044136769867,0.00292605109072483 +0.048,0.250016083603827,0.996686281967905,1.57408748320265,1.56650395072498,-1.56748263781732,0.00426871405256183,0.00331301006055872 +0.05,0.250020430919184,0.996273434206366,1.5744961598244,1.5659638248122,-1.56706980271328,0.00480246004122252,0.00372759060550077 +0.052,0.250025633908619,0.995834842120738,1.57492998019918,1.56538869142435,-1.56663122768133,0.00536996406205006,0.00416847570485169 +0.054,0.250031783692519,0.995371897991795,1.57538750408525,1.56478013501692,-1.56616830610017,0.00596951069649039,0.00463434831107265 +0.056,0.250038970168209,0.99488599335771,1.57586729254991,1.56413974287151,-1.56568243076169,0.00659938402243554,0.00512389092294426 +0.058,0.25004728096993,0.9943785186053,1.57636790897784,1.56346910696895,-1.56517499346128,0.00725786727738968,0.00563578517830169 +0.06,0.250056800477393,0.993850862585161,1.57688792003087,1.56276982575956,-1.56464738460857,0.00794324254144105,0.00616871146619708 +0.062,0.250067608872629,0.993304412250724,1.57742589655942,1.56204350583053,-1.56410099285821,0.00865379044002418,0.00672134855840227 +0.064,0.25007978124502,0.992740552321238,1.5779804144656,1.56129176347049,-1.56353720476104,0.00938778986643393,0.00729237326022261 +0.066,0.250093386744532,0.992160664968665,1.57855005551796,1.56051622613134,-1.56295740443543,0.0101435177240324,0.00788046008064738 +0.068,0.250108487783359,0.99156612952845,1.57913340811758,1.55971853378741,-1.56236297325902,0.010919248688072,0.00848428092191383 +0.07,0.250125139286311,0.99095832223412,1.57972906801527,1.55890034019194,-1.56175528958103,0.011713254987041,0.00910250478861007 +0.0720000000000001,0.250143387990412,0.990338615975651,1.58033563897934,1.55806331403094,-1.56113572845519,0.0125238062034277,0.00973379751648673 +0.0740000000000001,0.250163271794316,0.989708380081501,1.58095173341353,1.55720913997425,-1.56050566139353,0.0133491690937844,0.0103768215211895 +0.0760000000000001,0.250184819158247,0.989068980124236,1.58157597292431,1.556339519624,-1.55986645614138,0.0141876074279683,0.0110302355671626 +0.0780000000000001,0.250208048555296,0.988421777749621,1.58220698883701,1.55545617236019,-1.55921947647365,0.0150373818474275,0.0116926945570092 +0.0800000000000001,0.250232967974984,0.987768130529062,1.58284342265983,1.55456083608344,-1.55856608201271,0.015896749742399,0.0123628493416266 +0.0820000000000001,0.250259574480117,0.987109391835274,1.58348392649512,1.55365526785475,-1.55790762806833,0.0167639651478828,0.0130393465514625 +0.0840000000000001,0.250287853818022,0.986446910741014,1.58412716339672,1.55274124443218,-1.55724546549966,0.017637278658258,0.0137208284492661 +0.0860000000000001,0.250317780087308,0.985782031940758,1.58477180767277,1.55182056270426,-1.55658094059987,0.0185149373604086,0.0144059328047278 +0.0880000000000001,0.250349315461412,0.985116095695143,1.58541654513265,1.55089504002003,-1.55591539500352,0.0193951847852332,0.0150932927914228 +0.0900000000000001,0.2503824125247,0.984450386239577,1.58606012312441,1.54996644252518,-1.55525011409184,0.0202763290998467,0.0157815902117251 +0.0920000000000001,0.250417060457035,0.983785250774172,1.58670220080629,1.54903525108903,-1.55458544658045,0.0211579108645194,0.0164704682938852 +0.0940000000000001,0.250453264050706,0.983120688507673,1.58734277476063,1.54810146488693,-1.5539213933712,0.0220399298632816,0.0171599288281249 +0.0960000000000001,0.250491028110505,0.982456698651251,1.58798184157159,1.54716508307865,-1.55325795536161,0.0229223858895967,0.0178499736070507 +0.0980000000000001,0.250530357453793,0.981793280418534,1.58861939782517,1.54622610480824,-1.55259513344485,0.0238052787463989,0.0185406044257214 +0.1,0.250571256910563,0.981130433025651,1.58925544010919,1.54528452920397,-1.55193292850982,0.0246886082461314,0.0192318230817154 +0.102,0.250613731323512,0.980468155691259,1.5898899650133,1.54434035537826,-1.55127134144121,0.0255723742107852,0.0199236313751982 +0.104,0.250657785548104,0.97980644763659,1.59052296912896,1.54339358242756,-1.55061037311953,0.0264565764719367,0.020616031108991 +0.106,0.250703424452646,0.979145308085482,1.59115444904944,1.5424442094323,-1.5499500244212,0.0273412148707874,0.0213090240886387 +0.108,0.25075065291835,0.97848473626442,1.5917844013698,1.54149223545675,-1.54929029621858,0.0282262892582018,0.0220026121224789 +0.11,0.250799475839408,0.977824731402575,1.59241282268692,1.54053765954899,-1.54863118938001,0.0291117994947469,0.0226967970217108 +0.112,0.250849898123063,0.977165292731841,1.59303970959946,1.53958048074076,-1.54797270476992,0.0299977454507311,0.0233915806004647 +0.114,0.250901924689682,0.976506419486877,1.59366505870787,1.53862069804743,-1.54731484324884,0.0308841270062436,0.0240869646758713 +0.116,0.250955560472827,0.975848110905142,1.59428886661435,1.53765831046784,-1.54665760567347,0.031770944051194,0.0247829510681323 +0.118,0.251010810419331,0.975190366226941,1.5949111299229,1.53669331698427,-1.54600099289674,0.0326581964853515,0.0254795416005906 +0.12,0.251067679489371,0.974533184695462,1.59553184523928,1.53572571656231,-1.54534500576787,0.0335458842183851,0.0261767380998005 +0.122,0.251126172656548,0.973876565556816,1.596151009171,1.53475550815078,-1.54468964513243,0.0344340071699037,0.0268745423955994 +0.124,0.251186294907958,0.973220508060081,1.59676861832732,1.53378269068162,-1.54403491183238,0.0353225652694958,0.0275729563211785 +0.126,0.251248051244275,0.972565011457343,1.59738466931925,1.53280726306982,-1.54338080670617,0.0362115584567702,0.028271981713155 +0.128,0.251311446679825,0.971910075003735,1.59799915875953,1.53182922421328,-1.54272733058875,0.0371009866813968,0.0289716204116439 +0.13,0.251376486242669,0.971255697957483,1.59861208326261,1.53084857299276,-1.54207448431169,0.0379908499031465,0.0296718742603302 +0.132,0.25144317497468,0.970601879579948,1.59922343944471,1.52986530827177,-1.54142226870317,0.0388811480919331,0.0303727451065417 +0.134,0.251511517931629,0.969948619135666,1.59983322392371,1.52887942889643,-1.54077068458811,0.0397718812278536,0.0310742348013222 +0.136,0.251581520183261,0.969295915892398,1.60044143331923,1.52789093369542,-1.54011973278819,0.0406630493012304,0.0317763451995045 +0.138,0.251653186813383,0.968643769121165,1.60104806425257,1.52689982147987,-1.53946941412194,0.0415546523126517,0.0324790781597844 +0.14,0.251726522919947,0.967992178096301,1.60165311334674,1.52590609104323,-1.53881972940476,0.0424466902730143,0.0331824355447946 +0.142,0.251801533615132,0.96734114209549,1.6022565772264,1.52490974116118,-1.53817067944904,0.043339163203565,0.0338864192211791 +0.144,0.251878224025433,0.966690660399818,1.60285845251792,1.52391077059156,-1.53752226506419,0.0442320711359427,0.0345910310596681 +0.146,0.25195659929175,0.966040732293811,1.60345873584931,1.52290917807422,-1.53687448705672,0.0451254141122208,0.0352962729351529 +0.148,0.252036664569467,0.965391357065486,1.60405742385025,1.52190496233091,-1.53622734623029,0.0460191921849503,0.0360021467267612 +0.15,0.252118425028552,0.964742534006397,1.60465451315208,1.52089812206524,-1.53558084338578,0.0469134054172017,0.0367086543179334 +0.152,0.252201885853638,0.964094262411677,1.60525000038776,1.51988865596248,-1.53493497932138,0.0478080538826086,0.0374157975964982 +0.154,0.252287052244118,0.963446541580088,1.6058438821919,1.51887656268955,-1.53428975483262,0.0487031376654111,0.0381235784547495 +0.156,0.252373929414236,0.962799370814067,1.60643615520072,1.51786184089481,-1.53364517071248,0.0495986568604986,0.038831998789523 +0.158,0.252462522593178,0.962152749419776,1.60702681605207,1.51684448920805,-1.53300122775141,0.0504946115734544,0.0395410605022734 +0.16,0.252552837025169,0.961506676707143,1.6076158613854,1.51582450624029,-1.53235792673745,0.0513910019205988,0.0402507654991522 +0.162,0.252644877969564,0.960861151989918,1.60820328784178,1.51480189058373,-1.53171526845625,0.0522878280290339,0.0409611156910854 +0.164,0.252738650700946,0.960216174585717,1.60878909206383,1.5137766408116,-1.53107325369118,0.0531850900366878,0.0416721129938519 +0.166,0.252834160509221,0.959571743816071,1.60937327069578,1.51274875547809,-1.53043188322337,0.0540827880923594,0.0423837593281623 +0.168,0.252931412699718,0.958927859006475,1.60995582038344,1.51171823311817,-1.52979115783183,0.0549809223557633,0.0430960566197376 +0.17,0.253030412593285,0.95828451948644,1.61053673777416,1.51068507224755,-1.52915107829344,0.0558794929975749,0.0438090067993887 +0.172,0.25313116552639,0.957641724589539,1.61111601951686,1.50964927136249,-1.5285116453831,0.0567785001994763,0.0445226118030963 +0.174,0.253233676851224,0.956999473653459,1.61169366226201,1.50861082893974,-1.52787285987377,0.0576779441542014,0.0452368735720911 +0.176,0.253337951935798,0.956357766020052,1.6122696626616,1.50756974343639,-1.52723472253653,0.0585778250655826,0.0459517940529339 +0.178,0.25344399616405,0.955716601035382,1.61284401736916,1.50652601328977,-1.5265972341407,0.0594781431485964,0.0466673751975969 +0.18,0.253551814935947,0.955075978049783,1.61341672303974,1.5054796369173,-1.52596039545385,0.0603788986294108,0.0473836189635446 +0.182,0.25366141366759,0.954435896417903,1.61398777632989,1.50443061271642,-1.52532420724193,0.0612800917454314,0.0481005273138157 +0.184,0.253772797791321,0.953796355498761,1.61455717389767,1.50337893906439,-1.52468867026931,0.0621817227453489,0.0488181022171049 +0.186,0.25388597275583,0.953157354655793,1.61512491240264,1.50232461431826,-1.5240537852989,0.0630837918891866,0.0495363456478457 +0.188,0.254000944026261,0.952518893256914,1.61569098850581,1.50126763681466,-1.52341955309217,0.0639862994483482,0.0502552595862923 +0.19,0.254117717084327,0.951880970674558,1.61625539886969,1.50020800486973,-1.52278597440927,0.0648892457056655,0.0509748460186038 +0.192,0.254236297428412,0.951243586285743,1.61681814015825,1.49914571677896,-1.52215305000907,0.0657926309554471,0.0516951069369269 +0.194,0.25435669057369,0.950606739472115,1.6173792090369,1.49808077081711,-1.5215207806493,0.066696455503527,0.0524160443394802 +0.196,0.254478902052233,0.949970429620007,1.61793860217251,1.49701316523801,-1.52088916708656,0.0676007196673136,0.0531376602306385 +0.198,0.254602937413128,0.94933465612049,1.61849631623337,1.49594289827449,-1.52025821007644,0.068505423775839,0.0538599566210173 +0.2,0.25472880222259,0.948699418369431,1.6190523478892,1.49486996813822,-1.5196279103736,0.0694105681698086,0.0545829355275583 +0.202,0.254856502064078,0.948064715767539,1.61960669381115,1.49379437301959,-1.51899826873182,0.0703161532016514,0.0553065989736143 +0.204,0.254986042538413,0.947430547720431,1.62015935067175,1.49271611108757,-1.51836928590413,0.0712221792355698,0.0560309489890355 +0.206,0.255117429263894,0.946796913638677,1.62071031514495,1.49163518048958,-1.51774096264284,0.0721286466475909,0.0567559876102554 +0.208,0.255250667876422,0.946163812937863,1.62125958390606,1.49055157935135,-1.51711329969967,0.073035555825617,0.0574817168803781 +0.21,0.255385764029618,0.945531245038639,1.62180715363181,1.48946530577679,-1.51648629782579,0.0739429071694773,0.0582081388492645 +0.212,0.255522723394941,0.944899209366782,1.62235302100025,1.48837635784784,-1.51585995777195,0.0748507010909798,0.0589352555736206 +0.214,0.25566155166182,0.944267705353247,1.62289718269082,1.48728473362435,-1.51523428028851,0.0757589380139628,0.0596630691170848 +0.216,0.255802254537769,0.943636732434229,1.62343963538429,1.48619043114393,-1.51460926612557,0.0766676183743483,0.0603915815503161 +0.218,0.255944837748518,0.943006290051211,1.62398037576278,1.48509344842179,-1.51398491603305,0.0775767426201941,0.0611207949510835 +0.22,0.256089307038139,0.942376377651029,1.62451940050973,1.48399378345065,-1.51336123076075,0.0784863112117477,0.0618507114043543 +0.222,0.256235668169171,0.941746994685927,1.62505670630992,1.48289143420053,-1.51273821105846,0.0793963246214993,0.0625813330023838 +0.224,0.256383926922752,0.941118140613612,1.6255922898494,1.48178639861865,-1.51211585767603,0.0803067833342367,0.0633126618448056 +0.226,0.256534089098747,0.940489814897315,1.62612614781556,1.48067867462927,-1.5114941713635,0.081217687847099,0.0640447000387213 +0.228,0.256686160515882,0.939862017005847,1.62665827689705,1.47956826013355,-1.51087315287111,0.0821290386696321,0.0647774496987917 +0.23,0.256840147011874,0.939234746413658,1.62718867378383,1.47845515300939,-1.51025280294949,0.0830408363238436,0.0655109129473276 +0.232,0.256996054443569,0.938608002600895,1.6277173351671,1.47733935111128,-1.50963312234966,0.083953081344259,0.0662450919143818 +0.234,0.257153888687071,0.937981785053463,1.62824425773935,1.47622085227018,-1.50901411182317,0.0848657742779772,0.0669799887378405 +0.236,0.257313655637886,0.937356093263082,1.6287694381943,1.47509965429331,-1.50839577212218,0.085778915684728,0.0677156055635161 +0.238,0.257475361211054,0.936730926727345,1.62929287322692,1.47397575496404,-1.50777810399955,0.0866925061369285,0.0684519445452402 +0.24,0.257639011341291,0.936106284949783,1.62981455953341,1.47284915204173,-1.50716110820894,0.0876065462197407,0.0691890078449561 +0.242,0.257804611983128,0.935482167439918,1.63033449381119,1.47171984326157,-1.50654478550489,0.08852103653113,0.0699267976328133 +0.244,0.257972169111054,0.934858573713327,1.63085267275892,1.47058782633441,-1.50592913664292,0.0894359776819231,0.0706653160872612 +0.246,0.25814168871966,0.934235503291705,1.63136909307642,1.46945309894661,-1.50531416237963,0.0903513702958672,0.0714045653951436 +0.248,0.258313176823779,0.933612955702917,1.63188375146474,1.46831565875988,-1.5046998634728,0.0912672150096896,0.0721445477517936 +0.25,0.258486639458639,0.93299093048107,1.63239664462609,1.46717550341114,-1.50408624068146,0.0921835124731572,0.0728852653611296 +0.252,0.258662082680002,0.932369427166563,1.63290776926387,1.46603263051231,-1.50347329476603,0.0931002633491374,0.0736267204357502 +0.254,0.258839512564318,0.931748445306158,1.63341712208264,1.46488703765017,-1.50286102648835,0.0940174683136586,0.0743689151970312 +0.256,0.259018935208874,0.931127984453035,1.63392469978813,1.46373872238621,-1.50224943661187,0.0949351280559721,0.075111851875222 +0.258,0.259200356731944,0.930508044166855,1.63443049908719,1.46258768225644,-1.50163852590165,0.0958532432786137,0.0758555327095429 +0.26,0.259383783272942,0.929888624013825,1.63493451668783,1.46143391477123,-1.50102829512455,0.0967718146974664,0.0765999599482823 +0.262,0.259569220992575,0.929269723566758,1.63543674929918,1.46027741741514,-1.50041874504925,0.0976908430418237,0.0773451358488955 +0.264,0.259756676073002,0.928651342405134,1.6359371936315,1.45911818764673,-1.49980987644643,0.0986103290544527,0.0780910626781024 +0.266,0.259946154717986,0.928033480115166,1.63643584639614,1.45795622289843,-1.49920169008878,0.0995302734916586,0.0788377427119867 +0.268,0.260137663153057,0.92741613628986,1.63693270430557,1.45679152057631,-1.4985941867512,0.10045067712335,0.0795851782360957 +0.27,0.260331207625669,0.92679931052908,1.63742776407333,1.45562407805995,-1.49798736721082,0.101371540733102,0.0803333715455396 +0.272,0.260526794405361,0.926183002439611,1.63792102241408,1.45445389270224,-1.49738123224717,0.102292865118226,0.0810823249450918 +0.274,0.26072442978392,0.925567211635221,1.6384124760435,1.45328096182919,-1.49677578264222,0.103214651089832,0.0818320407492901 +0.276,0.260924120075547,0.924951937736728,1.6389021216784,1.45210528273979,-1.49617101918054,0.1041368994729,0.0825825212825375 +0.278,0.26112587161702,0.92433718037206,1.63938995603658,1.45092685270577,-1.49556694264939,0.105059611106342,0.0833337688792041 +0.28,0.261329690767863,0.923722939176322,1.63987597583694,1.44974566897146,-1.49496355383881,0.105982786843077,0.0840857858837292 +0.282,0.261535583910514,0.923109213791858,1.64036017779938,1.44856172875359,-1.49436085354172,0.106906427550095,0.0848385746507236 +0.284,0.261743557450495,0.922496003868321,1.64084255864486,1.44737502924111,-1.49375884255409,0.10783053410853,0.0855921375450732 +0.286,0.261953617816583,0.921883309062728,1.64132311509534,1.44618556759496,-1.49315752167497,0.108755107413726,0.086346476942042 +0.288,0.262165771460987,0.921271129039536,1.6418018438738,1.44499334094795,-1.49255689170665,0.109680148375314,0.0871015952273766 +0.29,0.262380024859516,0.920659463470698,1.64227874170422,1.4437983464045,-1.49195695345474,0.110605657917278,0.0878574947974103 +0.292,0.262596384511763,0.920048312035735,1.64275380531159,1.4426005810405,-1.49135770772833,0.111531636978031,0.0886141780591683 +0.294,0.262814856941277,0.919437674421797,1.64322703142187,1.44140004190306,-1.49075915534003,0.112458086510486,0.0893716474304728 +0.296,0.263035448695746,0.918827550323731,1.643698416762,1.44019672601037,-1.49016129710614,0.11338500748213,0.0901299053400493 +0.298,0.263258166347178,0.918217939444146,1.6441679580599,1.43899063035144,-1.48956413384675,0.114312400875101,0.0908889542276331 +0.3,0.263483016492081,0.91760884149348,1.64463565204445,1.43778175188595,-1.48896766638585,0.115240267686258,0.0916487965440754 +0.302,0.26371000575165,0.917000256190064,1.64510149544548,1.43657008754401,-1.48837189555142,0.116168608927262,0.0924094347514517 +0.304,0.263939140771955,0.916392183260192,1.64556548499376,1.43535563422598,-1.4877768221756,0.11709742562465,0.0931708713231689 +0.306,0.264170428224124,0.915784622438185,1.64602761742103,1.43413838880226,-1.48718244709476,0.118026718819915,0.093933108744074 +0.308,0.264403874804535,0.915177573466456,1.64648788945991,1.43291834811306,-1.48658877114964,0.11895648956958,0.0946961495105628 +0.31,0.264639487235008,0.914571036095582,1.64694629784398,1.4316955089682,-1.48599579518547,0.11988673894528,0.0954599961306894 +0.312,0.264877272262996,0.913965010084368,1.64740283930774,1.43046986814692,-1.48540352005206,0.120817468033841,0.0962246511242759 +0.314,0.265117236661782,0.913359495199913,1.64785751058656,1.42924142239761,-1.48481194660398,0.121748677937363,0.0969901170230229 +0.316,0.265359387230672,0.912754491217679,1.64831030841675,1.42801016843768,-1.48422107570061,0.122680369773297,0.09775639637062 +0.318,0.265603730795196,0.912149997921559,1.64876122953548,1.42677610295323,-1.48363090820631,0.123612544674529,0.0985234917228577 +0.32,0.265850274207307,0.911546015103944,1.64921027068084,1.42553922259895,-1.48304144499052,0.124545203789467,0.0992914056477387 +0.322,0.266099024345583,0.910942542565791,1.64965742859177,1.42429952399778,-1.48245268692791,0.125478348282118,0.100060140725591 +0.324,0.266349988115426,0.910339580116689,1.65010270000809,1.42305700374077,-1.48186463489847,0.12641197933218,0.100829699549179 +0.326,0.266603172449277,0.909737127574931,1.65054608167049,1.42181165838683,-1.48127728978765,0.127346098135122,0.101600084723821 +0.328,0.266858584306814,0.90913518476758,1.65098757032051,1.42056348446248,-1.4806906524865,0.128280705902275,0.102371298867497 +0.33,0.267116230675163,0.908533751530536,1.65142716270054,1.41931247846164,-1.48010472389178,0.129215803860917,0.103143344610968 +0.332,0.267376118569112,0.907932827708607,1.65186485555381,1.41805863684538,-1.47951950490609,0.130151393254361,0.103916224597891 +0.334,0.267638255031322,0.907332413155575,1.65230064562441,1.4168019560417,-1.478934996438,0.131087475342049,0.104689941484931 +0.336,0.267902647132539,0.90673250773427,1.65273452965723,1.4155424324453,-1.47835119940219,0.132024051399635,0.10546449794188 +0.338,0.268169301971814,0.906133111316632,1.65316650439801,1.41428006241731,-1.47776811471957,0.132961122719086,0.106239896651771 +0.34,0.26843822667672,0.905534223783783,1.65359656659329,1.41301484228506,-1.47718574331741,0.133898690608764,0.107016140310998 +0.342,0.268709428403572,0.904935845026098,1.65402471299042,1.41174676834185,-1.47660408612949,0.134836756393529,0.107793231629434 +0.344,0.268982914337651,0.904337974943273,1.65445094033758,1.4104758368467,-1.4760231440962,0.135775321414825,0.108571173330544 +0.346,0.269258691693424,0.903740613444392,1.65487524538371,1.40920204402408,-1.47544291816472,0.136714387030783,0.109349968151509 +0.348,0.269536767714778,0.903143760447999,1.65529762487859,1.40792538606366,-1.47486340928911,0.137653954616312,0.110129618843344 +0.35,0.26981714967524,0.902547415882166,1.65571807557276,1.40664585912009,-1.47428461843048,0.1385940255632,0.110910128171017 +0.352,0.270099844878211,0.901951579684566,1.65613659421754,1.40536345931271,-1.47370654655711,0.139534601280211,0.111691498913569 +0.354,0.270384860657201,0.901356251802538,1.65655317756503,1.40407818272529,-1.4731291946446,0.140475683193185,0.112473733864235 +0.356,0.270672204376059,0.900761432193159,1.65696782236812,1.4027900254058,-1.47255256367601,0.14141727274514,0.113256835830569 +0.358,0.270961883429212,0.900167120823314,1.65738052538043,1.40149898336611,-1.47197665464196,0.142359371396373,0.114040807634561 +0.36,0.271253905241904,0.899573317669767,1.65779128335639,1.40020505258174,-1.47140146854086,0.143301980624563,0.114825652112762 +0.362,0.271548277270435,0.898980022719228,1.65820009305114,1.3989082289916,-1.47082700637895,0.144245101924877,0.115611372116407 +0.364,0.271845007002406,0.898387235968428,1.65860695122059,1.39760850849769,-1.47025326917052,0.145188736810073,0.116397970511538 +0.366,0.272144101956964,0.897794957424183,1.6590118546214,1.39630588696485,-1.46968025793802,0.146132886810612,0.117185450179132 +0.368,0.272445569685048,0.89720318710347,1.65941480001097,1.39500036022049,-1.46910797371222,0.147077553474757,0.11797381401522 +0.37,0.272749417769638,0.896611925033494,1.65981578414743,1.39369192405427,-1.46853641753233,0.148022738368693,0.118763064931017 +0.372,0.273055653826011,0.89602117125176,1.66021480378963,1.39238057421786,-1.4679655904462,0.148968443076628,0.119553205853045 +0.374,0.273364285501991,0.895430925806141,1.66061185569719,1.39106630642462,-1.4673954935104,0.149914669200912,0.120344239723264 +0.376,0.273675320478207,0.894841188754953,1.6610069366304,1.38974911634935,-1.46682612779045,0.150861418362146,0.121136169499193 +0.378,0.273988766468352,0.894251960167021,1.66140004335032,1.38842899962796,-1.46625749436089,0.151808692199298,0.121928998154046 +0.38,0.274304631219443,0.893663240121753,1.66179117261868,1.3871059518572,-1.46568959430549,0.152756492369818,0.122722728676851 +0.382,0.274622922512086,0.893075028709209,1.66218032119795,1.38577996859435,-1.4651224287174,0.153704820549757,0.123517364072588 +0.384,0.274943648160741,0.892487326030172,1.6625674858513,1.38445104535694,-1.46455599869926,0.154653678433882,0.124312907362311 +0.386,0.275266816013988,0.891900132196218,1.66295266334261,1.38311917762241,-1.4639903053634,0.1556030677358,0.125109361583286 +0.388,0.275592433954804,0.891313447329788,1.66333585043645,1.38178436082786,-1.463425349832,0.156552990188077,0.125906729789114 +0.39,0.275920509900827,0.890727271564261,1.66371704389808,1.38044659036966,-1.4628611332372,0.157503447542361,0.126705015049868 +0.392,0.276251051804642,0.890141605044018,1.66409624049348,1.37910586160322,-1.46229765672132,0.158454441569507,0.127504220452224 +0.394,0.276584067654049,0.88955644792452,1.66447343698929,1.37776216984264,-1.46173492143697,0.159405974059703,0.128304349099592 +0.396,0.276919565472352,0.888971800372375,1.66484863015286,1.37641551036036,-1.46117292854726,0.160358046822596,0.129105404112253 +0.398,0.277257553318636,0.888387662565412,1.66522181675221,1.37506587838691,-1.46061167922591,0.161310661687421,0.129907388627487 +0.4,0.277598039288057,0.887804034692746,1.66559299355605,1.37371326911051,-1.46005117465745,0.162263820503135,0.130710305799715 +0.402,0.277941031512129,0.887220916954857,1.66596215733376,1.3723576776768,-1.45949141603739,0.163217525138541,0.131514158800629 +0.404,0.278286538159014,0.886638309563653,1.66632930485541,1.37099909918845,-1.45893240457236,0.164171777482432,0.132318950819329 +0.406,0.278634567433818,0.886056212742548,1.66669443289172,1.36963752870488,-1.45837414148031,0.165126579443717,0.133124685062462 +0.408,0.278985127578884,0.885474626726528,1.66705753821409,1.3682729612419,-1.45781662799063,0.166081932951564,0.133931364754356 +0.41,0.279338226874097,0.884893551762222,1.66741861759461,1.36690539177137,-1.4572598653444,0.167037839955535,0.134738993137162 +0.412,0.279693873637179,0.884312988107977,1.66777766780599,1.36553481522082,-1.45670385479448,0.167994302425729,0.135547573470987 +0.414,0.280052076224,0.883732936033925,1.66813468562166,1.36416122647317,-1.45614859760573,0.168951322352921,0.136357109034038 +0.416,0.280412843028881,0.883153395822055,1.66848966781566,1.36278462036631,-1.45559409505517,0.169908901748709,0.13716760312276 +0.418,0.280776182484906,0.882574367766284,1.66884261116271,1.36140499169277,-1.45504034843218,0.170867042645657,0.137979059051978 +0.42,0.281142103064239,0.881995852172526,1.66919351243821,1.36002233519938,-1.45448735903862,0.171825747097444,0.138791480155036 +0.422,0.281510613278432,0.881417849358768,1.66954236841817,1.35863664558686,-1.4539351281891,0.172785017179012,0.139604869783941 +0.424,0.281881721678752,0.880840359655133,1.66988917587929,1.35724791750951,-1.45338365721105,0.173744854986719,0.140419231309506 +0.426,0.282255436856502,0.880263383403958,1.67023393159891,1.35585614557477,-1.45283294744501,0.174705262638489,0.141234568121489 +0.428,0.28263176744334,0.879686920959858,1.67057663235502,1.35446132434289,-1.45228300024473,0.175666242273971,0.142050883628745 +0.43,0.283010722111617,0.879110972689803,1.67091727492625,1.35306344832656,-1.4517338169774,0.17662779605469,0.142868181259363 +0.432,0.2833923095747,0.878535538973182,1.6712558560919,1.35166251199047,-1.45118539902383,0.177589926164214,0.143686464460817 +0.434,0.283776538587311,0.877960620201879,1.67159237263191,1.35025850975098,-1.45063774777863,0.178552634808307,0.144505736700108 +0.436,0.284163417945865,0.877386216780341,1.67192682132684,1.3488514359757,-1.45009086465039,0.179515924215096,0.145326001463915 +0.438,0.284552956488807,0.876812329125646,1.67225919895793,1.34744128498308,-1.44954475106188,0.180479796635237,0.146147262258741 +0.44,0.284945163096961,0.876238957667577,1.67258950230703,1.34602805104205,-1.44899940845028,0.181444254342078,0.146969522611059 +0.442,0.285340046693875,0.875666102848691,1.67291772815666,1.34461172837157,-1.44845483826729,0.182409299631832,0.147792786067465 +0.444,0.285737616246171,0.875093765124386,1.67324387328997,1.34319231114027,-1.44791104197941,0.183374934823748,0.148617056194827 +0.446,0.286137880763902,0.874521944962975,1.67356793449074,1.34176979346597,-1.44736802106809,0.184341162260282,0.149442336580434 +0.448,0.286540849300905,0.873950642845755,1.67388990854339,1.34034416941532,-1.44682577702992,0.185307984307276,0.150268630832148 +0.45,0.286946530955168,0.873379859267073,1.674209792233,1.33891543300336,-1.44628431137689,0.186275403354134,0.15109594257856 +0.452,0.287354934869189,0.8728095947344,1.67452758234526,1.33748357819307,-1.44574362563652,0.187243421814005,0.151924275469135 +0.454,0.287766070230348,0.872239849768399,1.6748432756665,1.33604859889496,-1.4452037213521,0.188212042123963,0.152753633174374 +0.456,0.288179946271276,0.871670624902993,1.67515686898371,1.33461048896664,-1.44466460008291,0.189181266745194,0.153584019385964 +0.458,0.288596572270231,0.871101920685435,1.67546835908448,1.33316924221235,-1.44412626340438,0.190151098163182,0.154415437816934 +0.46,0.289015957551478,0.870533737676379,1.67577774275706,1.33172485238254,-1.44358871290835,0.191121538887902,0.155247892201814 +0.462,0.289438111485671,0.869966076449945,1.67608501679032,1.3302773131734,-1.44305195020325,0.19209259145401,0.156081386296786 +0.464,0.289863043490237,0.869398937593791,1.67639017797376,1.3288266182264,-1.44251597691431,0.193064258421038,0.156915923879847 +0.466,0.29029076302977,0.86883232170918,1.67669322309751,1.32737276112785,-1.44198079468379,0.194036542373595,0.157751508750967 +0.468,0.290721279616424,0.86826622941105,1.67699414895235,1.32591573540843,-1.4414464051712,0.195009445921563,0.158588144732247 +0.47,0.291154602810309,0.867700661328079,1.67729295232967,1.32445553454267,-1.44091281005348,0.195982971700303,0.159425835668077 +0.472,0.291590742219895,0.867135618102758,1.67758963002148,1.32299215194855,-1.44038001102528,0.196957122370858,0.160264585425304 +0.474,0.292029707502416,0.866571100391453,1.67788417882044,1.32152558098695,-1.43984800979911,0.197931900620165,0.161104397893387 +0.476,0.292471508364282,0.86600710886448,1.67817659551984,1.32005581496119,-1.43931680810564,0.198907309161263,0.161945276984562 +0.478,0.292916154561492,0.865443644206165,1.67846687691357,1.31858284711655,-1.43878640769385,0.199883350733508,0.162787226634009 +0.48,0.293363655900052,0.864880707114916,1.67875501979615,1.31710667063973,-1.43825681033131,0.200860028102791,0.16363025080001 +0.482,0.2938140222364,0.86431829830329,1.67904102096275,1.31562727865839,-1.4377280178044,0.201837344061755,0.164474353464122 +0.484,0.294267263477828,0.863756418498057,1.67932487720913,1.31414466424059,-1.4372000319185,0.202815301430021,0.165319538631337 +0.486,0.294723389582917,0.86319506844027,1.6796065853317,1.31265882039432,-1.43667285449829,0.203793903054411,0.166165810330253 +0.488,0.29518241056197,0.86263424888533,1.67988614212746,1.31116974006693,-1.43614648738794,0.204773151809178,0.167013172613239 +0.49,0.295644336477457,0.862073960603053,1.68016354439406,1.30967741614466,-1.43562093245134,0.205753050596239,0.167861629556608 +0.492,0.296109177444451,0.861514204377735,1.68043878892973,1.30818184145205,-1.43509619157238,0.206733602345406,0.168711185260784 +0.494,0.296576943631084,0.860954981008218,1.68071187253334,1.30668300875141,-1.43457226665516,0.20771481001463,0.169561843850474 +0.496,0.297047645259,0.860396291307956,1.68098279200438,1.3051809107423,-1.43404915962422,0.208696676590235,0.170413609474839 +0.498,0.297521292603808,0.859838136105081,1.68125154414293,1.30367554006094,-1.43352687242483,0.209679205087172,0.171266486307668 +0.5,0.297997895995552,0.859280516242466,1.68151812574968,1.30216688927967,-1.43300540702321,0.210662398549257,0.172120478547551 +0.502,0.298477465819175,0.858723432577792,1.68178253362595,1.30065495090639,-1.43248476540678,0.21164626004943,0.172975590418054 +0.504,0.298960012514992,0.858166885983611,1.68204476457365,1.29913971738395,-1.43196494958441,0.212630792690006,0.173831826167895 +0.506,0.299445546579171,0.857610877347412,1.68230481539528,1.29762118108961,-1.43144596158668,0.213615999602936,0.17468919007112 +0.508,0.29993407856421,0.857055407571683,1.68256268289394,1.29609933433443,-1.43092780346614,0.214601883950065,0.175547686427281 +0.51,0.300425619079432,0.856500477573973,1.68281836387336,1.29457416936267,-1.43041047729756,0.215588448923401,0.176407319561615 +0.512,0.300920178791473,0.855946088286961,1.68307185513782,1.29304567835122,-1.42989398517821,0.216575697745384,0.177268093825223 +0.514,0.301417768424783,0.855392240658514,1.6833231534922,1.29151385340895,-1.42937832922808,0.217563633669155,0.178130013595251 +0.516,0.301918398762127,0.854838935651752,1.68357225574199,1.28997868657611,-1.4288635115902,0.21855225997884,0.178993083275075 +0.518,0.302422080645099,0.85428617424511,1.68381915869322,1.28844016982371,-1.42834953443087,0.219541579989821,0.179857307294478 +0.52,0.302928824974632,0.853733957432398,1.68406385915252,1.28689829505292,-1.42783639993996,0.220531597049028,0.180722690109838 +0.522,0.303438642711521,0.853182286222867,1.6843063539271,1.28535305409434,-1.42732411033114,0.221522314535226,0.181589236204313 +0.524,0.303951544876947,0.852631161641266,1.68454663982471,1.28380443870745,-1.4268126678422,0.222513735859305,0.182456950088025 +0.526,0.304467542553012,0.852080584727905,1.68478471365369,1.28225244057992,-1.42630207473531,0.223505864464577,0.183325836298249 +0.528,0.30498664688327,0.851530556538717,1.6850205722229,1.28069705132691,-1.4257923332973,0.224498703827078,0.1841958993996 +0.53,0.305508869073279,0.850981078145316,1.68525421234179,1.27913826249047,-1.42528344583995,0.225492257455872,0.185067143984226 +0.532,0.306034220391142,0.850432150635056,1.68548563082032,1.27757606553881,-1.42477541470026,0.22648652889336,0.185939574671993 +0.534,0.306562712168069,0.849883775111094,1.685714824469,1.27601045186561,-1.42426824224075,0.227481521715596,0.186813196110682 +0.536,0.307094355798934,0.849335952692448,1.68594179009887,1.27444141278937,-1.42376193084976,0.2284772395326,0.187688012976178 +0.538,0.307629162742846,0.848788684514052,1.68616652452149,1.27286893955266,-1.42325648294173,0.229473685988685,0.188564029972667 +0.54,0.30816714452372,0.848241971726819,1.68638902454892,1.27129302332143,-1.42275190095751,0.23047086476278,0.189441251832831 +0.542,0.30870831273086,0.847695815497697,1.68660928699373,1.26971365518427,-1.42224818736465,0.231468779568766,0.190319683318042 +0.544,0.309252679019549,0.847150217009727,1.686827308669,1.26813082615171,-1.42174534465769,0.23246743415581,0.191199329218564 +0.546,0.309800255111634,0.846605177462099,1.68704308638828,1.26654452715544,-1.42124337535849,0.233466832308704,0.192080194353748 +0.548,0.310351052796137,0.846060698070207,1.68725661696559,1.26495474904759,-1.42074228201655,0.234466977848216,0.192962283572236 +0.55,0.310905083929856,0.845516780065711,1.68746789721542,1.26336148259998,-1.42024206720926,0.235467874631437,0.193845601752158 +0.552,0.31146236043798,0.844973424696586,1.68767692395271,1.26176471850329,-1.41974273354228,0.236469526552136,0.19473015380134 +0.554,0.312022894314714,0.84443063322718,1.68788369399285,1.26016444736637,-1.41924428364984,0.237471937541128,0.195615944657505 +0.556,0.312586697623905,0.84388840693827,1.68808820415165,1.25856065971538,-1.41874672019503,0.238475111566631,0.19650297928848 +0.558,0.313153782499676,0.843346747127113,1.68829045124531,1.25695334599304,-1.41825004587016,0.239479052634644,0.197391262692402 +0.56,0.313724161147074,0.842805655107503,1.68849043209048,1.25534249655777,-1.41775426339707,0.240483764789321,0.198280799897925 +0.562,0.314297845842721,0.842265132209822,1.68868814350415,1.25372810168293,-1.41725937552746,0.241489252113356,0.199171595964433 +0.564,0.314874848935465,0.841725179781094,1.68888358230369,1.25211015155595,-1.41676538504325,0.242495518728367,0.20006365598225 +0.566,0.315455182847057,0.841185799185034,1.68907674530684,1.25048863627751,-1.41627229475685,0.243502568795293,0.200956985072848 +0.568,0.316038860072818,0.840646991802105,1.68926762933166,1.24886354586067,-1.41578010751159,0.244510406514792,0.201851588389068 +0.57,0.316625893182323,0.840108759029564,1.68945623119652,1.24723487023003,-1.41528882618199,0.245519036127643,0.20274747111533 +0.572,0.317216294820093,0.839571102281515,1.68964254772012,1.24560259922084,-1.41479845367413,0.246528461915165,0.203644638467852 +0.574,0.317810077706292,0.83903402298896,1.68982657572139,1.24396672257813,-1.41430899292603,0.247538688199624,0.20454309569487 +0.576,0.318407254637435,0.838497522599845,1.69000831201957,1.24232722995579,-1.41382044690797,0.248549719344663,0.205442848076851 +0.578,0.319007838487104,0.837961602579111,1.69018775343409,1.24068411091571,-1.41333281862283,0.24956155975573,0.206343900926725 +0.58,0.319611842206671,0.837426264408742,1.69036489678464,1.23903735492678,-1.41284611110651,0.25057421388051,0.207246259590098 +0.582,0.320219278826034,0.836891509587813,1.69053973889105,1.23738695136406,-1.41236032742825,0.251587686209372,0.208149929445481 +0.584,0.320830161454355,0.836357339632536,1.69071227657335,1.23573288950774,-1.41187547069101,0.252601981275815,0.209054915904518 +0.586,0.321444503280817,0.835823756076305,1.69088250665171,1.23407515854225,-1.41139154403184,0.253617103656921,0.20996122441221 +0.588,0.322062317575378,0.835290760469747,1.69105042594638,1.23241374755525,-1.41090855062227,0.254633057973822,0.210868860447145 +0.59,0.322683617689548,0.83475835438076,1.69121603127773,1.23074864553667,-1.41042649366867,0.255649848892165,0.211777829521733 +0.592,0.323308417057164,0.834226539394563,1.69137931946616,1.22907984137771,-1.40994537641264,0.256667481122588,0.212688137182432 +0.594,0.32393672919518,0.833695317113736,1.69154028733211,1.22740732386982,-1.40946520213143,0.257685959421204,0.213599789009989 +0.596,0.324568567704469,0.833164689158267,1.69169893169601,1.22573108170369,-1.40898597413828,0.25870528859009,0.214512790619674 +0.598,0.32520394627063,0.83263465716559,1.69185524937823,1.2240511034682,-1.40850769578284,0.259725473477784,0.215427147661516 +0.6,0.325842878664809,0.832105222790631,1.69200923719909,1.22236737764939,-1.40803037045157,0.260746518979793,0.216342865820544 +0.602,0.326485378744529,0.831576387705844,1.69216089197877,1.22067989262937,-1.40755400156818,0.2617684300391,0.217259950817033 +0.604,0.32713146045453,0.831048153601257,1.69231021053734,1.21898863668526,-1.40707859259396,0.262791211646687,0.21817840840674 +0.606,0.327781137827622,0.830520522184506,1.69245718969464,1.21729359798808,-1.40660414702827,0.263814868842065,0.219098244381156 +0.608,0.328434424985547,0.82999349518088,1.6926018262703,1.21559476460166,-1.40613066840892,0.264839406713806,0.220019464567748 +0.61,0.329091336139854,0.829467074333351,1.69274411708367,1.21389212448149,-1.40565816031261,0.265864830400088,0.220942074830215 +0.612,0.329751885592781,0.828941261402621,1.6928840589538,1.2121856654736,-1.40518662635532,0.266891145089247,0.221866081068733 +0.614,0.330416087738156,0.828416058167149,1.69302164869937,1.2104753753134,-1.40471607019281,0.267918356020342,0.222791489220211 +0.616,0.331083957062302,0.827891466423194,1.69315688313865,1.20876124162448,-1.40424649552098,0.268946468483716,0.223718305258547 +0.618,0.331755508144961,0.827367487984847,1.69328975908944,1.20704325191748,-1.40377790607638,0.26997548782158,0.224646535194885 +0.62,0.332430755660224,0.826844124684064,1.69342027336906,1.20532139358883,-1.4033103056366,0.271005419428598,0.225576185077874 +0.622,0.333109714377477,0.826321378370702,1.69354842279425,1.20359565391954,-1.40284369802077,0.272036268752482,0.226507260993931 +0.624,0.333792399162359,0.82579925091255,1.69367420418112,1.20186602007398,-1.40237808708997,0.273068041294597,0.227439769067504 +0.626,0.334478824977734,0.825277744195361,1.69379761434514,1.20013247909861,-1.40191347674773,0.274100742610575,0.228373715461337 +0.628,0.335169006884673,0.824756860122883,1.69391865010102,1.19839501792068,-1.40144987094048,0.275134378310939,0.229309106376743 +0.63,0.335862960043451,0.824236600616888,1.69403730826268,1.196653623347,-1.40098727365802,0.276168954061735,0.230245948053871 +0.632,0.336560699714559,0.823716967617205,1.6941535856432,1.19490828206257,-1.40052568893401,0.277204475585177,0.231184246771978 +0.634,0.337262241259729,0.823197963081741,1.69426747905472,1.19315898062925,-1.40006512084643,0.278240948660297,0.232124008849708 +0.636,0.337967600142971,0.822679588986517,1.6943789853084,1.19140570548446,-1.3996055735181,0.279278379123611,0.233065240645369 +0.638,0.338676791931626,0.822161847325688,1.69448810121431,1.18964844293975,-1.39914705111716,0.280316772869789,0.234007948557212 +0.64,0.339389832297435,0.821644740111569,1.69459482358143,1.18788717917947,-1.39868955785753,0.281356135852341,0.234952139023716 +0.642,0.34010673701762,0.821128269374666,1.69469914921749,1.18612190025931,-1.39823309799951,0.282396474084314,0.23589781852387 +0.644,0.340827521975984,0.82061243716369,1.69480107492895,1.18435259210489,-1.39777767585019,0.28343779363899,0.236844993577466 +0.646,0.341552203164018,0.820097245545589,1.69490059752091,1.18257924051032,-1.39732329576404,0.284480100650611,0.237793670745386 +0.648,0.342280796682039,0.819582696605564,1.694997713797,1.18080183113671,-1.39686996214339,0.285523401315102,0.238743856629897 +0.65,0.343013318740326,0.81906879244709,1.69509242055932,1.17902034951069,-1.39641767943898,0.286567701890809,0.239695557874945 +0.652,0.343749785660284,0.818555535191939,1.69518471460832,1.17723478102287,-1.3959664521505,0.287613008699255,0.24064878116646 +0.654,0.344490213875625,0.818042926980197,1.69527459274276,1.17544511092634,-1.39551628482714,0.288659328125899,0.241603533232649 +0.656,0.345234619933556,0.817530969970283,1.69536205175957,1.17365132433509,-1.39506718206809,0.289706666620909,0.242559820844309 +0.658,0.345983020495995,0.817019666338961,1.69544708845376,1.17185340622239,-1.39461914852315,0.290755030699957,0.243517650815128 +0.66,0.3467354323408,0.816509018281363,1.69552969961833,1.17005134141926,-1.3941721888933,0.29180442694501,0.244477030002002 +0.662,0.347491872363016,0.815999028011,1.69560988204414,1.16824511461275,-1.3937263079312,0.292854862005149,0.245437965305342 +0.664,0.348252357576138,0.815489697759775,1.69568763251984,1.16643471034435,-1.39328151044184,0.293906342597389,0.246400463669397 +0.666,0.349016905113397,0.814981029777997,1.69576294783173,1.16462011300827,-1.39283780128309,0.294958875507526,0.247364532082572 +0.668,0.349785532229064,0.814473026334392,1.69583582476364,1.16280130684977,-1.3923951853663,0.29601246759098,0.248330177577747 +0.67,0.350558256299769,0.813965689716114,1.69590626009682,1.16097827596335,-1.39195366765687,0.297067125773667,0.249297407232611 +0.672,0.351335094825847,0.813459022228753,1.69597425060984,1.1591510042911,-1.39151325317493,0.298122857052878,0.250266228169987 +0.674,0.352116065432694,0.812953026196347,1.6960397930784,1.15731947562081,-1.39107394699585,0.299179668498174,0.251236647558167 +0.676,0.352901185872157,0.812447703961383,1.69610288427528,1.15548367358421,-1.39063575425096,0.300237567252293,0.252208672611248 +0.678,0.35369047402393,0.811943057884808,1.69616352097012,1.15364358165513,-1.39019868012811,0.301296560532075,0.253182310589472 +0.68,0.354483947896985,0.811439090346032,1.69622169992937,1.15179918314758,-1.38976272987234,0.302356655629401,0.254157568799575 +0.682,0.355281625631012,0.810935803742933,1.69627741791604,1.14995046121391,-1.38932790878653,0.303417859912147,0.255134454595126 +0.684,0.356083525497892,0.810433200491857,1.69633067168965,1.14809739884286,-1.38889422223202,0.304480180825155,0.256112975376887 +0.686,0.356889665903186,0.809931283027622,1.69638145800603,1.14623997885756,-1.38846167562931,0.305543625891216,0.257093138593163 +0.688000000000001,0.357700065387648,0.809430053803515,1.69642977361715,1.14437818391359,-1.38803027445872,0.306608202712076,0.258074951740165 +0.690000000000001,0.358514742628765,0.808929515291296,1.69647561527099,1.14251199649693,-1.38760002426106,0.307673918969449,0.259058422362369 +0.692000000000001,0.359333716442316,0.808429669981188,1.69651897971134,1.14064139892193,-1.38717093063831,0.30874078242606,0.260043558052888 +0.694000000000001,0.360157005783957,0.807930520381884,1.69655986367765,1.13876637332919,-1.38674299925437,0.309808800926693,0.261030366453843 +0.696000000000001,0.360984629750835,0.807432069020531,1.69659826390486,1.13688690168344,-1.38631623583568,0.31087798239926,0.262018855256733 +0.698000000000001,0.361816607583219,0.806934318442732,1.69663417712318,1.13500296577143,-1.38589064617204,0.311948334855894,0.263009032202825 +0.700000000000001,0.362652958666169,0.806437271212536,1.69666760005791,1.1331145471997,-1.38546623611725,0.313019866394053,0.26400090508353 +0.702000000000001,0.363493702531216,0.805940929912428,1.69669852942929,1.13122162739234,-1.38504301158989,0.314092585197642,0.264994481740796 +0.704000000000001,0.364338858858083,0.805445297143323,1.69672696195224,1.1293241875888,-1.38462097857406,0.315166499538163,0.265989770067504 +0.706000000000001,0.365188447476426,0.804950375524552,1.69675289433619,1.1274222088415,-1.38420014312011,0.316241617775874,0.26698677800786 +0.708000000000001,0.366042488367606,0.804456167693848,1.69677632328484,1.12551567201358,-1.38378051134547,0.317317948360972,0.267985513557805 +0.710000000000001,0.366901001666487,0.803962676307337,1.69679724549598,1.12360455777646,-1.38336208943535,0.318395499834801,0.268985984765421 +0.712000000000001,0.367764007663267,0.803469904039519,1.69681565766121,1.12168884660748,-1.3829448836436,0.319474280831066,0.26998819973134 +0.714000000000001,0.368631526805336,0.802977853583254,1.69683155646577,1.11976851878742,-1.38252890029343,0.320554300077089,0.270992166609171 +0.716000000000001,0.369503579699164,0.802486527649743,1.69684493858823,1.117843554398,-1.38211414577831,0.321635566395064,0.271997893605915 +0.718000000000001,0.370380187112223,0.801995928968507,1.69685580070033,1.11591393331936,-1.38170062656274,0.322718088703354,0.2730053889824 +0.720000000000001,0.371261369974941,0.801506060287371,1.69686413946665,1.11397963522751,-1.38128834918306,0.323801876017791,0.274014661053711 +0.722000000000001,0.372147149382683,0.80101692437244,1.69686995154438,1.11204063959166,-1.38087732024836,0.324886937453017,0.275025718189634 +0.724000000000001,0.373037546597773,0.800528524008073,1.69687323358305,1.11009692567158,-1.3804675464413,0.325973282223834,0.276038568815098 +0.726000000000001,0.373932583051543,0.800040861996863,1.69687398222428,1.10814847251491,-1.38005903451898,0.327060919646585,0.27705322141063 +0.728000000000001,0.374832280346423,0.799553941159607,1.69687219410143,1.10619525895436,-1.37965179131384,0.328149859140556,0.278069684512809 +0.730000000000001,0.375736660258057,0.799067764335282,1.69686786583939,1.10423726360499,-1.37924582373451,0.329240110229403,0.279087966714733 +0.732000000000001,0.376645744737467,0.798582334381012,1.69686099405422,1.10227446486125,-1.37884113876681,0.330331682542605,0.280108076666483 +0.734000000000001,0.377559555913246,0.798097654172041,1.69685157535287,1.10030684089419,-1.37843774347457,0.331424585816943,0.281130023075608 +0.736000000000001,0.37847811609379,0.7976137266017,1.69683960633284,1.09833436964846,-1.37803564500062,0.332518829898,0.282153814707598 +0.738000000000001,0.379401447769568,0.797130554581374,1.69682508358188,1.09635702883929,-1.37763485056776,0.333614424741697,0.283179460386379 +0.740000000000001,0.380329573615434,0.796648141040466,1.69680800367765,1.09437479594951,-1.37723536747966,0.334711380415846,0.28420696899481 +0.742000000000001,0.381262516492979,0.796166488926362,1.69678836318734,1.09238764822638,-1.37683720312191,0.335809707101738,0.285236349475179 +0.744000000000001,0.382200299452919,0.795685601204394,1.69676615866738,1.09039556267849,-1.37644036496295,0.336909415095755,0.286267610829719 +0.746000000000001,0.383142945737533,0.795205480857798,1.69674138666302,1.08839851607248,-1.37604486055513,0.338010514811014,0.287300762121124 +0.748000000000001,0.384090478783135,0.794726130887675,1.69671404370796,1.08639648492985,-1.37565069753569,0.339113016779037,0.288335812473073 +0.750000000000001,0.385042922222597,0.79424755431295,1.696684126324,1.08438944552358,-1.37525788362786,0.340216931651456,0.28937277107076 +0.752000000000001,0.386000299887916,0.793769754170327,1.69665163102062,1.08237737387477,-1.37486642664184,0.341322270201743,0.290411647161438 +0.754000000000001,0.386962635812822,0.793292733514242,1.69661655429457,1.08036024574916,-1.37447633447595,0.342429043326975,0.291452450054964 +0.756000000000001,0.387929954235436,0.792816495416819,1.69657889262947,1.07833803665368,-1.37408761511765,0.343537262049631,0.292495189124354 +0.758000000000001,0.388902279600979,0.792341042967819,1.69653864249535,1.07631072183284,-1.3737002766447,0.344646937519421,0.293539873806351 +0.760000000000001,0.389879636564525,0.791866379274594,1.69649580034824,1.0742782762651,-1.37331432722627,0.345758081015148,0.294586513601993 +0.762000000000001,0.390862049993804,0.791392507462031,1.69645036262971,1.0722406746592,-1.37292977512405,0.346870703946606,0.295635118077199 +0.764000000000001,0.391849544972063,0.790919430672502,1.69640232576638,1.07019789145033,-1.37254662869348,0.347984817856511,0.296685696863354 +0.766000000000001,0.39284214680097,0.790447152065808,1.69635168616948,1.06814990079638,-1.37216489638485,0.349100434422465,0.297738259657913 +0.768000000000001,0.39383988100358,0.789975674819122,1.69629844023432,1.06609667657397,-1.37178458674457,0.350217565458968,0.298792816225007 +0.770000000000001,0.394842773327349,0.789505002126933,1.69624258433982,1.06403819237448,-1.37140570841632,0.351336222919451,0.29984937639606 +0.772000000000001,0.395850849747209,0.789035137200989,1.69618411484795,1.06197442150004,-1.37102827014237,0.352456418898361,0.300907950070421 +0.774000000000001,0.3968641364687,0.788566083270228,1.69612302810324,1.05990533695935,-1.37065228076476,0.353578165633276,0.301968547215996 +0.776000000000001,0.397882659931156,0.788097843580724,1.6960593204322,1.05783091146355,-1.37027774922662,0.354701475507065,0.3030311778699 +0.778000000000001,0.398906446810959,0.787630421395619,1.69599298814277,1.05575111742183,-1.36990468457349,0.355826361050085,0.304095852139115 +0.780000000000001,0.399935524024845,0.787163819995057,1.69592402752375,1.0536659269372,-1.36953309595459,0.356952834942425,0.305162580201157 +0.782000000000001,0.400969918733283,0.786698042676117,1.69585243484419,1.05157531180193,-1.36916299262423,0.358080910016186,0.306231372304759 +0.784000000000001,0.40200965834391,0.786233092752747,1.69577820635279,1.04947924349311,-1.3687943839431,0.359210599257809,0.307302238770558 +0.786000000000001,0.403054770515038,0.785768973555687,1.69570133827728,1.04737769316797,-1.36842727937975,0.360341915810446,0.308375189991801 +0.788000000000001,0.404105283159224,0.785305688432404,1.69562182682374,1.04527063165923,-1.36806168851195,0.361474872976378,0.30945023643506 +0.790000000000001,0.405161224446912,0.784843240747014,1.69553966817601,1.04315802947029,-1.36769762102814,0.362609484219476,0.310527388640954 +0.792000000000001,0.406222622810147,0.784381633880207,1.69545485849494,1.04103985677034,-1.36733508672891,0.363745763167715,0.311606657224894 +0.794000000000001,0.407289506946357,0.783920871229171,1.69536739391773,1.03891608338938,-1.36697409552847,0.364883723615732,0.312688052877827 +0.796000000000001,0.408361905822214,0.783460956207513,1.69527727055721,1.03678667881316,-1.3666146574562,0.366023379527437,0.313771586367008 +0.798000000000001,0.409439848677566,0.78300189224518,1.69518448450111,1.034651612178,-1.36625678265817,0.367164745038676,0.314857268536773 +0.800000000000001,0.410523365029454,0.782543682788374,1.69508903181128,1.03251085226553,-1.36590048139872,0.368307834459938,0.315945110309331 +0.802000000000001,0.411612484676203,0.782086331299471,1.69499090852296,1.03036436749728,-1.36554576406205,0.369452662279131,0.317035122685568 +0.804000000000001,0.412707237701594,0.781629841256938,1.69489011064393,1.02821212592921,-1.36519264115388,0.370599243164394,0.31812731674587 +0.806000000000001,0.413807654479129,0.78117421615524,1.69478663415375,1.02605409524615,-1.36484112330308,0.371747591966983,0.319221703650953 +0.808000000000001,0.414913765676368,0.78071945950476,1.69468047500288,1.02389024275604,-1.36449122126337,0.372897723724203,0.320318294642716 +0.810000000000001,0.416025602259366,0.7802655748317,1.69457162911183,1.02172053538414,-1.36414294591506,0.374049653662401,0.321417101045101 +0.812000000000001,0.417143195497194,0.779812565678,1.69446009237027,1.01954493966712,-1.36379630826676,0.375203397200024,0.322518134264975 +0.814000000000001,0.418266576966555,0.779360435601234,1.69434586063614,1.01736342174694,-1.36345131945721,0.37635896995073,0.323621405793029 +0.816000000000001,0.419395778556493,0.778909188174525,1.69422892973472,1.01517594736473,-1.36310799075706,0.377516387726573,0.324726927204686 +0.818000000000001,0.420530832473201,0.778458826986443,1.69410929545763,1.01298248185449,-1.36276633357077,0.378675666541243,0.325834710161034 +0.820000000000001,0.421671771244925,0.77800935564091,1.6939869535619,1.01078299013658,-1.36242635943845,0.37983682261338,0.326944766409773 +0.822000000000001,0.422818627726979,0.777560777757098,1.69386189976891,1.00857743671126,-1.36208808003783,0.38099987236995,0.328057107786179 +0.824000000000001,0.423971435106852,0.777113096969331,1.69373412976341,1.00636578565191,-1.36175150718617,0.382164832449694,0.329171746214089 +0.826000000000001,0.425130226909433,0.776666316926984,1.6936036391924,1.00414800059822,-1.36141665284231,0.383331719706647,0.3302886937069 +0.828000000000001,0.426295037002344,0.776220441294373,1.69347042366407,1.00192404474922,-1.36108352910867,0.384500551213734,0.331407962368596 +0.830000000000001,0.42746589960138,0.775775473750657,1.69333447874666,0.999693880856133,-1.36075214823337,0.385671344266435,0.332529564394784 +0.832000000000001,0.428642849276074,0.775331417989725,1.69319579996733,0.997457471215093,-1.36042252261229,0.38684411638653,0.333653512073758 +0.834000000000001,0.429825920955377,0.774888277720093,1.69305438281095,0.995214777659732,-1.3600946647913,0.388018885325928,0.334779817787582 +0.836000000000001,0.431015149933455,0.774446056664789,1.6929102227189,0.992965761553579,-1.35976858746839,0.389195669070562,0.335908494013194 +0.838000000000001,0.432210571875624,0.774004758561247,1.69276331508782,0.990710383782307,-1.35944430349599,0.390374485844389,0.337039553323528 +0.840000000000001,0.433412222824403,0.77356438716119,1.69261365526831,0.988448604745822,-1.3591218258832,0.391555354113452,0.338173008388667 +0.842000000000001,0.434620139205699,0.773124946230518,1.69246123856363,0.986180384350166,-1.35880116779819,0.392738292590051,0.339308871977014 +0.844000000000001,0.435834357835138,0.772686439549193,1.69230606022837,0.98390568199926,-1.35848234257052,0.393923320236982,0.340447156956484 +0.846000000000001,0.437054915924527,0.772248870911121,1.69214811546701,0.981624456586459,-1.35816536369366,0.395110456271891,0.341587876295726 +0.848000000000001,0.438281851088456,0.771812244124035,1.69198739943251,0.979336666485922,-1.35785024482738,0.396299720171701,0.342731043065368 +0.850000000000001,0.439515201351058,0.771376563009375,1.69182390722489,0.977042269543799,-1.35753699980037,0.39749113167715,0.343876670439285 +0.852000000000001,0.440755005152913,0.770941831402165,1.69165763388966,0.974741223069216,-1.35722564261279,0.398684710797425,0.345024771695899 +0.854000000000001,0.442001301358104,0.770508053150896,1.6914885744163,0.972433483825064,-1.35691618743891,0.399880477814895,0.346175360219501 +0.856000000000001,0.44325412926144,0.770075232117399,1.69131672373669,0.970119008018585,-1.35660864862987,0.401078453289951,0.347328449501603 +0.858000000000001,0.444513528595841,0.769643372176721,1.69114207672346,0.967797751291739,-1.35630304071633,0.402278658065955,0.348484053142316 +0.860000000000001,0.445779539539883,0.769212477216998,1.69096462818831,0.965469668711365,-1.35599937841142,0.403481113274301,0.349642184851766 +0.862000000000001,0.447052202725531,0.768782551139334,1.69078437288029,0.96313471475911,-1.35569767661351,0.404685840339581,0.350802858451529 +0.864000000000001,0.448331559246035,0.768353597857666,1.69060130548404,0.960792843321136,-1.3553979504092,0.405892860984882,0.351966087876102 +0.866000000000001,0.449617650664018,0.76792562129864,1.69041542061799,0.958444007677585,-1.35510021507633,0.407102197237186,0.353131887174411 +0.868000000000001,0.450910519019749,0.767498625401476,1.69022671283244,0.956088160491812,-1.35480448608705,0.408313871432913,0.354300270511339 +0.870000000000001,0.45221020683961,0.767072614117844,1.69003517660768,0.953725253799355,-1.35451077911091,0.409527906223571,0.355471252169306 +0.872000000000001,0.453516757144762,0.766647591411727,1.68984080635202,0.951355238996666,-1.35421911001815,0.410744324581551,0.356644846549863 +0.874000000000001,0.454830213460013,0.766223561259287,1.68964359639976,0.948978066829564,-1.35392949488292,0.411963149806047,0.357821068175339 +0.876000000000001,0.456150619822902,0.765800527648738,1.68944354100908,0.946593687381426,-1.35364194998666,0.413184405529123,0.358999931690516 +0.878000000000001,0.457478020792991,0.765378494580203,1.68924063435995,0.944202050061099,-1.3533564918215,0.414408115721909,0.360181451864343 +0.880000000000001,0.458812461461392,0.764957466065586,1.68903487055187,0.941803103590523,-1.35307313709383,0.415634304700956,0.36136564359169 +0.882000000000001,0.460153987460509,0.76453744612843,1.68882624360167,0.93939679599206,-1.35279190272783,0.416862997134732,0.362552521895141 +0.884000000000001,0.461502644974029,0.764118438803788,1.68861474744116,0.936983074575519,-1.35251280586914,0.418094218050275,0.363742101926829 +0.886000000000001,0.462858480747148,0.763700448138077,1.68840037591471,0.934561885924872,-1.35223586388867,0.419327992840007,0.364934398970313 +0.888000000000001,0.46422154209705,0.763283478188946,1.68818312277685,0.932133175884645,-1.35196109438641,0.420564347268709,0.366129428442499 +0.890000000000001,0.465591876923642,0.762867533025138,1.68796298168972,0.92969688954598,-1.35168851519533,0.421803307480661,0.367327205895602 +0.892000000000001,0.46696953372056,0.762452616726347,1.68773994622045,0.927252971232351,-1.35141814438548,0.423044900006966,0.368527747019162 +0.894000000000001,0.468354561586441,0.762038733383084,1.68751400983853,0.92480136448494,-1.35115000026807,0.424289151773041,0.3697310676421 +0.896000000000001,0.46974701023648,0.761625887096537,1.68728516591304,0.922342012047637,-1.35088410139967,0.425536090106295,0.370937183734827 +0.898000000000001,0.47114693001428,0.761214081978428,1.68705340770984,0.919874855851677,-1.3506204665866,0.426785742744001,0.372146111411403 +0.900000000000001,0.472554371903997,0.760803322150878,1.68681872838864,0.917399836999892,-1.35035911488929,0.428038137841358,0.373357866931744 +0.902000000000001,0.473969387542795,0.760393611746268,1.68658112100006,0.914916895750559,-1.35010006562685,0.429293303979753,0.37457246670389 +0.904000000000001,0.475392029233621,0.759984954907094,1.6863405784825,0.912425971500853,-1.34984333838171,0.430551270175239,0.37578992728632 +0.906000000000001,0.476822349958305,0.759577355785835,1.68609709365902,0.909927002769864,-1.34958895300441,0.431812065887212,0.377010265390331 +0.908000000000001,0.478260403390997,0.759170818544811,1.68585065923408,0.907419927181192,-1.34933692961841,0.433075721027319,0.378233497882469 +0.910000000000001,0.479706243911961,0.758765347356043,1.68560126779016,0.904904681445089,-1.34908728862518,0.434342265968594,0.379459641787025 +0.912000000000001,0.481159926621715,0.758360946401119,1.68534891178437,0.902381201340138,-1.34884005070925,0.435611731554817,0.380688714288591 +0.914000000000001,0.482621507355559,0.757957619871054,1.68509358354485,0.899849421694459,-1.34859523684352,0.436884149110127,0.381920732734683 +0.916000000000001,0.484091042698469,0.757555371966154,1.68483527526718,0.897309276366417,-1.34835286829465,0.438159550448874,0.383155714638424 +0.918000000000001,0.485568590000401,0.757154206895882,1.68457397901055,0.894760698224834,-1.34811296662854,0.439437967885736,0.384393677681301 +0.920000000000001,0.487054207391994,0.756754128878721,1.68430968669397,0.892203619128659,-1.34787555371608,0.440719434246096,0.38563463971599 +0.922000000000001,0.488547953800699,0.756355142142042,1.68404239009222,0.889637969906118,-1.3476406517389,0.442003982876693,0.386878618769256 +0.924000000000001,0.490049888967343,0.755957250921971,1.6837720808318,0.887063680333286,-1.3474082831954,0.44329164765656,0.388125633044921 +0.926000000000001,0.49156007346314,0.755560459463259,1.68349875038669,0.884480679112093,-1.34717847090683,0.444582463008251,0.389375700926921 +0.928000000000001,0.493078568707169,0.755164772019151,1.68322239007398,0.881888893847731,-1.34695123802363,0.445876463909367,0.390628840982429 +0.930000000000001,0.494605436984334,0.754770192851261,1.68294299104942,0.879288251025442,-1.34672660803187,0.447173685904398,0.391885071965071 +0.932000000000001,0.496140741463817,0.754376726229443,1.68266054430282,0.876678675986674,-1.34650460475987,0.448474165116885,0.393144412818224 +0.934000000000001,0.497684546218041,0.75398437643167,1.68237504065322,0.874060092904574,-1.34628525238504,0.449777938261915,0.394406882678398 +0.936000000000001,0.499236916242176,0.753593147743912,1.68208647074407,0.871432424758801,-1.34606857544087,0.451085042658959,0.395672500878717 +0.938000000000001,0.500797917474175,0.753203044460016,1.68179482503815,0.868795593309639,-1.34585459882411,0.452395516245067,0.396941286952482 +0.940000000000001,0.502367616815395,0.752814070881591,1.68150009381235,0.866149519071373,-1.34564334780218,0.453709397588432,0.398213260636843 +0.942000000000001,0.503946082151792,0.752426231317893,1.68120226715232,0.863494121284923,-1.34543484802076,0.455026725902326,0.399488441876566 +0.944000000000001,0.505533382375731,0.752039530085718,1.68090133494694,0.86082931788969,-1.34522912551157,0.456347541059442,0.400766850827896 +0.946000000000001,0.507129587408423,0.75165397150929,1.6805972868826,0.858155025494593,-1.34502620670044,0.457671883606638,0.402048507862545 +0.948000000000001,0.508734768223008,0.75126955992016,1.68029011243734,0.855471159348284,-1.34482611841553,0.458999794780093,0.40333343357177 +0.950000000000001,0.510348996868318,0.750886299657107,1.67997980087474,0.852777633308479,-1.34462888789586,0.46033131652092,0.404621648770582 +0.952000000000001,0.511972346493336,0.750504195066042,1.67966634123772,0.850074359810409,-1.34443454279998,0.461666491491206,0.405913174502065 +0.954000000000001,0.513604891372377,0.750123250499916,1.67934972234202,0.847361249834333,-1.34424311121502,0.463005363090545,0.407208032041816 +0.956000000000001,0.515246706931014,0.749743470318633,1.67902993276958,0.844638212872096,-1.3440546216659,0.464347975473034,0.408506242902521 +0.958000000000001,0.51689786977279,0.749364858888968,1.67870696086161,0.841905156892684,-1.34386910312487,0.465694373564786,0.409807828838651 +0.960000000000001,0.518558457706728,0.748987420584495,1.67838079471156,0.839161988306747,-1.34368658502129,0.467044603081956,0.411112811851305 +0.962000000000001,0.520228549775672,0.748611159785508,1.67805142215771,0.836408611930058,-1.34350709725175,0.468398710549312,0.412421214193182 +0.964000000000001,0.521908226285501,0.748236080878964,1.67771883077565,0.833644930945847,-1.34333067019044,0.469756743319362,0.41373305837371 +0.966000000000001,0.523597568835236,0.747862188258422,1.67738300787044,0.830870846865996,-1.34315733469985,0.471118749592062,0.415048367164317 +0.968000000000001,0.525296660348082,0.747489486323987,1.67704394046854,0.828086259491027,-1.34298712214183,0.472484778435127,0.416367163603869 +0.970000000000001,0.527005585103426,0.747117979482274,1.67670161530945,0.825291066868854,-1.34282006438896,0.473854879804969,0.417689471004256 +0.972000000000001,0.528724428769853,0.74674767214636,1.67635601883712,0.822485165252247,-1.34265619383623,0.475229104568283,0.41901531295616 +0.974000000000001,0.530453278439197,0.746378568735768,1.67600713719098,0.819668449054949,-1.34249554341317,0.476607504524301,0.420344713334991 +0.976000000000001,0.532192222661666,0.746010673676433,1.67565495619679,0.816840810806414,-1.34233814659628,0.477990132427756,0.421677696307004 +0.978000000000001,0.533941351482108,0.745643991400703,1.67529946135707,0.814002141105092,-1.34218403742192,0.479377042012564,0.423014286335604 +0.980000000000001,0.535700756477432,0.745278526347328,1.67494063784125,0.811152328570214,-1.34203325049952,0.480768288016265,0.424354508187849 +0.982000000000001,0.537470530795246,0.744914282961473,1.67457847047551,0.808291259792018,-1.3418858210253,0.48216392620525,0.425698386941153 +0.984000000000001,0.539250769193762,0.744551265694735,1.6742129437322,0.805418819280353,-1.3417417847964,0.483564013400801,0.427045947990197 +0.986000000000001,0.541041568083012,0.744189479005174,1.67384404171896,0.802534889411588,-1.34160117822545,0.484968607505984,0.428397217054064 +0.988000000000001,0.542843025567427,0.743828927357357,1.67347174816742,0.799639350373773,-1.34146403835561,0.486377767533419,0.429752220183594 +0.990000000000001,0.544655241489839,0.74346961522241,1.67309604642149,0.796732080109966,-1.34133040287617,0.487791553633977,0.431110983768983 +0.992000000000001,0.546478317476968,0.743111547078087,1.6727169194253,0.793812954259661,-1.34120031013859,0.489210027126422,0.432473534547618 +0.994000000000001,0.548312356986443,0.742754727408854,1.67233434971062,0.79088184609823,-1.34107379917311,0.490633250528062,0.433839899612172 +0.996000000000001,0.550157465355433,0.742399160705986,1.6719483193839,0.787938626474304,-1.34095090970594,0.492061287586428,0.435210106418966 +0.998000000000001,0.552013749850943,0.742044851467679,1.67155881011282,0.784983163745001,-1.34083168217698,0.493494203312033,0.4365841827966 +1,0.553881319721864,0.741691804199181,1.67116580311233,0.782015323708916,-1.3407161577582,0.494932064012265,0.437962156954878 +1.002,0.555758405581316,0.741340376897871,1.67076967518427,0.779037954278724,-1.34060449164699,0.496373491996242,0.439342672210621 +1.004,0.557641296806578,0.740991275605375,1.67037121748683,0.77605695670811,-1.34049693438926,0.497815636324553,0.440722963530672 +1.006,0.559526191303611,0.740645196098405,1.66997124926462,0.773078332552052,-1.34039370637666,0.499255605895268,0.442100234338917 +1.008,0.561409194007457,0.740302823887211,1.66957061800455,0.770108185619905,-1.34029499724412,0.500690468395963,0.443471656204101 +1.01,0.563286315823427,0.739964834267434,1.66917019945809,0.767152723452198,-1.34020096541209,0.502117249442449,0.444834368686746 +1.012,0.565153473041822,0.739631892426621,1.66877089752419,0.764218258280719,-1.34011173778583,0.503532931924247,0.44618547935351 +1.014,0.567006487260656,0.739304653606594,1.66837364398682,0.761311207429208,-1.34002740962483,0.504934455577989,0.447522063967599 +1.016,0.568841085852345,0.738983763322866,1.66797939810059,0.758438093110002,-1.33994804459585,0.506318716810961,0.448841166864055 +1.018,0.570652903011513,0.738669857642231,1.66758914601762,0.755605541570324,-1.33987367502359,0.507682568797832,0.450139801518866 +1.02,0.572437481421952,0.738363563519626,1.66720390004829,0.752820281540713,-1.33980430235314,0.509022821874288,0.451414951320892 +1.022,0.574190274581241,0.738065499195276,1.66682469774864,0.750089141937375,-1.33973989783875,0.510336244251613,0.452663570555551 +1.024,0.575906649821483,0.737776274653084,1.66645260082653,0.747419048770147,-1.33968040347321,0.511619563076409,0.453882585609056 +1.026,0.577581892064096,0.737496492141125,1.66608869385907,0.744817021208342,-1.33962573317189,0.512869465859324,0.455068896401687 +1.028,0.57921120834536,0.737226746755033,1.66573408281373,0.742290166758136,-1.33957577422521,0.514082602296021,0.456219378058186 +1.03,0.580789733147558,0.736967627084935,1.66538989336563,0.739845675507458,-1.3395303890323,0.515255586502492,0.457330882822769 +1.032,0.582312534567842,0.736719715926498,1.66505726900418,0.737490813397577,-1.33948941712787,0.516384999685212,0.458400242225522 +1.034,0.583774621353455,0.736483591056516,1.66473736892266,0.735232914484981,-1.33945267751257,0.517467393264478,0.459424269506035 +1.036,0.585170950827477,0.736259826073309,1.66443136568511,0.733079372162613,-1.33941997129601,0.51849929246651,0.460399762299019 +1.038,0.586496437723857,0.736048991302086,1.66414044266607,0.7310376293163,-1.339391084659,0.519477200396563,0.461323505585375 +1.04,0.587745963944106,0.735851654765217,1.66386579125972,0.72911516740025,-1.33936579213958,0.520397602601262,0.462192274910672 +1.042,0.588914389240553,0.735668383217213,1.66360860785671,0.727319494424782,-1.33934386024439,0.521256972123737,0.463002839871291 +1.044,0.589996562822635,0.735499743243998,1.66337009058842,0.725658131860135,-1.33932505138409,0.522051775049799,0.463751967866613 +1.046,0.590989059404645,0.735346033274813,1.66315105679059,0.724135954353283,-1.33930909818422,0.522779742607552,0.464437622312473 +1.048,0.591894687209801,0.735206593116185,1.66295094815956,0.72274829054946,-1.33929564424948,0.523443168762674,0.465062066340033 +1.05,0.592716923057084,0.735080688472566,1.66276905075676,0.721489491939428,-1.33928436281209,0.524044806490726,0.465627984127698 +1.052,0.593459355242655,0.734967596363792,1.66260461813664,0.72035378558767,-1.33927496451317,0.524587460369519,0.466138096720439 +1.054,0.594125674283363,0.734866604090695,1.66245687418363,0.719335284285894,-1.33926719435492,0.525073982512669,0.466595158830546 +1.056,0.594719663814018,0.734777008242572,1.66232501586567,0.718427996583489,-1.33926082868947,0.525507268515069,0.467001955724433 +1.058,0.595245191692822,0.734698113748438,1.66220821589435,0.717625836627583,-1.33925567226483,0.525890253443684,0.467361300208939 +1.06,0.595706201366119,0.734629232973779,1.66210562528246,0.716922633749214,-1.33925155534715,0.526225907905239,0.467676029729605 +1.062,0.596106703540097,0.734569684864375,1.66201637578971,0.716312141736411,-1.33924833093719,0.526517234220225,0.467949003592413 +1.064,0.596450768203253,0.734518794138573,1.66193958224869,0.715788047739679,-1.33924587209726,0.526767262730371,0.468183100319498 +1.066,0.596742517039433,0.734475890529248,1.66187434476322,0.715343980760245,-1.33924406940371,0.526979048264279,0.468381215148247 +1.068,0.596986116267167,0.734440308076518,1.66181975077239,0.714973519676534,-1.33924282853819,0.52715566678343,0.46854625768223 +1.07,0.597185769936912,0.734411384472169,1.66177487697399,0.714670200769406,-1.33924206802949,0.52730021222822,0.468681149701363 +1.072,0.597345713713731,0.734388460456575,1.66173879110216,0.714427524711745,-1.33924171715626,0.527415793581198,0.468788823137707 +1.074,0.597470209169014,0.734370879268795,1.66171055355451,0.714238962992904,-1.33924171401945,0.527505532162211,0.468872218222402 +1.076,0.597563538601069,0.73435798615043,1.66168921886482,0.71409796375318,-1.33924200379182,0.52757255916785,0.468934281808296 +1.078,0.597630000400889,0.734349127903667,1.66167383701822,0.713997957007945,-1.33924253715056,0.527620013465371,0.468977965872043 +1.08,0.597673904976121,0.734343652503911,1.66166345460603,0.713932359245073,-1.33924326889796,0.527651039649255,0.469006226198644 +1.082,0.597699571243345,0.734340908767259,1.66165711581861,0.713894577383047,-1.33924415677389,0.527668786366719,0.469022021250757 +1.084,0.597711323696132,0.734340246073043,1.66165386327444,0.713878012080344,-1.33924516046282,0.527676404916871,0.469028311224472 +1.086,0.597713490054134,0.734341014141577,1.66165273868446,0.713876060389526,-1.33924624079735,0.527677048126806,0.469028057292741 +1.088,0.59771039949657,0.734342562867199,1.6616527833512,0.713882117751796,-1.33924735915956,0.527673869506765,0.469024221037236 +1.08944271909999,0.597707380219204,0.734343792421707,1.66165299416632,0.713887684591453,-1.33924816665973,0.527671006538038,0.469020923291632 diff --git a/data/ik_movel_scurve_tcp.csv b/data/ik_movel_scurve_tcp.csv index 2eb2aafe..2832757f 100644 --- a/data/ik_movel_scurve_tcp.csv +++ b/data/ik_movel_scurve_tcp.csv @@ -1,547 +1,547 @@ t,px,py,pz,vx,vy,vz,ax,ay,az,speed,acc -0,0.559809314390095,-0.292914056655931,0.00429163619695835,0,0,0,-2.63539190470397e-08,1.86795023893183e-08,0.0499999791552658,0,0.0499999791552762 -0.002,0.559809314390095,-0.292914056655931,0.00429163619695835,-5.27078380940793e-11,3.73590047786365e-11,9.99999583105315e-05,-2.55427623496729e-07,1.76723219391661e-07,0.124999884896886,9.99999583105524e-05,0.124999884897271 -0.004,0.559809314389884,-0.292914056655782,0.0042920361967916,-1.02171049398692e-09,7.06892877566645e-10,0.000499999539587542,-1.48488860096663e-06,1.02245295563463e-06,0.299999433996979,0.000499999539589086,0.299999434002397 -0.006,0.559809314386008,-0.292914056653104,0.0042936361951167,-5.99226224196059e-09,4.12717082731717e-09,0.00129999769429845,-5.00186697616201e-06,3.43832948224154e-06,0.499998216783697,0.00129999769431881,0.499998216820538 -0.008,0.559809314365915,-0.292914056639273,0.00429723618756879,-2.10291783986349e-08,1.44602108065328e-08,0.00249999240672233,-1.23088275683081e-05,8.45479589117737e-06,0.699995739690193,0.00249999240685259,0.699995739849473 -0.01,0.559809314301891,-0.292914056595263,0.00430363616474359,-5.52275725151929e-08,3.79463543920266e-08,0.00409998065305922,-2.49193027324068e-05,1.71093902934238e-05,0.899991498596285,0.00409998065360679,0.899991499103903 -0.012,0.559809314145005,-0.292914056487488,0.00431363611018103,-1.20706389328262e-07,8.28977719802282e-08,0.00609995840110747,-4.4341134930459e-05,3.04341309098621e-05,1.0999849891823,0.00609995840286503,1.09998499049703 -0.014,0.559809313819066,-0.292914056263672,0.00432803599834802,-2.32592112237029e-07,1.59682878031475e-07,0.00849992060978841,-7.20702525436323e-05,4.94503847592309e-05,1.29997570688222,0.00849992061447067,1.29997570982053 -0.016,0.559809313214636,-0.292914055848756,0.00434763579262018,-4.08987399502791e-07,2.80699311017152e-07,0.0112998612286363,-0.00010958457058452,7.51633512374726e-05,1.49996314679785,0.0112998612395242,1.4999631526841 -0.018,0.559809312183116,-0.292914055140874,0.00437323544326257,-6.70930394575108e-07,4.60336282981366e-07,0.0144997731969798,-0.000158344372336305,0.000108560615358355,1.69994680364674,0.0144997732198097,1.69994681448777 -0.02,0.559809310530915,-0.292914054007411,0.0044056348854081,-1.04236488884801e-06,7.14941772450572e-07,0.0180996484432233,-0.000219812813784248,0.000150624648170838,1.89992617181771,0.0180996484873586,1.89992619050408 -0.022,0.559809308013656,-0.292914052281107,0.00444563403703546,-1.5501816497121e-06,1.06283487566472e-06,0.0220994778842506,-0.000295522363524813,0.000202376244234914,2.09990074540641,0.0220994779641774,2.099900775953 -0.024,0.559809304330188,-0.292914049756072,0.0044940327969451,-2.22445434294727e-06,1.52444674939023e-06,0.0264992514248489,-0.000387126351808486,0.000264907408509262,2.29987001800109,0.0264992515620629,2.29987006583918 -0.026,0.559809299115839,-0.29291404618332,0.00455163104273486,-3.09868705694605e-06,2.12246450970177e-06,0.031298957956255,-0.000496138317929162,0.000339199945909029,2.49983348153334,0.0312989581816094,2.49983355378012 -0.028,0.55980929193544,-0.292914041266214,0.00461922862877012,-4.20900761466391e-06,2.88124653302635e-06,0.0364985853509823,-0.000623858811832355,0.000426072475911975,2.69979062634834,0.0364985857073979,2.69979073204868 -0.03,0.559809282279809,-0.292914034658334,0.00469762538413879,-5.59412230427547e-06,3.82675441334967e-06,0.0420981204616484,-0.000771733545124052,0.000526422933022629,2.89974094256316,0.0420981210072577,2.89974109304104 -0.032,0.55980926955895,-0.292914025959196,0.00478762111061672,-7.29594179516013e-06,4.98693826511686e-06,0.048097549121235,-0.00094130906541645,0.000641194729256166,3.09968391953071,0.04809754993313,3.09968412877678 -0.034,0.559809253096041,-0.292914014710581,0.00489001558062373,-9.35935856594127e-06,6.39153333037434e-06,0.0544968561397712,-0.00113406957436002,0.000771261061960348,3.2996190451268,0.0544968573182731,3.29961933015324 -0.036,0.559809232121516,-0.292914000393063,0.0050056085351758,-1.18322200926002e-05,8.07198251295826e-06,0.0612960253017422,-0.00135149165469972,0.000917458508470358,3.4995458059097,0.0612960269752465,3.49954618713913 -0.038,0.559809205767161,-0.292913982422651,0.0051351996818307,-1.47653251847401e-05,1.00613673642558e-05,0.0684950393634101,-0.00159505035568497,0.00108058697059765,3.69946368715748,0.0684950416938411,3.69946418883203 -0.04,0.559809173060215,-0.292913960147593,0.00527958869262944,-1.82124215153401e-05,1.23943303953489e-05,0.0760938800503721,-0.00186621951225873,0.00126140530312679,3.89937217291219,0.0760938832392732,3.89937282351926 -0.042,0.559809132917475,-0.29291393284533,0.00543957520203218,-2.22302032337751e-05,1.51069885767629e-05,0.0840925280550588,-0.00216647180056872,0.00146062667516489,4.09927074607359,0.0840925323503454,4.09927157878629 -0.044,0.559809084139403,-0.292913899719639,0.00561595880484968,-2.6878308717615e-05,1.82368370960084e-05,0.0924909630346664,-0.00248362769933452,0.00166967401363238,4.27872692292541,0.0924909687380651,4.27872796952373 -0.046,0.55980902540424,-0.292913859897981,0.00580953905417085,-3.21647140311132e-05,2.17856846312925e-05,0.10120743574676,-0.00274913172781943,0.00184189907423415,4.34426804258366,0.101207443202659,4.34426930290136 -0.048,0.559808955480546,-0.2929138125769,0.00602078854783672,-3.78748356288927e-05,2.5604433392945e-05,0.109868035205001,-0.00290092068422342,0.00193541701762334,4.23678787821439,0.109868044716822,4.23678931340281 -0.05,0.559808873904898,-0.292913757480248,0.00624901119499086,-4.37683967680069e-05,2.95273527017858e-05,0.118154587259618,-0.0029728832104392,0.00197356798958825,4.04326623543778,0.118154599055762,4.04326781003144 -0.052,0.559808780406959,-0.292913694467489,0.00649340689687519,-4.97663684706495e-05,3.3498705351298e-05,0.126041100146752,-0.00301010266018364,0.00198718631133632,3.84325163507032,0.126041114423264,3.84325332759865 -0.054,0.559808674839424,-0.292913623485426,0.00675317559557786,-5.58088074087415e-05,3.74760979471311e-05,0.133527593799899,-0.00301813474212764,0.00198024868724999,3.64324675648873,0.133527610721797,3.64324854480007 -0.056,0.55980855717173,-0.292913544563098,0.00702751727207479,-6.183890743916e-05,4.1419700100298e-05,0.140614087172707,-0.00299854609198124,0.00195404343747429,3.44325110939336,0.140614106870739,3.44325296949019 -0.058,0.559808427483794,-0.292913457806626,0.00731563194426869,-6.78029917766664e-05,4.52922716970283e-05,0.147300598237473,-0.00295289934171272,0.00190987015785682,3.2432642025357,0.147300620805728,3.24326610913568 -0.06,0.559808285959763,-0.292913363394011,0.00761671966502468,-7.36505048060109e-05,4.90591807317253e-05,0.15358714398285,-0.00288275330689868,0.00184903496680544,3.04328554332714,0.153587169477169,3.04328747038806 -0.062,0.559808132881775,-0.292913261569903,0.00792998052020009,-7.93340050042612e-05,5.268841156425e-05,0.159473740410781,-0.00278966325734097,0.0017728460054156,2.84331463755536,0.159473768847915,2.84331655876383 -0.064,0.559807968623743,-0.292913152640365,0.00825461462666781,-8.48091578353748e-05,5.61505647533877e-05,0.164960402533071,-0.00267518104890562,0.00168260933658404,2.64335098915363,0.164960433890531,2.64335287837763 -0.066,0.559807793645143,-0.292913036967644,0.00858982213033238,-9.00347291998837e-05,5.94188489105862e-05,0.170047144367396,-0.00254085526230074,0.00157962513208653,2.44339409995359,0.170047178583939,2.44339593165927 -0.068,0.559807608484826,-0.292912914964969,0.00893480320413739,-9.49725788845778e-05,6.24690652817339e-05,0.174733978932886,-0.00238823143206001,0.00146518393945305,2.24344346956561,0.174734015909591,2.24344521919969 -0.07,0.559807413754828,-0.292912787091383,0.00928875804606392,-9.95876549281237e-05,6.52795846683984e-05,0.179020918245658,-0.0022188522755262,0.00134056322639853,2.04349859530758,0.179020957847521,2.04350023964747 -0.0720000000000001,0.559807210134206,-0.29291265384663,0.00965088687712003,-0.000103847987986683,6.7831318187328e-05,0.182907973314116,-0.00203425765121778,0.00120702442832354,1.84355897213297,0.182908015372126,1.84356048960812 -0.0740000000000001,0.559806998362876,-0.29291251576611,0.0100203899393204,-0.000107724685532995,7.01076823816926e-05,0.18639515413419,-0.00183598489189585,0.00106581003744832,1.64362409268547,0.186395198447824,1.64362546367708 -0.0760000000000001,0.559806779235464,-0.292912373415901,0.0103964674936568,-0.000111191927554266,7.20945583371212e-05,0.189482469684858,-0.0016255689225253,0.000918140882766315,1.44369344735103,0.189482516024944,1.44369465448249 -0.0780000000000001,0.559806553595166,-0.292912227387877,0.0107783198180598,-0.000114226961223096,7.37802459127578e-05,0.192169927923594,-0.00140454227415265,0.000765213854087009,1.24376652437842,0.192169976035495,1.24376755282312 -0.0800000000000001,0.559806322327619,-0.292912078294917,0.0111651472053512,-0.000116810096650877,7.51554137534693e-05,0.194457535782372,-0.00117443541003403,0.000608199858531666,1.04384281008479,0.19445758538943,1.04384364795276 -0.0820000000000001,0.559806086354779,-0.292911926766222,0.0115561499611893,-0.000118924702863232,7.62130453468845e-05,0.196345299163934,-0.000936776670124039,0.000448241926215287,0.843921789023954,0.196345349971136,0.843922427987754 -0.0840000000000001,0.559805846628808,-0.292911773442736,0.0119505284020069,-0.000120557203331373,7.69483814583304e-05,0.197833222938468,-0.000693092486181434,0.000286453628178806,0.644002944286473,0.197833274636281,0.644003380955815 -0.0860000000000001,0.559805604125966,-0.292911618972696,0.0123474828529432,-0.000121697072807958,7.73588598595997e-05,0.198921310941079,-0.000444907499730362,0.000123917771877036,0.444085757754682,0.198921363209406,0.444085997909017 -0.0880000000000001,0.559805359840516,-0.292911464007296,0.0127462136457712,-0.000122336833330294,7.74440525458386e-05,0.199609565969486,-0.000196202408486723,-3.67662289502135e-05,0.2461099028149,0.199609618481697,0.246109983768843 -0.0900000000000001,0.559805114778632,-0.292911309196486,0.0131459211168211,-0.000122481882441905,7.72117949437989e-05,0.199905750552339,4.69950467429924e-06,-0.00016523175189187,0.0870502566792038,0.199905802985723,0.0870504136206856 -0.0920000000000001,0.559804869912987,-0.292911155160117,0.0135458366479806,-0.000122318035311597,7.67831255382711e-05,0.199957766996203,9.82884745814871e-05,-0.000224601264670054,0.0130049113153743,0.199957819150585,0.0130072220198272 -0.0940000000000001,0.559804625506491,-0.292911002063983,0.0139457521848059,-0.000122088728543579,7.63133898851186e-05,0.199957770197601,0.000114519255189904,-0.000234676850868753,1.78492637337157e-06,0.199957822032016,0.000261134199416859 -0.0960000000000001,0.559804381558072,-0.292910849906557,0.014345667728771,-0.000121859958290837,7.58444181347961e-05,0.199957774135909,0.000114251323679593,-0.000234295319256894,2.15339465009512e-06,0.199957825652342,0.000260676617088583 -0.0980000000000001,0.559804138066658,-0.292910698686311,0.0147455832813496,-0.000121631723248861,7.53762086080911e-05,0.199957778811179,0.000113984127692035,-0.000233914620312303,2.52190282545861e-06,0.199957830011604,0.000260220658202657 -0.1,0.559803895031179,-0.292910548401723,0.0151454988440157,-0.000121404021780069,7.49087596535469e-05,0.19995778422352,0.000113717694982807,-0.000233534701993276,2.89047431822897e-06,0.199957835109901,0.000259766291171652 -0.102,0.559803652450571,-0.292910399051272,0.0155454144182437,-0.000121176852468929,7.4442069800118e-05,0.199957790373076,0.000113451942285181,-0.000233155553891473,3.25909698534188e-06,0.199957840947372,0.000259313472908523 -0.104,0.55980341032377,-0.292910250633443,0.015945330005508,-0.000120950214010929,7.3976137437981e-05,0.199957797259908,0.000113186841843582,-0.000232777203762468,3.62776302054168e-06,0.199957847524066,0.000258862218994065 -0.106,0.559803168649715,-0.292910103146723,0.0163452456072833,-0.000120724105101555,7.35109609850681e-05,0.199957804884129,0.000112922435291374,-0.0002323996099729,3.99648543425446e-06,0.199957854840089,0.000258412513248713 -0.108,0.559802927427349,-0.292909956589599,0.0167451612250445,-0.000120498524269763,7.30465389980894e-05,0.19995781324585,0.000112658736506344,-0.000232022758644978,4.36525815494803e-06,0.199957862895544,0.000257964352031809 -0.11,0.559802686655618,-0.292909810960567,0.0171450768602667,-0.00012027347015553,7.25828699504882e-05,0.199957822345161,0.000112395703855128,-0.000231646635900917,4.73407077428156e-06,0.199957871690513,0.000257517707343393 -0.112,0.559802446333469,-0.29290966625812,0.0175449925144252,-0.000120048941454343,7.21199524544857e-05,0.199957832182133,0.000112133330398834,-0.00023127124867961,5.1029180880846e-06,0.199957881225057,0.000257072585129518 -0.114,0.559802206459852,-0.292909522480757,0.0179449081889952,-0.000119824936833934,7.16577849557697e-05,0.199957842756834,0.000111871609198566,-0.000230896607389397,5.47182351512409e-06,0.199957891499236,0.000256628995051593 -0.116,0.559801967033721,-0.29290937962698,0.0183448238854525,-0.000119601455017548,7.11963660249281e-05,0.199957854069427,0.000111610554132113,-0.000230522646110787,5.840770575527e-06,0.199957902513206,0.000256186886402314 -0.118,0.559801728054032,-0.292909237695293,0.0187447396052729,-0.000119378494617406,7.07356943713266e-05,0.199957866119916,0.000111350151321687,-0.000230149364843779,6.20973367865262e-06,0.199957914266962,0.000255746255358152 -0.12,0.559801489519743,-0.292909096684202,0.0191446553499322,-0.000119156054412262,7.0275768565553e-05,0.199957878908361,0.000111090400767289,-0.00022977680175229,6.57873061582492e-06,0.199957926760556,0.00025530713946669 -0.122,0.559801251429814,-0.292908956592218,0.0195445711209064,-0.000118934133014337,6.98165871643174e-05,0.199957892434838,0.000110831288591024,-0.000229404925611237,6.94777022719472e-06,0.199957939994055,0.000254869507663381 -0.124,0.559801013783211,-0.292908817417854,0.0199444869196715,-0.000118712729257897,6.9358148863108e-05,0.199957906699442,0.000110572759281952,-0.000229033698256826,7.31685996513411e-06,0.199957953967547,0.000254433304565603 -0.126,0.559800776578897,-0.292908679159623,0.0203444027477041,-0.000118491841977209,6.89004523712901e-05,0.199957921702278,0.000110314882229118,-0.000228663116219666,7.68595716238443e-06,0.199957968681129,0.000253998558850271 -0.128,0.559800539815843,-0.292908541816044,0.0207443186064806,-0.000118271469728981,6.84434963982294e-05,0.199957937443271,0.000110057664370993,-0.000228293169091178,8.05507032602958e-06,0.199957984134717,0.000253565267137376 -0.13,0.559800303493018,-0.292908405385637,0.0211442344974772,-0.000118051611319725,6.79872796949254e-05,0.199957953922559,0.00010980104325764,-0.00022792387421866,8.42422789859575e-06,0.199958000328443,0.000253133421647552 -0.132,0.559800067609397,-0.292908269866925,0.0215441504221709,-0.00011783226555595,6.75318009013547e-05,0.199957971140183,0.000109544998072586,-0.000227555214254993,8.79339396436806e-06,0.199958017262337,0.000252702999417837 -0.134,0.559799832163956,-0.292908135258434,0.021944066382038,-0.000117613431327435,6.70770588379054e-05,0.199957989096135,0.000109289549632407,-0.00022718714062786,9.1625812839724e-06,0.199958034936387,0.000252273968972827 -0.136,0.559799597155672,-0.29290800155869,0.0223439823785554,-0.000117395107357421,6.66230523388433e-05,0.199958007790508,0.000109034739570468,-0.00022681966721505,9.53179722651409e-06,0.199958053350674,0.000251846363934668 -0.138,0.559799362583527,-0.292907868766224,0.0227438984132,-0.000117177292369153,6.61697801690452e-05,0.199958027223324,0.000108780512375617,-0.00022645279748601,9.90101403641751e-06,0.199958072505216,0.000251420165143711 -0.14,0.559799128446503,-0.292907736879569,0.0231438144874487,-0.000116959985307918,6.57172411488993e-05,0.199958047394564,0.000108526833353384,-0.000226086517562951,1.02702395199383e-05,0.199958092399984,0.00025099534816656 -0.142,0.559798894743585,-0.29290760589726,0.0235437306027782,-0.000116743185035739,6.52654340987934e-05,0.199958068304282,0.000108273751076027,-0.000225720803159746,1.06394866875026e-05,0.199958113035026,0.000250571915481619 -0.144,0.559798661473763,-0.292907475817833,0.0239436467606658,-0.000116526890303614,6.48143579362603e-05,0.199958089952511,0.000108021272482439,-0.000225355636929159,1.10087173751938e-05,0.199958134410365,0.000250149855612029 -0.146,0.559798428636024,-0.292907346639828,0.0243435629625883,-0.000116311099945809,6.43640115510768e-05,0.199958112339152,0.000107769335122576,-0.000224991025810084,1.13779502278199e-05,0.199958156525896,0.000249729151360474 -0.148,0.559798196229363,-0.292907218361787,0.0247434792100224,-0.000116095812963124,6.39143938330199e-05,0.199958135464312,0.000107517952874225,-0.000224626952455287,1.17472082408754e-05,0.199958179381718,0.000249309796961668 -0.15,0.559797964252772,-0.292907090982253,0.0251433955044455,-0.000115881028134312,6.34655037412556e-05,0.199958159327985,0.000107267139615175,-0.000224263389109191,1.21164614799718e-05,0.199958202977819,0.000248891774823431 -0.152,0.55979773270525,-0.292906964499772,0.0255433118473344,-0.000115666744404663,6.30173402765832e-05,0.199958183930157,0.000107016881467636,-0.00022390035658848,1.24857069128126e-05,0.199958227314176,0.000248475100315141 -0.154,0.559797501585795,-0.292906838912892,0.0259432282401662,-0.000115452960608442,6.25699023149017e-05,0.199958209270812,0.000106767171492716,-0.000223537854893152,1.28549289268864e-05,0.199958252390765,0.000248059772386621 -0.156,0.559797270893408,-0.292906714220163,0.0263431446844176,-0.000115239675718692,6.21231888570106e-05,0.199958235349873,0.000106517974995945,-0.000223175828512056,1.3224138797896e-05,0.199958278207502,0.000247645729372072 -0.158,0.559797040627092,-0.292906590420136,0.0267430611815657,-0.000115026888708458,6.16771990008535e-05,0.199958262167368,0.000106269340549581,-0.000222814287853534,1.35933391279264e-05,0.199958304764408,0.000247233004433403 -0.16,0.559796810785853,-0.292906467511367,0.0271429777330871,-0.000114814598356494,6.12319317055964e-05,0.19995828972323,0.000106021233459153,-0.000222453222509244,1.39625143044664e-05,0.199958332061409,0.00024682157514012 -0.162,0.559796581368698,-0.292906345492409,0.0275428943404586,-0.000114602803774622,6.07873861108165e-05,0.199958318017425,0.00010577363984687,-0.000222092608193058,1.4331685581348e-05,0.199958360098463,0.000246411417576486 -0.164,0.559796352374638,-0.292906224361822,0.0279428110051568,-0.000114391503797107,6.03435612728242e-05,0.199958347049972,0.000105526594407209,-0.000221732483068893,1.47008277912031e-05,0.199958388875582,0.00024600258232959 -0.166,0.559796123802683,-0.292906104118164,0.0283427277286585,-0.000114180697396993,5.9900456178541e-05,0.199958376820736,0.000105280027751231,-0.000221372788156149,1.50699175360813e-05,0.199958418392624,0.000245594987721393 -0.168,0.559795895651848,-0.292905984759997,0.0287426445124397,-0.000113970383686102,5.94580701201996e-05,0.199958407329642,0.000105033967634505,-0.000221013485290911,1.54390025070006e-05,0.199958448649507,0.000245188616856274 -0.17,0.559795667921149,-0.292905866285884,0.029142561357977,-0.000113760561526455,5.90164022373773e-05,0.199958438576746,0.000104788455690397,-0.00022065464039267,1.58080597223442e-05,0.199958479646279,0.00024478354831357 -0.172,0.559795440609602,-0.292905748694388,0.0295424782667467,-0.00011355122986334,5.85754515586289e-05,0.199958470561881,0.000104543429468862,-0.000220296236114192,1.61770449466636e-05,0.199958511382767,0.000244379739492449 -0.174,0.559795213716229,-0.292905631984078,0.0299423952402246,-0.000113342387808579,5.81352172929205e-05,0.199958503284926,0.000104298861214325,-0.000219938199597092,1.65459972112369e-05,0.199958543858841,0.000243977118017918 -0.176,0.559794987240051,-0.292905516153519,0.0303423122798864,-0.000113134034418483,5.76956987602406e-05,0.19995853674587,0.000104054806437936,-0.000219580544719155,1.69149208528729e-05,0.199958577074485,0.000243575723152896 -0.178,0.559794761180091,-0.292905401201283,0.030742229387208,-0.000112926168582827,5.72568951140439e-05,0.199958570944609,0.000103811258200803,-0.000219223306174852,1.72837889833577e-05,0.199958611029588,0.000243175584015106 -0.18,0.559794535535377,-0.292905287125939,0.0311421465636649,-0.000112718789385679,5.68188055355411e-05,0.199958605881026,0.000103568140175092,-0.000218866418044692,1.76526024700529e-05,0.199958645724024,0.0002427766112726 -0.182,0.559794310304934,-0.29290517392606,0.0315420638107321,-0.000112511896022127,5.63814294418651e-05,0.199958641555019,0.000103325507871954,-0.000218509880328676,1.80213673844909e-05,0.199958681157686,0.000242378831709076 -0.184,0.559794085487793,-0.292905061600221,0.031941981129885,-0.000112305487354192,5.59447660142264e-05,0.199958677966495,0.000103083389046965,-0.000218153703435142,1.83900715836072e-05,0.199958717330473,0.000241982268260974 -0.186,0.559793861082984,-0.292904950146996,0.0323418985225981,-0.000112099562465939,5.55088146281246e-05,0.199958715115305,0.000102841707372292,-0.000217797859608515,1.8758701189614e-05,0.199958754242229,0.000241586864907055 -0.188,0.559793637089543,-0.292904839564962,0.0327418159903462,-0.000111894120524703,5.50735745757924e-05,0.1999587530013,0.000102600476725723,-0.00021744236619603,1.91272601091086e-05,0.199958791892799,0.000241192646074783 -0.19,0.559793413506502,-0.292904729852698,0.0331417335346033,-0.000111689160559036,5.46390451633405e-05,0.199958791624346,0.000102359704046151,-0.000217087178094877,1.94957583063426e-05,0.199958830282042,0.000240799577444712 -0.192,0.5597931903329,-0.292904621008782,0.0335416511568436,-0.000111484681708518,5.42052258634129e-05,0.199958830984333,0.000102119396272471,-0.000216732288366161,1.98641892865114e-05,0.199958869409841,0.000240407657805439 -0.194,0.559792967567776,-0.292904513031795,0.0339415688585407,-0.000111280682973946,5.37721160098758e-05,0.199958871081103,0.000101879546465788,-0.000216377707418225,2.02325326631447e-05,0.199958909276031,0.000240016894475382 -0.196,0.559792745210169,-0.292904405920318,0.034341486641168,-0.000111077163522655,5.333971503374e-05,0.199958911914464,0.000101640106053846,-0.000216023407495491,2.06007893036042e-05,0.199958949880414,0.00023962724441209 -0.198,0.559792523259121,-0.292904299672934,0.0347414045061985,-0.000110874122549731,5.29080223798938e-05,0.19995895348426,0.000101401095853326,-0.000215669385128514,2.09689583405281e-05,0.199958991222827,0.000239238715796281 -0.2,0.559792301713678,-0.292904194288228,0.035141322455105,-0.000110671559139241,5.24770374932259e-05,0.199958995790297,0.000101162543619804,-0.000215315633378399,2.13370284982139e-05,0.199959033303068,0.000238851315662911 -0.202,0.559792080572885,-0.292904089764784,0.0355412404893597,-0.000110469472375252,5.20467598463802e-05,0.199959038832374,0.000100924421597703,-0.000214962141836805,2.1705008450279e-05,0.199959076120932,0.000238465026135332 -0.204,0.559791859835789,-0.292903986101189,0.0359411586104345,-0.000110267861452851,5.16171889258787e-05,0.199959082610331,0.000100686715909237,-0.000214608896625945,2.20729077377024e-05,0.19995911967625,0.000238079832257145 -0.206,0.559791639501439,-0.292903883296029,0.036341076819801,-0.000110066725511615,5.11883242598765e-05,0.199959127124005,0.000100449440432193,-0.000214255869990243,2.24406942680999e-05,0.199959163968853,0.000237695714449485 -0.208,0.559791419568887,-0.292903781347892,0.0367409951189305,-0.000109866063691122,5.07601654459177e-05,0.199959172373108,0.000100212567410995,-0.000213903075807486,2.28083602352158e-05,0.199959208998447,0.000237312675185347 -0.21,0.559791200037184,-0.292903680255367,0.0371409135092935,-0.000109665875241971,5.03327119566465e-05,0.199959218357446,9.99760829678563e-05,-0.000213550524486017,2.31759281904553e-05,0.199959254764832,0.000236930722620216 -0.212,0.559790980905386,-0.292903580017044,0.0375408319923603,-0.00010946615935925,4.99059633479737e-05,0.199959265076821,9.97400287361393e-05,-0.000213198143167446,2.35433773171367e-05,0.199959301267802,0.000236549809126598 -0.214,0.559790762172547,-0.292903480631514,0.0379407505696008,-0.000109266915127026,4.94799193839767e-05,0.199959312530955,9.95043908380565e-05,-0.000212845935321225,2.39107006763661e-05,0.199959348507074,0.000236169933703794 -0.216,0.559790543837725,-0.292903382097366,0.0383406692424841,-0.000109068141795898,4.90545796066888e-05,0.199959360719624,9.92691345791385e-05,-0.000212493946050163,2.42779017375905e-05,0.199959396482416,0.000235791125152837 -0.218,0.55979032589998,-0.292903284413195,0.0387405880124792,-0.00010886983858871,4.86299435997761e-05,0.199959409642562,9.90342807760668e-05,-0.000212142137190341,2.46449726945541e-05,0.199959445193557,0.000235413359475696 -0.22,0.559790108358371,-0.292903187577592,0.0391405068810544,-0.000108672004672794,4.82060110579274e-05,0.199959459299514,9.8799801673266e-05,-0.000211790484455632,2.50119161493423e-05,0.199959494640235,0.000235036605800223 -0.222,0.559789891211961,-0.292903091589151,0.0395404258496773,-0.000108474639382017,4.77827816619536e-05,0.199959509690227,9.85657042096299e-05,-0.000211438980907141,2.53787225609758e-05,0.19995954482219,0.000234660862208034 -0.224,0.559789674459813,-0.292902996446465,0.0399403449198153,-0.000108277741855955,4.73602551342988e-05,0.199959560814405,9.83320092018402e-05,-0.000211087605728188,2.57453806537522e-05,0.199959595739121,0.000234286119842249 -0.226,0.559789458100994,-0.29290290214813,0.0403402640929349,-0.000108081311345209,4.69384312390408e-05,0.19995961267175,9.80986888943213e-05,-0.000210736358918773,2.61118834887774e-05,0.199959647390723,0.000233912368617946 -0.228,0.559789242134568,-0.29290280869274,0.0407401833705023,-0.000107885347100378,4.65173096986237e-05,0.199959665261939,9.78657363481793e-05,-0.000210385240478894,2.6478239739669e-05,0.199959699776667,0.000233539608936139 -0.23,0.559789026559605,-0.292902716078892,0.0411401027539827,-0.000107689848399817,4.60968902771252e-05,0.199959718584708,9.76331446245204e-05,-0.000210034236530765,2.6844455477959e-05,0.199959752896683,0.000233167828471772 -0.232,0.559788811375174,-0.292902624305179,0.0415400222448411,-0.00010749481452188,4.56771727525007e-05,0.199959772639761,9.74009206622383e-05,-0.000209683340135491,2.7210500776198e-05,0.199959806750467,0.000232797022808968 -0.234,0.559788596580347,-0.292902533370201,0.0419399418445417,-0.000107300244717168,4.52581569165833e-05,0.199959827426712,9.71690852780149e-05,-0.000209332530476391,2.75763734763901e-05,0.199959861337628,0.000232427183916866 -0.236,0.559788382174196,-0.292902443272552,0.042339861554548,-0.000107106138180768,4.48398426305951e-05,0.199959882945254,9.69375898995927e-05,-0.000208981790206231,2.79420818080633e-05,0.199959916657855,0.000232058279122101 -0.238,0.559788168155794,-0.29290235401083,0.0427397813763227,-0.000106912494357569,4.44222297557583e-05,0.199959939195039,9.6706420649184e-05,-0.000208631115855562,2.83076175347507e-05,0.199959972710789,0.000231690300850958 -0.24,0.559787954524218,-0.292902265583633,0.0431397013113281,-0.000106719312498171,4.40053181671729e-05,0.199959996175725,9.64756538546218e-05,-0.000208280500485491,2.86729706852617e-05,0.199960029496086,0.000231323275778901 -0.242,0.559787741278544,-0.292902177989557,0.0435396213610256,-0.000106526591742151,4.35891077538164e-05,0.199960053886922,9.62452201269669e-05,-0.000207929937157125,2.90381369089099e-05,0.199960087013348,0.000230957170398745 -0.244,0.559787528417851,-0.292902091227202,0.0439395415268758,-0.000106334331617663,4.31735984185444e-05,0.199960112328272,9.60150986495378e-05,-0.000207579415462122,2.9403130534511e-05,0.199960145262212,0.000230591970756803 -0.246,0.559787315941218,-0.292902005295164,0.0443394618103387,-0.000106142531347553,4.27587900919679e-05,0.199960171499444,9.57853379944086e-05,-0.000207228900705939,2.97679254510075e-05,0.199960204242339,0.000230227664819308 -0.248,0.559787103847726,-0.292901920192041,0.0447393822128736,-0.000105951190265686,4.23446828157206e-05,0.199960231399974,9.55559034674766e-05,-0.000206878417174856,3.01325010082511e-05,0.199960263953262,0.00022986425950389 -0.25,0.559786892136457,-0.292901835916432,0.0451393027359386,-0.000105760307733683,4.19312764232685e-05,0.199960292029448,9.5326815885605e-05,-0.000206527971807831,3.0496879417641e-05,0.199960324394557,0.000229501774764725 -0.252,0.559786680806495,-0.292901752466936,0.0455392233809914,-0.000105569883002143,4.15185709284893e-05,0.199960353387492,9.5098075248062e-05,-0.000206177474398954,3.08610572860579e-05,0.199960385565848,0.000229140131191005 -0.254,0.559786469856925,-0.292901669842149,0.0459391441494886,-0.000105379915432691,4.11065665256726e-05,0.199960415473677,9.48696399216686e-05,-0.00020582695270388,3.12250286391147e-05,0.199960447466697,0.000228779337903898 -0.256,0.559786259286833,-0.29290158804067,0.0463390650428861,-0.000105190404442457,4.06952631176738e-05,0.199960478287606,9.46415099067898e-05,-0.000205476444886665,3.1588783672154e-05,0.199960510096704,0.000228419430116148 -0.258,0.559786049095307,-0.292901507061096,0.046738986062639,-0.000105001349393063,4.0284660746126e-05,0.199960541828812,9.44137129588184e-05,-0.000205125888497193,3.19523010480771e-05,0.199960573455392,0.00022806036233572 -0.26,0.559785839281436,-0.292901426902027,0.0471389072102013,-0.000104812749590621,3.9874759563685e-05,0.19996060609681,9.41862490777544e-05,-0.000204775276596569,3.23155825016075e-05,0.199960637542274,0.000227702130628935 -0.262,0.559785629844309,-0.292901347562058,0.0475388284870263,-0.000104624604396752,3.94655596397397e-05,0.199960671091142,9.39590905080222e-05,-0.000204424616123688,3.26786427778946e-05,0.199960702356883,0.000227344733961922 -0.264,0.559785420783018,-0.292901269039788,0.0479387498945659,-0.000104436913228589,3.90570610991902e-05,0.199960736811382,9.3732223371834e-05,-0.000204073875853527,3.30414758054065e-05,0.199960767898789,0.000226988139838864 -0.266,0.559785212096656,-0.292901191333814,0.0483386714342718,-0.000104249675503265,3.86492641363256e-05,0.199960803257045,9.35056754247653e-05,-0.000203723055786087,3.34040607674612e-05,0.199960834167502,0.000226632358835226 -0.268,0.559785003784316,-0.292901114442731,0.0487385931075941,-0.00010406289052689,3.82421688760459e-05,0.199960870427625,9.32794397279223e-05,-0.000203372176738048,3.37663902949536e-05,0.199960901162507,0.000226277407755786 -0.27,0.559784795845094,-0.292901038365138,0.0491385149159823,-0.000103876557744353,3.78357754293734e-05,0.199960938322606,9.30534815868355e-05,-0.000203021176259366,3.41284656785179e-05,0.199960968883285,0.000225923218405585 -0.272,0.559784588278085,-0.29290096309963,0.0495384368608845,-0.000103690676600543,3.74300841710084e-05,0.199961006941487,9.28278287570804e-05,-0.000202670037002805,3.44902908316902e-05,0.199961037329329,0.000225569789316138 -0.274,0.559784381082387,-0.292900888644801,0.0499383589437482,-0.000103505246429325,3.70250952813622e-05,0.199961076283769,9.2602488177551e-05,-0.000202318817948965,3.48518436332767e-05,0.199961106500132,0.000225217175005767 -0.276,0.559784174257099,-0.292900814999249,0.0503382811660196,-0.000103320266647833,3.66208088992126e-05,0.199961146348862,9.23774182148839e-05,-0.000201967477464482,3.52131214811923e-05,0.1999611763951,0.000224865322524089 -0.278,0.559783967801321,-0.292900742161566,0.0507382035291437,-0.000103135736756465,3.62172253715043e-05,0.199961217136255,9.21526396857607e-05,-0.000201615970446545,3.55741382532248e-05,0.199961247013716,0.000224514204122178 -0.28,0.559783761714152,-0.292900670130347,0.0511381260345646,-0.00010295165608909,3.58143450174264e-05,0.199961288645415,9.19281664679693e-05,-0.000201264303834048,3.59348809389481e-05,0.19996131835544,0.000224163831730417 -0.282,0.559783555994696,-0.292900598904186,0.0515380486837253,-0.000102768024090594,3.54121681561681e-05,0.199961360875779,9.17039430503585e-05,-0.000200912481096438,3.62953261195953e-05,0.199961390419706,0.000223814183932649 -0.284,0.559783350642056,-0.292900528481674,0.0519379714780677,-0.000102584840316888,3.50106950930406e-05,0.199961433826719,9.14800110662916e-05,-0.000200560495294821,3.66554842035071e-05,0.199961463205878,0.000223465275199048 -0.286,0.559783145655335,-0.292900458861405,0.0523378944190322,-0.000102402104046328,3.46099261749888e-05,0.199961507497716,9.12563982713443e-05,-0.000200208332551408,3.70153573556186e-05,0.199961536713431,0.000223117106759293 -0.288,0.55978294103364,-0.29290039004197,0.0527378175080586,-0.000102219814723803,3.4209861762835e-05,0.199961581888149,9.10330075210019e-05,-0.00019985596164118,3.73749165297199e-05,0.199961610941741,0.000222769607997687 -0.29,0.559782736776076,-0.292900322021958,0.0531377407465848,-0.000102037972016244,3.38105023284241e-05,0.199961656997382,9.08098596319461e-05,-0.000199503375625243,3.77341738619363e-05,0.199961685890164,0.000222422785184619 -0.292,0.559782532881752,-0.29290025479996,0.0535376641360481,-0.000101856575285275,3.3411848260334e-05,0.199961732824844,9.05870517486917e-05,-0.000199150598789721,3.80931228366465e-05,0.199961761558124,0.000222076700602005 -0.294,0.559782329349775,-0.292900188374565,0.0539375876778842,-0.00010167562380925,3.30138999332652e-05,0.199961809369873,9.03645006045117e-05,-0.000198797592970701,3.84517469947942e-05,0.199961837944953,0.000221731285089405 -0.296,0.559782126179256,-0.292900122744361,0.0543375113735276,-0.000101495117282857,3.26166578884512e-05,0.199961886631832,9.01421784438305e-05,-0.000198444326943159,3.88100545659075e-05,0.199961915050009,0.000221386502673337 -0.298,0.559781923369306,-0.292900057907934,0.0547374352244115,-0.000101315055095474,3.22201226254925e-05,0.199961964610092,8.99201407777993e-05,-0.000198090821523777,3.91680455430476e-05,0.199961992872656,0.000221042396569361 -0.3,0.559781720919036,-0.29289999386387,0.055137359231968,-0.000101135436719746,3.18242946023561e-05,0.199962043304014,8.96983459730548e-05,-0.00019773708365145,3.95256978188984e-05,0.199962071412253,0.00022069895402868 -0.302,0.559781518827559,-0.292899930610755,0.0555372833976276,-0.000100956261711582,3.14291742908867e-05,0.199962122712883,8.94768148462787e-05,-0.00019738306475392,3.98830014153306e-05,0.199962150668075,0.000220356140029228 -0.304,0.559781317093989,-0.292899868147173,0.0559372077228195,-0.000100777529460361,3.10347623433404e-05,0.19996220283602,8.92555543363648e-05,-0.000197028740545058,4.0239974977152e-05,0.19996223063944,0.000220013940846541 -0.306,0.559781115717441,-0.292899806471706,0.0563371322089717,-0.000100599239494237,3.06410593287065e-05,0.199962283672783,8.90345366877376e-05,-0.000196674138780439,4.0596608540111e-05,0.199962311325699,0.000219672370163356 -0.308,0.559780914697031,-0.292899745582936,0.0567370568575106,-0.00010042139131361,3.02480657882187e-05,0.199962365222454,8.88137341448213e-05,-0.000196319252521171,4.09528700048844e-05,0.199962392726131,0.000219331406426109 -0.31,0.559780714031876,-0.292899685479443,0.0571369816698615,-0.000100243984557657,2.98557823186218e-05,0.199962447484263,8.85931952798734e-05,-0.000195964054011677,4.13087728051708e-05,0.199962474839958,0.000218991048733181 -0.312,0.559780513721093,-0.292899626159806,0.0575369066474477,-0.00010006701853249,2.9464209572172e-05,0.199962530457545,8.83729617262573e-05,-0.000195608518965828,4.16643160944252e-05,0.199962557666511,0.000218651293826246 -0.314,0.559780313763802,-0.292899567622604,0.0579368317916917,-9.98904927107524e-05,2.90733482427585e-05,0.199962614141527,8.81529155227766e-05,-0.000195252661261413,4.20194768771531e-05,0.19996264120501,0.000218312103837281 -0.316,0.559780114159122,-0.292899509866413,0.0583367571040138,-9.97144068703991e-05,2.86831989271263e-05,0.199962698535453,8.7933077486113e-05,-0.000194896467020643,4.23742776978208e-05,0.199962725454694,0.000217973480918481 -0.318,0.559779914906174,-0.292899452889809,0.0587366825858335,-9.95387604008079e-05,2.82937623746759e-05,0.199962783638638,8.77135308829934e-05,-0.00019453990154905,4.27287085921768e-05,0.199962810414873,0.000217635427567828 -0.32,0.55977971600408,-0.292899396691364,0.0591366082385684,-9.93635527468672e-05,2.79050393209301e-05,0.199962869450287,8.74942202022665e-05,-0.000194182985663316,4.30827266223454e-05,0.199962896084747,0.000217297933315386 -0.322,0.559779517451963,-0.292899341269652,0.0595365340636346,-9.91887835199988e-05,2.75170304320227e-05,0.199962955969544,8.72751246272473e-05,-0.000193825702016204,4.3436350870285e-05,0.199962982463454,0.000216960979712257 -0.324,0.559779319248946,-0.292899286623242,0.0599364600624465,-9.90144522483582e-05,2.71297365128653e-05,0.199963043195691,8.70562649746241e-05,-0.000193468015913245,4.37896021526773e-05,0.19996306955027,0.000216624550107852 -0.326,0.559779121394154,-0.292899232750705,0.0603363862364174,-9.88405584601003e-05,2.67431583683697e-05,0.199963131127953,8.68376343055032e-05,-0.000193109930823887,4.41424353667119e-05,0.199963157344415,0.000216288637507122 -0.328,0.559778923886713,-0.292899179650609,0.0607363125869583,-9.86671017111362e-05,2.63572967895697e-05,0.199963219765432,8.66192256809838e-05,-0.000192751446748129,4.44948587488558e-05,0.199963245844987,0.000215953242476585 -0.33,0.559778726725747,-0.292899127321518,0.0611362391154791,-9.84940815573764e-05,2.59721525813772e-05,0.199963309107388,8.64010599177511e-05,-0.000192392563685972,4.48468822841774e-05,0.199963335051238,0.000215618377155526 -0.332,0.559778529910386,-0.292899075761998,0.0615361658233879,-9.83214974714652e-05,2.55877265348259e-05,0.199963399152961,8.61831161991233e-05,-0.000192033233065158,4.51984625976509e-05,0.199963424962305,0.00021528398254501 -0.334,0.559778333439758,-0.292899024970612,0.061936092712091,-9.81493490925799e-05,2.52040196491165e-05,0.199963489901238,8.59653806473125e-05,-0.000191673447946794,4.55496257031895e-05,0.199963515577267,0.000214950054011043 -0.336,0.55977813731299,-0.29289897494592,0.0623360197829929,-9.79776359488759e-05,2.48210327430387e-05,0.199963581351464,8.57478879567885e-05,-0.000191313225678113,4.59003733424556e-05,0.199963606895366,0.000214616623031375 -0.338,0.559777941529214,-0.292898925686481,0.0627359470374968,-9.78063575407528e-05,2.44387667464041e-05,0.199963673502732,8.55306173108693e-05,-0.000190952552381328,4.62506621543013e-05,0.199963698915688,0.000214283661318901 -0.34,0.55977774608756,-0.292898877190853,0.0631358744770038,-9.76355134796324e-05,2.40572225335134e-05,0.199963766354113,8.53135340150856e-05,-0.000190591424586994,4.66005155505544e-05,0.1999637916373,0.000213951158589573 -0.342,0.55977755098716,-0.292898829457591,0.0635358021029133,-9.74651034046924e-05,2.36764010480561e-05,0.199963859904794,8.5096658886119e-05,-0.00019022978331451,4.69499335242762e-05,0.199963885059384,0.000213619072349358 -0.344,0.559777356227146,-0.292898782485249,0.063935729916623,-9.72951268440879e-05,2.32963034002553e-05,0.199963954153847,8.4880026618439e-05,-0.000189867645911112,4.72988896313419e-05,0.199963979181005,0.000213287427765755 -0.346,0.559777161806652,-0.292898736272377,0.0643356579195287,-9.71255832982187e-05,2.29169304644117e-05,0.199964049100352,8.46636233342578e-05,-0.000189505026254588,4.76473816998779e-05,0.199964074001239,0.000212956232767888 -0.348,0.559776967724813,-0.292898690817527,0.0647355861130244,-9.69564723507509e-05,2.2538283295237e-05,0.199964144743374,8.44474351557877e-05,-0.000189141889650468,4.79954079951605e-05,0.199964169519143,0.000212625452177209 -0.35,0.559776773980763,-0.292898646119244,0.0651355144985022,-9.67877935575955e-05,2.21603629058098e-05,0.199964241081984,8.42314135107713e-05,-0.000188778232629305,4.83429737213603e-05,0.199964265733787,0.00021229506646385 -0.352,0.559776580573639,-0.292898602176075,0.0655354430773523,-9.66195466967078e-05,2.17831703647198e-05,0.199964338115269,8.40156000325719e-05,-0.000188414027435524,4.86900650006894e-05,0.199964362644249,0.000211965066020509 -0.354,0.559776387502576,-0.292898558986563,0.0659353718509632,-9.64517311574652e-05,2.14067067960677e-05,0.199964435842244,8.3800050232341e-05,-0.000188049274069124,4.90366670879982e-05,0.199964460249542,0.000211635471072234 -0.356,0.559776194766714,-0.292898516549248,0.0663353008207213,-9.62843464957785e-05,2.10309732684433e-05,0.199964534261937,8.35846530877759e-05,-0.000187683969060658,4.93827851874573e-05,0.199964558548685,0.000211306237359517 -0.358,0.55977600236519,-0.29289847486267,0.066735229988011,-9.61173925451141e-05,2.06559709198251e-05,0.199964633373385,8.33694710489218e-05,-0.000187318095062893,4.97284071560022e-05,0.199964657540713,0.000210977373002986 -0.36,0.559775810297144,-0.292898433925364,0.0671351593542148,-9.59508686115828e-05,2.02817008881917e-05,0.199964733175566,8.31545388102483e-05,-0.000186951659014722,5.00735321262713e-05,0.199964757224596,0.000210648899243986 -0.362,0.559775618561716,-0.292898393735866,0.0675350889207132,-9.57847743898731e-05,1.99081642837662e-05,0.199964833667514,8.29397384105589e-05,-0.000186584626221675,5.04181765781375e-05,0.199964857599365,0.000210320744291566 -0.364,0.559775427158046,-0.292898354292707,0.0679350186888849,-9.56191096579405e-05,1.9535362383305e-05,0.199964934848272,8.27251531165806e-05,-0.00018621697586707,5.07622980108757e-05,0.199964958664057,0.000209992914067556 -0.366,0.559775236085277,-0.292898315594417,0.0683349486601063,-9.54538737774068e-05,1.91632963802979e-05,0.199965036716706,8.25108106838889e-05,-0.000185848735706484,5.11058799446129e-05,0.199965060417533,0.000209665441616696 -0.368,0.559775045342551,-0.292898277639522,0.0687348788357517,-9.5289066415205e-05,1.8791967440479e-05,0.199965139271792,8.22966417235448e-05,-0.000185479871045446,5.14489596759037e-05,0.199965162858762,0.00020933827947545 -0.37,0.559774854929012,-0.292898240426547,0.0691348092171935,-9.51246872105126e-05,1.84213768961161e-05,0.199965242512544,8.20826878689118e-05,-0.000185110357597828,5.17915189901518e-05,0.199965265986756,0.000209011419728895 -0.372,0.559774664843802,-0.292898203954014,0.0695347398058019,-9.49607356637293e-05,1.80515260100877e-05,0.199965346437868,8.18689074866263e-05,-0.000184740216180312,5.21335448769311e-05,0.199965369800412,0.000208684862864649 -0.374,0.559774475086069,-0.292898168220443,0.069934670602945,-9.47972115805661e-05,1.76824160313948e-05,0.199965451046724,8.16553005766884e-05,-0.000184369425976216,5.24750338667945e-05,0.199965474298687,0.000208358591198899 -0.376,0.559774285654956,-0.29289813322435,0.0703346016099888,-9.46341144614226e-05,1.73140483061829e-05,0.199965556338003,8.1441950405825e-05,-0.00018399796269941,5.28159755514012e-05,0.199965579480467,0.00020803261483932 -0.378,0.559774096549611,-0.29289809896425,0.070734532828297,-9.44714437789428e-05,1.69464241805972e-05,0.199965662310626,8.12287598295213e-05,-0.000183625833288791,5.31563699307513e-05,0.199965685344667,0.000207706903444878 -0.38,0.559773907769181,-0.292898065438653,0.0711344642592313,-9.43091994221045e-05,1.65795449730277e-05,0.199965768963483,8.10157288477775e-05,-0.000183253003049888,5.34962048617804e-05,0.199965791890172,0.00020738142487251 -0.382,0.559773719312813,-0.29289803264607,0.0715343959041509,-9.41473808635517e-05,1.62134121683977e-05,0.199965876295446,8.08029615440019e-05,-0.000182879475452147,5.38354864160206e-05,0.199965899115848,0.00020705622601985 -0.384,0.559773531179658,-0.292898000585005,0.0719343277644131,-9.39859875759285e-05,1.58480270712191e-05,0.199965984305429,8.0590367712574e-05,-0.000182505281720591,5.41742137261103e-05,0.199966007020605,0.000206731300621851 -0.386,0.559773343368863,-0.292897969253961,0.0723342598413726,-9.38250193927014e-05,1.54833910415153e-05,0.199966092992301,8.03779057201303e-05,-0.000182130335119046,5.45123685774528e-05,0.199966115603307,0.000206406552770144 -0.388,0.55977315587958,-0.29289793865144,0.0727341921363823,-9.3664475953048e-05,1.51195057307429e-05,0.199966202354903,8.01656727111854e-05,-0.000181754642586407,5.48499457658779e-05,0.199966224862789,0.000206082026711378 -0.39,0.559772968710959,-0.292897908775939,0.0731341246507922,-9.35043567018567e-05,1.47563724711697e-05,0.199966312392084,7.99536617468454e-05,-0.000181378249225483,5.51869348830447e-05,0.199966334797894,0.000205757758299938 -0.392,0.559772781862153,-0.292897879625951,0.0735340573859506,-9.33446613060606e-05,1.4393992733841e-05,0.199966423102642,7.97418103770652e-05,-0.000181001089116783,5.55233333268678e-05,0.199966445407418,0.000205433666002921 -0.394,0.559772595332314,-0.292897851199968,0.0739339903432028,-9.31853894603484e-05,1.40323681147025e-05,0.199966534485417,7.95301255407387e-05,-0.000180623165729754,5.58591402299857e-05,0.199966556690192,0.000205109756993657 -0.396,0.559772409120595,-0.292897823496478,0.0743339235238923,-9.30265408038976e-05,1.3671500070922e-05,0.199966646539203,7.93186488712293e-05,-0.000180244475594948,5.61943477861426e-05,0.199966668645007,0.000204786043871073 -0.398,0.559772223226151,-0.292897796513967,0.0747338569293596,-9.28681148648635e-05,1.33113902123227e-05,0.199966759262808,7.91073734296432e-05,-0.000179864997895684,5.65289638015942e-05,0.199966781270664,0.000204462509393369 -0.4,0.559772037648136,-0.292897770250917,0.0751337905609435,-9.27101113101791e-05,1.29520400793392e-05,0.199966872655058,7.88962506437229e-05,-0.000179484749979197,5.68629770006379e-05,0.199966894565987,0.000204139148607285 -0.402,0.559771852385706,-0.292897744705807,0.0755337244199799,-9.25525298622886e-05,1.25934512124059e-05,0.199966986714716,7.86853429635136e-05,-0.000179103683273229,5.71963683013154e-05,0.19996700852973,0.00020381593928391 -0.404,0.559771667438016,-0.292897719877112,0.0759336585078024,-9.2395369938325e-05,1.22356253462463e-05,0.199967101440532,7.84746434501215e-05,-0.000178721780430546,5.7529127295286e-05,0.199967123160641,0.000203492862165348 -0.406,0.559771482804226,-0.292897695763306,0.076333592825742,-9.22386312884881e-05,1.18785640906838e-05,0.199967216831225,7.82640688368196e-05,-0.00017833907267617,5.78612583193582e-05,0.199967238457432,0.00020316991541119 -0.408,0.559771298483491,-0.292897672362856,0.0767335273751273,-9.20823136629777e-05,1.15222690555416e-05,0.199967332885565,7.80536885125471e-05,-0.000177955528785079,5.81927700471496e-05,0.199967354418869,0.000202847102521458 -0.41,0.559771114474971,-0.292897649674229,0.0771334621572843,-9.19264165344379e-05,1.11667419755434e-05,0.199967449602305,7.78435371717733e-05,-0.000177571152226719,5.8523659009213e-05,0.199967471043699,0.000202524440614866 -0.412,0.559770930777825,-0.292897627695888,0.0775333971735365,-9.17709395142906e-05,1.08119844466348e-05,0.199967566980201,7.76335384866655e-05,-0.000177185911776067,5.88538991846965e-05,0.199967588330673,0.000202201867150428 -0.414,0.559770747391213,-0.292897606426292,0.0779333324252051,-9.16158823804913e-05,1.04579983284392e-05,0.199967685017902,7.74237132739052e-05,-0.000176799783146996,5.91834740937269e-05,0.199967706278435,0.00020187936572059 -0.416,0.559770564314296,-0.292897585863895,0.0783332679136081,-9.1461244661195e-05,1.01047853140468e-05,0.199967803714097,7.72141239835377e-05,-0.000176412811442317,5.9512404552986e-05,0.199967824885669,0.000201557007516995 -0.418,0.559770381546234,-0.29289756600715,0.0787332036400614,-9.13070258845571e-05,9.75234708266991e-06,0.19996792306752,7.700469428773e-05,-0.000176024934211982,5.9840686225665e-05,0.199967944151104,0.000201234709070374 -0.42,0.559770199086192,-0.292897546854506,0.0791331396058782,-9.11532258840441e-05,9.40068557719886e-06,0.199968043076842,7.67954033698003e-05,-0.000175636141047652,6.0168306101338e-05,0.199968064073405,0.000200912451281511 -0.422,0.559770016933331,-0.292897528404408,0.0795330758123688,-9.09998442710779e-05,9.0498025184793e-06,0.199968163740745,7.65863206186878e-05,-0.000175246459704903,6.04952485674936e-05,0.199968184651249,0.000200590282028451 -0.424,0.559769835086815,-0.292897510655296,0.0799330122608412,-9.08468806015693e-05,8.69969973837924e-06,0.199968285057837,7.63774460343924e-05,-0.000174855841611476,6.08215049505145e-05,0.199968305883239,0.000200268157910436 -0.426,0.559769653545809,-0.292897493605609,0.0803329489526002,-9.06943344869403e-05,8.3503791520334e-06,0.199968407026765,7.61687588002324e-05,-0.00017446430064516,6.11471030059762e-05,0.199968427768018,0.000199946093292158 -0.428,0.559769472309477,-0.29289747725378,0.0807328858889482,-9.05422055663684e-05,8.0018425357986e-06,0.199968529646249,7.59602380995261e-05,-0.000174071840275403,6.1472012376218e-05,0.199968550304299,0.000199624075839023 -0.43,0.559769291376986,-0.292897461598239,0.0811328230711852,-9.03904935345422e-05,7.65409179093179e-06,0.199968652914814,7.57518908711673e-05,-0.000173678401521604,6.17962096424728e-05,0.199968673490604,0.000199302051298609 -0.432,0.559769110747503,-0.292897446637413,0.0815327605006075,-9.02391980028837e-05,7.30712892971219e-06,0.199968776831087,7.55437379318379e-05,-0.000173283994792106,6.21197390401895e-05,0.199968797325553,0.000198980052191786 -0.434,0.559768930420194,-0.292897432369723,0.0819326981785095,-9.00883185828149e-05,6.96095581176336e-06,0.19996890139377,7.53357515259622e-05,-0.000172888637434143,6.24425693443453e-05,0.199968921807844,0.000198658075290534 -0.436,0.559768750394229,-0.292897418793589,0.0823326361061826,-8.99378549967799e-05,6.61557437997561e-06,0.199969026601365,7.51279594091158e-05,-0.000172492284344905,6.27646719320029e-05,0.199969046935972,0.000198336084680104 -0.438,0.559768570668774,-0.292897405907426,0.082732574284915,-8.97878067451784e-05,6.27098667438374e-06,0.199969152452458,7.49203823979804e-05,-0.000172094935524391,6.30860728240145e-05,0.19996917270852,0.000198014098276261 -0.44,0.559768391243002,-0.292897393709643,0.0831325127159924,-8.9638173467188e-05,5.92719463787805e-06,0.199969278945656,7.47129580425109e-05,-0.000171696590972603,6.34067650814862e-05,0.199969299124089,0.000197692092176632 -0.442,0.55976821211608,-0.292897382198647,0.0835324514006976,-8.94889549130084e-05,5.58420031049333e-06,0.199969406079518,7.45057210371768e-05,-0.000171297212525623,6.37267330919066e-05,0.199969426181233,0.000197370043311757 -0.444,0.559768033287182,-0.292897371372841,0.0839323903403105,-8.93401505830393e-05,5.24200578777556e-06,0.199969533852588,7.42986921986599e-05,-0.000170896810591791,6.40459707837437e-05,0.199969553878491,0.000197047968541952 -0.446,0.559767854755478,-0.292897361230624,0.0843323295361079,-8.91917601442137e-05,4.90061306812617e-06,0.199969662263401,7.40918090769149e-05,-0.000170495395579451,6.436446948338e-05,0.199969682214392,0.000196725852439277 -0.448,0.559767676520141,-0.292897351770389,0.0847322689893641,-8.90437833467316e-05,4.56002420545775e-06,0.199969791310466,7.38851202441992e-05,-0.000170092929324683,6.46822300581772e-05,0.19996981118744,0.00019640368246306 -0.45,0.559767498580345,-0.292897342990527,0.0851322087013498,-8.88962196632369e-05,4.22024135082743e-06,0.199969920992322,7.36786395783006e-05,-0.000169689397949701,6.49992611817529e-05,0.19996994079617,0.000196081456698013 -0.452,0.559767320935263,-0.292897334889424,0.0855321486733334,-8.87490687884184e-05,3.88126661365895e-06,0.199970051307511,7.34723254458557e-05,-0.000169284794515611,6.53155472415956e-05,0.199970071039118,0.00019575915041725 -0.454,0.55976714358407,-0.292897327465461,0.0859320889065799,-8.86023303614535e-05,3.54310217276499e-06,0.199970182254511,7.32662264191219e-05,-0.000168879115552967,6.56310656863001e-05,0.199970201914757,0.00019543677337382 -0.456,0.559766966525941,-0.292897320717015,0.0863320294023514,-8.84560038827419e-05,3.20575015144708e-06,0.199970313831774,7.30602939258417e-05,-0.000168472361061766,6.59458234547605e-05,0.199970333421534,0.000195114311677496 -0.458,0.559766789760054,-0.29289731464246,0.086731970161907,-8.83100891857501e-05,2.86921272851792e-06,0.199970446037804,7.28545349049092e-05,-0.000168064492878095,6.62598370268496e-05,0.199970465557949,0.000194791742703346 -0.46,0.559766613285585,-0.292897309240164,0.0871319111865026,-8.81645857431223e-05,2.5334921799347e-06,0.199970578871122,7.26489979285815e-05,-0.000167655517940845,6.65730760449068e-05,0.199970598322514,0.000194469082504446 -0.462,0.559766437101711,-0.292897304508491,0.0875318524773914,-8.80194931940358e-05,2.19859065675454e-06,0.199970712330109,7.24436205468137e-05,-0.000167245432780571,6.68855240290588e-05,0.199970731713609,0.000194146301197147 -0.464,0.559766261207612,-0.292897300445802,0.0879317940358231,-8.7874811260935e-05,1.86451044881242e-06,0.199970846413218,7.22384305151812e-05,-0.000166834216580591,6.71972043980728e-05,0.19997086572968,0.000193823401436134 -0.466,0.559766085602466,-0.29289729705045,0.0883317358630443,-8.77305394719751e-05,1.53125379043217e-06,0.199970981118926,7.20334625281537e-05,-0.000166421858932564,6.75081136825017e-05,0.199971000369199,0.000193500388287297 -0.468,0.559765910285454,-0.292897294320786,0.0887316779602988,-8.75866774108224e-05,1.19882301308216e-06,0.199971116445673,7.18286749523677e-05,-0.000166008339019807,6.78182275962169e-05,0.1999711356306,0.000193177222166125 -0.47,0.559765735255756,-0.292897292255158,0.089131620328827,-8.74432247721656e-05,8.67220434352944e-07,0.199971252391837,7.16240539100354e-05,-0.000165593681128451,6.81275426697713e-05,0.199971271512258,0.000192853919841937 -0.472,0.559765560512555,-0.292897290851905,0.0895315629698661,-8.73001811951823e-05,5.36448288568358e-07,0.199971388955843,7.14196479734141e-05,-0.00016517784362513,6.84360623726121e-05,0.199971408012592,0.00019253046713277 -0.474,0.559765386055032,-0.292897290109364,0.0899315058846504,-8.7157546180272e-05,2.06509059852422e-07,0.199971526136086,7.12154363258221e-05,-0.000164760819570952,6.8743773694313e-05,0.199971545129991,0.000192206848105118 -0.476,0.55976521188237,-0.292897290025868,0.0903314490744105,-8.7015319449879e-05,-1.22594989715452e-07,0.199971663930938,7.10113842727899e-05,-0.000164342626313152,6.9050683573768e-05,0.199971682862822,0.000191883069659224 -0.478,0.559765037993754,-0.292897290599744,0.0907313925403741,-8.68735006431808e-05,-4.50861445400185e-07,0.19997180233882,7.08075612032565e-05,-0.000163923215279471,6.93567668574868e-05,0.199971821209501,0.00019155910925883 -0.48,0.559764864388367,-0.292897291829314,0.0911313362837657,-8.6732089205066e-05,-7.78287850833336e-07,0.199971941358005,7.06039463005403e-05,-0.000163502603817145,6.966203395381e-05,0.199971960168296,0.000191234980227932 -0.482,0.559764691065397,-0.292897293712896,0.0915312803058062,-8.65910848579786e-05,-1.10487186066876e-06,0.199972080986956,7.04005048701716e-05,-0.000163080767640044,6.996651088359e-05,0.199972099737665,0.000190910661057115 -0.484,0.559764518024028,-0.292897296248802,0.0919312246077136,-8.64504871855853e-05,-1.43061092139351e-06,0.199972221224049,7.01972646677262e-05,-0.000162657692870382,7.02701377988667e-05,0.199972239915978,0.000190586130877927 -0.486,0.559764345263448,-0.292897299435339,0.0923311691907023,-8.63102957993077e-05,-1.75550263215029e-06,0.199972362067507,6.99942118154161e-05,-0.000162233382977605,7.0572919036449e-05,0.199972380701454,0.000190261391620436 -0.488,0.559764172782845,-0.292897303270812,0.0927311140559836,-8.61705103383236e-05,-2.07954445330394e-06,0.199972503515725,6.97913671299232e-05,-0.000161807813675585,7.08748745456566e-05,0.199972522092482,0.000189936440248308 -0.49,0.559764000581407,-0.292897307753517,0.0931310592047652,-8.6031130330788e-05,-2.40273388685263e-06,0.199972645567006,6.95887167334596e-05,-0.000161381016189343,7.11759783056375e-05,0.199972664087359,0.00018961129125313 -0.492,0.559763828658324,-0.292897312881748,0.0935310046382516,-8.58921554713898e-05,-2.72506851806131e-06,0.199972788219638,6.93862675649192e-05,-0.000160952952354964,7.14762433268178e-05,0.199972806684371,0.000189285922240626 -0.494,0.559763657012785,-0.292897318653791,0.0939309503576438,-8.57535852605284e-05,-3.04654569627249e-06,0.199972931471979,6.91840404409837e-05,-0.000160523559722403,7.17756574661332e-05,0.199972949881867,0.000188960285992552 -0.496,0.559763485643982,-0.29289732506793,0.0943308963641395,-8.56154193096258e-05,-3.36716275695092e-06,0.199973075322268,6.89819937279916e-05,-0.000160092890333376,7.20742162133025e-05,0.199973093678083,0.000188634412652477 -0.498,0.559763314551108,-0.292897332122442,0.0947308426589329,-8.54776572856164e-05,-3.686917257606e-06,0.199973219768844,6.87801551821157e-05,-0.000159660954596202,7.23719147111011e-05,0.199973238071352,0.000188308322196331 -0.5,0.559763143733353,-0.2928973398156,0.0951307892432149,-8.53402986888974e-05,-4.00580657533572e-06,0.199973364809927,6.85785386814401e-05,-0.000159227690060815,7.26687323163176e-05,0.199973383059889,0.000187981961651232 -0.502,0.559762973189913,-0.292897348145669,0.0955307361181726,-8.52033431308906e-05,-4.32382801784925e-06,0.199973510443773,6.83771164691965e-05,-0.00015879310713561,7.29646865496591e-05,0.199973528641945,0.000187655339422339 -0.504,0.559762802919981,-0.292897357110912,0.09593068328499,-8.50667902230206e-05,-4.64097900387816e-06,0.199973656668673,6.81758885456851e-05,-0.000158357205820581,7.32597697783443e-05,0.199973674815806,0.000187328455612786 -0.506,0.559762632922752,-0.292897366709585,0.0963306307448473,-8.49306395767079e-05,-4.95725684113157e-06,0.199973803482852,6.79748757281828e-05,-0.000157919951421226,7.35539745430613e-05,0.199973821579692,0.000187001288653917 -0.508,0.559762463197423,-0.292897376939939,0.0967305784989214,-8.47948907201078e-05,-5.27265880956306e-06,0.199973950884571,6.77740918941793e-05,-0.000157481350876454,7.38472937314438e-05,0.199973968931857,0.000186673849795335 -0.51,0.559762293743189,-0.29289738780022,0.0971305265483856,-8.46595432091312e-05,-5.58718224463738e-06,0.199974098872027,6.75735023492052e-05,-0.000157041411125158,7.41397169351509e-05,0.199974116870494,0.000186346131312521 -0.512,0.55976212455925,-0.292897399288668,0.0975304748944095,-8.4524596710711e-05,-5.9008244540637e-06,0.199974247443439,6.73731070932603e-05,-0.000156600100942316,7.44312346132036e-05,0.199974265393818,0.00018601810627084 -0.514,0.559761955644802,-0.292897411403518,0.0979304235381593,-8.43900507807581e-05,-6.21358264840665e-06,0.199974396596966,6.7172961637496e-05,-0.000156157406450141,7.47218606433896e-05,0.19997441449998,0.000185689791917588 -0.516,0.559761786999047,-0.292897424142999,0.0983303724807974,-8.4255904864161e-05,-6.52545407986426e-06,0.199974546330882,6.69730382263367e-05,-0.000155713331118079,7.50116054340499e-05,0.199974564187251,0.000185361188728786 -0.518,0.559761618621183,-0.292897437505334,0.0987303217234828,-8.41221586278528e-05,-6.83643597287897e-06,0.199974696643387,6.67732952264188e-05,-0.000155267868007236,7.53004290865444e-05,0.199974714453826,0.000185032263163819 -0.52,0.559761450510412,-0.292897451488742,0.0991302712673709,-8.39888116832554e-05,-7.14652555189321e-06,0.199974847532598,6.65737742711024e-05,-0.00015482099630093,7.55883359376818e-05,0.199974865297815,0.00018470301796706 -0.522,0.559761282665936,-0.292897466091436,0.0995302211136132,-8.38558635307684e-05,-7.45571995808269e-06,0.199974998996731,6.63744822992883e-05,-0.000154372712529715,7.58753355284413e-05,0.199975016717429,0.000184373460172103 -0.524,0.559761115086958,-0.292897481311622,0.0999301712633578,-8.37233137540582e-05,-7.76401640201207e-06,0.19997515103394,6.61754123720825e-05,-0.00015392300628525,7.61613931643534e-05,0.199975168710819,0.0001840435678725 -0.526,0.559760947772681,-0.292897497147502,0.100330121717749,-8.35911618812801e-05,-8.07141198322369e-06,0.199975303642304,6.59765506116904e-05,-0.000153471853281406,7.64465279273762e-05,0.199975321276055,0.000183713327258524 -0.528,0.55976078072231,-0.29289751359727,0.100730072477927,-8.34594075516115e-05,-8.37790381513769e-06,0.199975456820052,6.5777931712585e-05,-0.000153019288212652,7.67307545626594e-05,0.199975474411365,0.000183382789708433 -0.53,0.559760613935051,-0.292897530659117,0.101130023545029,-8.33280501544297e-05,-8.6834891360743e-06,0.199975610565322,6.55795487358723e-05,-0.000152565279853966,7.70140323042012e-05,0.199975628114879,0.000183051913404565 -0.532,0.55976044741011,-0.292897548331227,0.101529974920188,-8.3197089356668e-05,-8.98816493455356e-06,0.199975764876181,6.53813392315072e-05,-0.000152109786571985,7.72963741624277e-05,0.199975782384659,0.000182720650582627 -0.534,0.559760281146694,-0.292897566611777,0.101929926604534,-8.30665247975037e-05,-9.29192828236224e-06,0.199975919750819,6.51833795273226e-05,-0.000151652836122285,7.75777792699772e-05,0.19997593721889,0.00018238905533775 -0.536,0.559760115144011,-0.29289758549894,0.102329878599192,-8.29363558385587e-05,-9.5947762790427e-06,0.199976075187298,6.49856904400003e-05,-0.000151194414627076,7.78582207386357e-05,0.199976092615629,0.000182057116073429 -0.538,0.55975994940127,-0.292897604990882,0.102729830905283,-8.28065820357437e-05,-9.89670594087055e-06,0.199976231183702,6.47881956417073e-05,-0.000150734515147466,7.81377081093825e-05,0.199976248572954,0.000181724807827211 -0.54,0.559759783917682,-0.292897625085764,0.103129783523926,-8.26772030559918e-05,-1.01977143396326e-05,0.199976387738131,6.45909228880192e-05,-0.000150273113397325,7.84162561273671e-05,0.199976405088962,0.000181392130906179 -0.542,0.559759618692458,-0.292897645781739,0.103529736456236,-8.25482183441916e-05,-1.04977983944598e-05,0.199976544848726,6.43939207511934e-05,-0.000149810195498867,7.86938431085459e-05,0.199976562161786,0.000181059086069471 -0.544,0.559759453724809,-0.292897667076957,0.103929689703321,-8.24196273729871e-05,-1.0796955121628e-05,0.199976702513503,6.41971198422908e-05,-0.00014934578573822,7.89704465015139e-05,0.199976719789438,0.000180725663187219 -0.546,0.559759289013948,-0.29289768896956,0.10432964326629,-8.22914298648225e-05,-1.10951815374127e-05,0.199976860730512,6.40005340390992e-05,-0.000148879870237595,7.92460966639318e-05,0.199976877969962,0.000180391873308423 -0.548,0.55975912455909,-0.292897711457683,0.104729597146243,-8.21636252368307e-05,-1.13924746025784e-05,0.19997701949789,6.38042396694515e-05,-0.000148412414302523,7.95207823200971e-05,0.19997703670149,0.000180057714468725 -0.55,0.559758960359447,-0.292897734539458,0.105129551344281,-8.20362129061446e-05,-1.16888311946228e-05,0.199977178813641,6.36081742833027e-05,-0.000147943417933005,7.9794469642902e-05,0.199977195982022,0.000179723154131422 -0.552,0.559758796414238,-0.292897758213008,0.105529505861498,-8.19091925396975e-05,-1.19842482743104e-05,0.199977338675768,6.3412324002865e-05,-0.000147472891537381,8.00671820511133e-05,0.199977355809554,0.000179388210915452 -0.554,0.559758632722677,-0.292897782476451,0.105929460698984,-8.17825636101332e-05,-1.22787227607723e-05,0.19997749908237,6.32167374003955e-05,-0.000147000786543394,8.03389160752842e-05,0.199977516182179,0.000179052865288659 -0.556,0.559758469283984,-0.292897807327899,0.106329415857827,-8.16563255900959e-05,-1.2572251420484e-05,0.199977660031433,6.30214075370005e-05,-0.000146527123767725,8.06096422251156e-05,0.199977677097881,0.000178717123486386 -0.558,0.559758306097375,-0.292897832765457,0.10672937133911,-8.15304779799852e-05,-1.28648312558432e-05,0.199977821520938,6.28263066571044e-05,-0.000146051930965951,8.08793865214596e-05,0.199977838554633,0.000178381014996392 -0.56,0.559758143162072,-0.292897858787224,0.107129327143911,-8.14050203634675e-05,-1.31564591443478e-05,0.199977983548979,6.2631434760707e-05,-0.00014557515262692,8.11481359538901e-05,0.199978000550524,0.000178044493459716 -0.562,0.559757980477294,-0.292897885391294,0.107529283273306,-8.12799522409424e-05,-1.34471318663509e-05,0.199978146113482,6.24368404200659e-05,-0.000145096771403397,8.1415880981428e-05,0.199978163083476,0.000177707562555206 -0.564,0.559757818042263,-0.292897912575752,0.107929239728365,-8.11552730017872e-05,-1.37368462299614e-05,0.199978309212503,6.22425028184991e-05,-0.000144616794234276,8.16826294103289e-05,0.199978326151537,0.000177370229404753 -0.566,0.559757655856202,-0.292897940338678,0.108329196510156,-8.10309822296684e-05,-1.4025599043288e-05,0.199978472844,6.20483942004312e-05,-0.000144135224589003,8.19483474134852e-05,0.199978489752663,0.000177032476777499 -0.568,0.559757493918334,-0.292897968678148,0.108729153619741,-8.09070794249855e-05,-1.43133871283174e-05,0.199978637005892,6.18545492603317e-05,-0.000143652048589792,8.22130427971523e-05,0.199978653884766,0.000176694314379975 -0.57,0.559757332227884,-0.292897997592227,0.10912911105818,-8.0783564032627e-05,-1.46002072376472e-05,0.199978801696171,6.16609679982005e-05,-0.000143167245419962,8.2476723367586e-05,0.199978818545832,0.000176355734335654 -0.572,0.559757170784078,-0.292898027078977,0.109529068826526,-8.06604355529927e-05,-1.48860561099973e-05,0.199978966912786,6.14676573529315e-05,-0.000142680846304532,8.27394038699358e-05,0.199978983733805,0.000176016776914553 -0.574,0.559757009586142,-0.292898057136451,0.109929026925831,-8.05376934032153e-05,-1.51709306228653e-05,0.199979132653787,6.12745965078431e-05,-0.000142192813079589,8.30010738958608e-05,0.199979149446731,0.000175677404741054 -0.576,0.559756848633304,-0.292898087762699,0.110328985357141,-8.04153371669613e-05,-1.54548273623156e-05,0.199979298917081,6.10817646462535e-05,-0.000141703111050662,8.3261684005742e-05,0.199979315682511,0.000175337564934638 -0.578,0.559756687924793,-0.292898118955761,0.110728944121499,-8.02933663446303e-05,-1.5737743067068e-05,0.199979465700523,6.08892242182079e-05,-0.000141211788790008,8.35212489447289e-05,0.199979482438994,0.000174997331249526 -0.58,0.559756527459839,-0.292898150713672,0.111128903219943,-8.01717802700885e-05,-1.60196745174756e-05,0.199979633002077,6.06969752237063e-05,-0.000140718801194817,8.37797869274181e-05,0.199979649714139,0.000174656681832151 -0.582,0.559756367237672,-0.292898183034459,0.111528862653507,-8.00505784437355e-05,-1.63006182718472e-05,0.19997980081967,6.05049690904912e-05,-0.000140224144795642,8.40372849433834e-05,0.199979817505868,0.00017431559691825 -0.584,0.559756207257525,-0.292898215916145,0.111928822423222,-7.99297603937265e-05,-1.65805710966582e-05,0.199979969151217,6.03132266352446e-05,-0.000139727847348059,8.42937403905396e-05,0.19997998581209,0.000173974111007082 -0.586,0.55975604751863,-0.292898249356743,0.112328782530112,-7.98093255371945e-05,-1.68595296612395e-05,0.199980137994632,6.01217756135419e-05,-0.000139229874157598,8.45491489320781e-05,0.199980154630714,0.00017363220998998 -0.588,0.559755888020223,-0.292898283354263,0.1127287429752,-7.96892732912723e-05,-1.71374905932886e-05,0.199980307347813,5.99305743920197e-05,-0.000138730211346471,8.48034906186789e-05,0.199980323959632,0.000173289864990155 -0.59,0.559755728761537,-0.292898317906706,0.113128703759503,-7.95696032396264e-05,-1.74144505066254e-05,0.199980477208594,5.97396368484659e-05,-0.000138228858914679,8.50567758586828e-05,0.199980493796675,0.000172947092392775 -0.592,0.55975556974181,-0.292898353012065,0.113528664884035,-7.94503147438785e-05,-1.76904060289473e-05,0.199980647574916,5.95490115551378e-05,-0.000137725806453881,8.53090176625159e-05,0.199980664139776,0.000172603913730568 -0.594,0.559755410960278,-0.29289838866833,0.113928626349803,-7.93314071934059e-05,-1.79653537324409e-05,0.199980818444665,5.93586707564597e-05,-0.000137221043555735,8.55601856725174e-05,0.199980834986816,0.000172260302916297 -0.596,0.559755252416181,-0.29289842487348,0.114328588157813,-7.92128800608526e-05,-1.82392902031703e-05,0.199980989815659,5.9168586696856e-05,-0.000136714591036924,8.58102781539637e-05,0.199981006335608,0.000171916272909103 -0.598,0.559755094108758,-0.292898461625491,0.114728550309066,-7.90947328466184e-05,-1.85122120965886e-05,0.199981161685778,5.89787871319025e-05,-0.000136206428080765,8.60593037804724e-05,0.199981178184028,0.000171571827959168 -0.6,0.55975493603725,-0.292898498922329,0.115128512804556,-7.8976964912325e-05,-1.87841159154933e-05,0.199981334052874,5.8789272061599e-05,-0.000135696537340024,8.63072400006381e-05,0.199981350529921,0.000171226950075428 -0.602,0.559754778200898,-0.292898536761954,0.115528475645277,-7.8859575758372e-05,-1.90549982459487e-05,0.199981506914738,5.86000414859455e-05,-0.000135184932692489,8.65541032943339e-05,0.199981523371073,0.000170881665740692 -0.604,0.559754620598947,-0.292898575142322,0.115928438832215,-7.87425647463812e-05,-1.93248556462633e-05,0.199981680269287,5.84111231605177e-05,-0.00013467157944369,8.67999066719859e-05,0.199981696705396,0.000170535971035429 -0.606,0.559754463230639,-0.292898614061377,0.116328402366354,-7.862593126573e-05,-1.95936845637234e-05,0.199981854114364,5.82224754519522e-05,-0.00013415646371584,8.70445980918898e-05,0.199981870530727,0.000170189821850401 -0.608,0.559754306095221,-0.29289865351706,0.116728366248673,-7.85096748445734e-05,-1.98614815011266e-05,0.199982028447679,5.80341261158245e-05,-0.00013363961673396,8.72881905644717e-05,0.199982044844771,0.00016984326647485 -0.61,0.55975414919194,-0.292898693507303,0.117128330480145,-7.83937947612667e-05,-2.01282430306593e-05,0.199982203267127,5.78460612743469e-05,-0.000133121045436946,8.75306979675194e-05,0.199982219645416,0.000169496320402131 -0.612,0.559753992520042,-0.292898734030032,0.117528295061741,-7.8278290599476e-05,-2.03939656828744e-05,0.199982378570471,5.76582809275194e-05,-0.00013260071166088,8.77721046885215e-05,0.199982394930424,0.000169148953452102 -0.614,0.559753836078777,-0.292898775083166,0.117928259994427,-7.81631616375566e-05,-2.06586458773028e-05,0.199982554355545,5.74708405864932e-05,-0.00013207859458908,8.80124315441599e-05,0.199982570697619,0.000168801187118634 -0.616,0.559753679867396,-0.292898816664615,0.118328225279163,-7.80484072371301e-05,-2.09222800612307e-05,0.199982730620197,5.72836708623292e-05,-0.00013155470116044,8.82516386357945e-05,0.199982746944846,0.000168452990673301 -0.618,0.559753523885149,-0.292898858772286,0.118728190916908,-7.79340269541073e-05,-2.11848646819446e-05,0.1999829073621,5.7096799510603e-05,-0.000131029041783303,8.84897120856375e-05,0.19998292366977,0.000168104382602012 -0.62,0.559753368131288,-0.292898901404074,0.119128156908612,-7.78200200390877e-05,-2.1446396228364e-05,0.199983084579046,5.69102542868904e-05,-0.000130501630335455,8.87266995985846e-05,0.199983100870182,0.000167755416558953 -0.622,0.559753212605068,-0.292898944557871,0.119528123255224,-7.77063859369597e-05,-2.17068712032864e-05,0.199983262268898,5.67239866189339e-05,-0.000129972425183533,8.89625725516984e-05,0.199983278543936,0.00016740603717026 -0.624,0.559753057305744,-0.292898988231559,0.119928089957687,-7.75931240926119e-05,-2.19662859290981e-05,0.199983440429336,5.6538038140097e-05,-0.000129441402041408,8.91973057914885e-05,0.199983456688708,0.000167056235081594 -0.626,0.559752902232572,-0.292899032423015,0.120328057016941,-7.74802337843993e-05,-2.2224636811452e-05,0.199983619058121,5.63524227281675e-05,-0.000128908599072996,8.94309331450626e-05,0.199983635302254,0.000166706071471597 -0.628,0.559752747384809,-0.292899077130106,0.12072802443392,-7.73677144016992e-05,-2.24819203253901e-05,0.199983798153069,5.61671056886758e-05,-0.000128374005869958,8.96634329283773e-05,0.199983814382383,0.000166355523951098 -0.63,0.559752592761714,-0.292899122350696,0.121127992209554,-7.72555653616446e-05,-2.27381328349318e-05,0.199983977711853,5.5982093960516e-05,-0.000127837591207269,8.98947886615597e-05,0.199983993926764,0.000166004571016546 -0.632,0.559752438362548,-0.292899168082638,0.121527960344767,-7.71437860258572e-05,-2.29932706902191e-05,0.199984157732223,5.57973944825818e-05,-0.000127299365493272,9.01250133550357e-05,0.199984173933141,0.000165653239383599 -0.634,0.55975228418657,-0.292899214323779,0.121927928840482,-7.70323757837143e-05,-2.32473302969049e-05,0.199984338211907,5.56130141937671e-05,-0.000126759339136306,9.03540966004645e-05,0.199984354399237,0.000165301543182377 -0.636,0.559752130233045,-0.292899261071959,0.122327897697615,-7.69213339690821e-05,-2.35003080467644e-05,0.19998451914861,5.54289669718599e-05,-0.000126217498258585,9.05820479388252e-05,0.199984535322751,0.000164949491223875 -0.638,0.559751976501234,-0.292899308325011,0.122727866917077,-7.68106599158268e-05,-2.37522002899393e-05,0.199984700540098,5.52452389390722e-05,-0.000125673835921214,9.0808860431224e-05,0.199984716701445,0.000164597079540935 -0.64,0.559751822990405,-0.29289935608076,0.123127836499775,-7.67003530133258e-05,-2.40030033904492e-05,0.199984882384051,5.50618300954042e-05,-0.000125128331307511,9.10345149957025e-05,0.199984898532992,0.000164244291680899 -0.642,0.559751669699822,-0.292899404337024,0.123527806446613,-7.65904125954452e-05,-2.42527136151693e-05,0.199985064678158,5.48787543186435e-05,-0.000124580994825818,9.12590272447721e-05,0.199985080815076,0.000163891159008101 -0.644,0.559751516628755,-0.292899453091614,0.123927776758488,-7.64808379960513e-05,-2.45013273697525e-05,0.19998524742016,5.46959769143207e-05,-0.000124031833415028,9.14823789638363e-05,0.199985263545433,0.000163537675328311 -0.646,0.55975136377647,-0.292899502342334,0.124327747436294,-7.6371628687788e-05,-2.47488409488294e-05,0.199985430607674,5.4513546454693e-05,-0.000123480826258459,9.17045640813629e-05,0.199985446721674,0.000163183848079068 -0.648,0.55975121114224,-0.292899552086978,0.124727718480919,-7.62627838102325e-05,-2.49952506747863e-05,0.199985614238417,5.43314837564423e-05,-0.000122927990703347,9.19255956077779e-05,0.199985630341511,0.000162829715203901 -0.65,0.559751058725335,-0.292899602323337,0.125127689893247,-7.61543027527622e-05,-2.52405529116428e-05,0.199985798310057,5.41497124917356e-05,-0.000122373292055222,9.21454527263998e-05,0.199985814402607,0.000162475224048233 -0.652,0.559750906525029,-0.29289965304919,0.125527661674159,-7.60461849602655e-05,-2.54847438430072e-05,0.199985982820228,5.39682742939362e-05,-0.000121816726844637,9.23641493150162e-05,0.19998599890259,0.000162120404786359 -0.654,0.559750754540595,-0.292899704262312,0.125927633824528,-7.59384296555864e-05,-2.57278198190214e-05,0.199986167766654,5.3787183040832e-05,-0.000121258340174402,9.25816680263924e-05,0.199986183839179,0.000161765297192779 -0.656,0.55975060277131,-0.292899755960469,0.126327606345226,-7.58310362281022e-05,-2.59697772037048e-05,0.1999863531469,5.36064317935291e-05,-0.000120698107758388,9.27980071258049e-05,0.199986369209934,0.000161409890876026 -0.658,0.55975045121645,-0.292899808141421,0.126727579237116,-7.57240039284123e-05,-2.62106122500549e-05,0.199986538958682,5.34260413687093e-05,-0.000120135991432679,9.3013189164659e-05,0.199986555012565,0.000161054188683264 -0.66,0.559750299875294,-0.292899860802918,0.12712755250106,-7.56173320626274e-05,-2.64503211694356e-05,0.199986725199656,5.32459840107968e-05,-0.00011957199813617,9.3227179448485e-05,0.199986741244722,0.000160698178395009 -0.662,0.559750148747122,-0.292899913942705,0.127527526137915,-7.55110199923691e-05,-2.66889002425996e-05,0.1999869118674,5.30662527808978e-05,-0.000119006145216094,9.34399797120066e-05,0.199986927903978,0.000160341883461497 -0.664,0.559749997831214,-0.292899967558519,0.12792750014853,-7.54050670515038e-05,-2.69263457502999e-05,0.199987098959575,5.2886868495694e-05,-0.000118438453489134,9.36515916899471e-05,0.199987114987989,0.00015998533917281 -0.666,0.559749847126854,-0.292900021648088,0.128327474533753,-7.52994725183864e-05,-2.71626540565561e-05,0.199987286473767,5.27078519718671e-05,-0.00011786889866916,9.38620032392422e-05,0.199987302494335,0.000159628539381442 -0.668,0.559749696633324,-0.292900076209135,0.128727449294425,-7.51942356436163e-05,-2.73978213449766e-05,0.199987474407588,5.25291962705232e-05,-0.000117297435653363,9.40712230335094e-05,0.199987490420625,0.000159271465977752 -0.67,0.559749546349911,-0.292900131239374,0.129127424431383,-7.50893557333043e-05,-2.76318437991696e-05,0.199987662758659,5.23508666971928e-05,-0.000116724088727871,9.42792510727485e-05,0.199987678764471,0.000158914138188323 -0.672,0.559749396275901,-0.292900186736511,0.12952739994546,-7.49848321768275e-05,-2.78647176998881e-05,0.199987851524592,5.21728840685575e-05,-0.000116148878709366,9.44860821527892e-05,0.199987867523482,0.000158556587936477 -0.674,0.559749246410583,-0.292900242698245,0.129927375837482,-7.488066419703e-05,-2.8096439314007e-05,0.199988040702988,5.19952969568748e-05,-0.0001155718021284,9.46916937222263e-05,0.199988056695253,0.000158198828101929 -0.676,0.559749096753245,-0.292900299122268,0.130327352108272,-7.4776850989e-05,-2.83270049084017e-05,0.199988230291367,5.18180498509934e-05,-0.000114992838168293,9.48960892505068e-05,0.199988246277299,0.000157840840327842 -0.678,0.559748947303179,-0.292900356006264,0.130727328758647,-7.46733919976261e-05,-2.85564106666802e-05,0.199988420287345,5.16411358120194e-05,-0.000114411986829043,9.50992791459714e-05,0.199988436267229,0.000157482642002882 -0.68,0.559748798059677,-0.29290041334791,0.131127305789421,-7.4570286445752e-05,-2.87846528557178e-05,0.199988610688484,5.14646172899979e-05,-0.000113829255049546,9.53012599391733e-05,0.199988626662603,0.0001571242701938 -0.682,0.559748649022033,-0.292900471144876,0.131527283201401,-7.44675335284661e-05,-2.90117276868784e-05,0.199988801492384,5.12884734682472e-05,-0.000113244628952014,9.55020246912185e-05,0.199988817461013,0.000156765717608489 -0.684,0.559748500189543,-0.292900529394821,0.13192726099539,-7.4365132551879e-05,-2.92376313715259e-05,0.199988992696583,5.11126765911917e-05,-0.000112658122414233,9.57015577895958e-05,0.199989008659991,0.000156406989669808 -0.686,0.559748351561503,-0.292900588095401,0.132327239172187,-7.42630828221013e-05,-2.94623601765353e-05,0.199989184298616,5.09372335977253e-05,-0.000112069738905651,9.58998609690286e-05,0.199989200257068,0.000156048106296902 -0.688000000000001,0.559748203137211,-0.292900647244262,0.132727217732585,-7.41613836174881e-05,-2.96859103271485e-05,0.199989376296027,5.07621653045298e-05,-0.000111479447201246,9.60969324947935e-05,0.199989392249784,0.000155689065157476 -0.690000000000001,0.559748054915968,-0.292900706839042,0.133127196677371,-7.40600341608832e-05,-2.99082779653403e-05,0.199989568686345,5.05874855893928e-05,-0.000110887264648252,9.62927914488486e-05,0.199989584635661,0.000155329909690439 -0.692000000000001,0.559747906897075,-0.292900766877374,0.13352717600733,-7.39590336751305e-05,-3.01294593857415e-05,0.199989761467192,5.04131805745267e-05,-0.000110293208593903,9.6487411810342e-05,0.199989777412315,0.000154970646496156 -0.694000000000001,0.559747759079834,-0.29290082735688,0.13392715572324,-7.38583814385851e-05,-3.03494507997159e-05,0.199989954635993,5.02392225043557e-05,-0.000109697261690966,9.66807710278682e-05,0.199989970577167,0.000154611255045943 -0.696000000000001,0.559747611463549,-0.292900888275177,0.134327135825874,-7.37580767851131e-05,-3.05682484325054e-05,0.199990148190276,5.00656530122434e-05,-0.0001090994135311,9.68728985917266e-05,0.19999016412774,0.000154251775114373 -0.698000000000001,0.559747464047526,-0.292900949629874,0.134727116316001,-7.36581188265361e-05,-3.07858484538403e-05,0.199990342127587,4.98924790370836e-05,-0.00010849966758375,9.70637823588526e-05,0.199990358061573,0.000153892219348662 -0.700000000000001,0.559747316831074,-0.292901011418571,0.135127097194385,-7.35585068689647e-05,-3.10022471028404e-05,0.199990536445406,4.97196658844067e-05,-0.000107898041196153,9.72534101861821e-05,0.199990552376142,0.000153532596742403 -0.702000000000001,0.559747169813499,-0.292901073638862,0.135527078461783,-7.34592401629985e-05,-3.12174406186249e-05,0.199990731141228,4.95472551875764e-05,-0.000107294517021073,9.74417994209497e-05,0.199990747068937,0.000153172935448142 -0.704000000000001,0.559747022994113,-0.292901136288333,0.13592706011895,-7.33603178482144e-05,-3.14314251709246e-05,0.199990926212604,4.93752261299107e-05,-0.000106689115875191,9.76289361853676e-05,0.199990942137502,0.000152813250676427 -0.706000000000001,0.559746876372228,-0.292901199364563,0.136327042166633,-7.32617392584788e-05,-3.16441970821257e-05,0.199991121656973,4.92035509558341e-05,-0.000106081813472381,9.78147909891368e-05,0.199991137579273,0.000152453513939498 -0.708000000000001,0.559746729947156,-0.292901262865122,0.136727024605577,-7.31635036443911e-05,-3.18557524248142e-05,0.199991317471768,4.90322712987101e-05,-0.000105472602873746,9.79993759753216e-05,0.199991333391676,0.000152093758179702 -0.710000000000001,0.559746583718213,-0.292901326787572,0.13712700743652,-7.3065610173284e-05,-3.20660874936207e-05,0.199991513654476,4.88614079752203e-05,-0.000104861539590439,9.81827171647741e-05,0.199991529572195,0.000151734062176086 -0.712000000000001,0.559746437684715,-0.292901391129472,0.137526990660195,-7.29680580124902e-05,-3.22751985831759e-05,0.199991710202636,4.8690940168683e-05,-0.000104248561172415,9.8364805883877e-05,0.19999172611836,0.000151374387521806 -0.714000000000001,0.559746291845981,-0.292901455888367,0.137926974277331,-7.28708464126093e-05,-3.24830817383104e-05,0.1999919071137,4.85208331846287e-05,-0.000103633688436355,9.85455952950964e-05,0.19999192302762,0.000151014724394408 -0.716000000000001,0.55974614620133,-0.292901521061799,0.13832695828865,-7.27739746797517e-05,-3.26897333369214e-05,0.199992104385017,4.8351114778633e-05,-0.000103016969954517,9.87251010109435e-05,0.19999212029732,0.000150655142522807 -0.718000000000001,0.559746000750082,-0.2929015866473,0.138726942694871,-7.26774419534947e-05,-3.28951496181284e-05,0.199992302014104,4.81818127062716e-05,-0.000102398350215749,9.89033421133766e-05,0.199992317924969,0.000150295642930076 -0.720000000000001,0.559745855491562,-0.292901652642397,0.139126927496707,-7.25812474289266e-05,-3.30993267377843e-05,0.199992499998386,4.80128992119688e-05,-0.000101777836158945,9.90802977857141e-05,0.199992515907989,0.000149936225887013 -0.722000000000001,0.559745710425092,-0.292901719044607,0.139526912694865,-7.24853903566468e-05,-3.33022609627642e-05,0.199992698335295,4.78443881735124e-05,-0.00010115544513134,9.92559767015732e-05,0.199992714243806,0.000149576931611798 -0.724000000000001,0.55974556555,-0.292901785851441,0.139926898290048,-7.23898698762326e-05,-3.35039485183097e-05,0.199992897022293,4.76762865297963e-05,-0.000100531184071828,9.94303563095488e-05,0.199992912929877,0.00014921777046987 -0.726000000000001,0.559745420865613,-0.292901853060401,0.140326884282954,-7.22946852105277e-05,-3.37043856990515e-05,0.19999309605672,4.75085942808206e-05,-9.99050356331742e-05,9.96034262013e-05,0.199993111963537,0.000148858742391716 -0.728000000000001,0.559745276371259,-0.292901920668984,0.140726870674275,-7.21998354991093e-05,-3.39035686608424e-05,0.199993295435997,4.73412836710096e-05,-9.92770067542724e-05,9.97752123976791e-05,0.199993311342201,0.000148499879524223 -0.730000000000001,0.559745132066271,-0.292901988674676,0.141126857464698,-7.21053200758436e-05,-3.41014937260686e-05,0.19999349515757,4.71743408225755e-05,-9.86471321295922e-05,9.99457201028563e-05,0.199993511063309,0.000148141223419804 -0.732000000000001,0.559744987949979,-0.292902057074959,0.141526844654905,-7.2011138135819e-05,-3.42981571893608e-05,0.199993695218878,4.7007862880033e-05,-9.80153701257684e-05,0.000100114912887639,0.199993711124298,0.00014778277212158 -0.734000000000001,0.559744844021718,-0.292902125867305,0.141926832245573,-7.19172886243235e-05,-3.44935552065717e-05,0.199993895617222,4.68418012711247e-05,-9.73817415594862e-05,0.000100282771670068,0.199993911522461,0.000147424530622665 -0.736000000000001,0.559744700280825,-0.292902195049179,0.142326820237374,-7.18237709307345e-05,-3.46876841555987e-05,0.199994096349965,4.66760935458054e-05,-9.67462672474272e-05,0.000100449348491849,0.199994112255157,0.000147066548023146 -0.738000000000001,0.559744556726635,-0.292902264618041,0.142726808630973,-7.17305842501403e-05,-3.48805402755614e-05,0.199994297414616,4.65108229708022e-05,-9.61089124951186e-05,0.000100614615597405,0.19999431331989,0.000146708829271552 -0.740000000000001,0.559744413358488,-0.29290233457134,0.143126797427032,-7.16377276388513e-05,-3.50721198055792e-05,0.199994498808427,4.63459964850088e-05,-9.54697154664785e-05,0.000100778546965885,0.199994514713908,0.000146351404127169 -0.742000000000001,0.559744270175724,-0.29290240490652,0.143526786626207,-7.15452002642002e-05,-3.52624191374273e-05,0.199994700528803,4.61815516383801e-05,-9.48286761615068e-05,0.000100941175557034,0.199994716434609,0.000145994295974563 -0.744000000000001,0.559744127177687,-0.292902475621017,0.143926776229147,-7.14530014322978e-05,-3.54514345102252e-05,0.199994902573129,4.60175092475978e-05,-9.41857598857342e-05,0.000101102458002766,0.199994918479373,0.000145637479849134 -0.746000000000001,0.559743984363718,-0.292902546712258,0.144326766236499,-7.13611302272098e-05,-3.56391621769702e-05,0.199995104938635,4.58539109460254e-05,-9.35410013336301e-05,0.000101262416854486,0.199995120845426,0.000145281028172146 -0.748000000000001,0.559743841733166,-0.292902618177666,0.144726756648902,-7.12695857885137e-05,-3.58255985155597e-05,0.199995307622797,4.56907151002994e-05,-9.2894435199664e-05,0.000101421065989982,0.199995323530238,0.000144924981159169 -0.750000000000001,0.559743699285375,-0.292902690014653,0.145126747466991,-7.11783673668086e-05,-3.60107399177689e-05,0.199995510622899,4.55279286493137e-05,-9.22460198504726e-05,0.000101578353367548,0.199995526531089,0.000144569299431841 -0.752000000000001,0.559743557019696,-0.292902762220625,0.145526738691394,-7.10874740739164e-05,-3.61945825949616e-05,0.19999571393621,4.5365579348644e-05,-9.1595737938821e-05,0.00010173432062055,0.199995729845242,0.000144214031877346 -0.754000000000001,0.559743414935479,-0.292902834792983,0.145926730322735,-7.09969050494141e-05,-3.63771228695242e-05,0.199995917560182,4.52036741371841e-05,-9.09436553842013e-05,0.000101888959075369,0.199995933470145,0.000143859238812123 -0.756000000000001,0.559743273032076,-0.292902907729117,0.146326722361634,-7.09066593773677e-05,-3.65583572164984e-05,0.199996121492046,4.50421644426768e-05,-9.02897721866135e-05,0.000102042227098642,0.199996137403024,0.000143504897653155 -0.758000000000001,0.559743131308841,-0.292902981026412,0.146726714808704,-7.08167363916434e-05,-3.67382819582706e-05,0.19999632572909,4.48810363873342e-05,-8.96340536515882e-05,0.000102194145507051,0.199996341641161,0.000143151019476402 -0.760000000000001,0.55974298976513,-0.292903054682245,0.147126707664551,-7.07271352318183e-05,-3.69168934311048e-05,0.199996530268628,4.47203871156709e-05,-8.89765240652539e-05,0.000102344735117277,0.199996546181865,0.000142797687883018 -0.762000000000001,0.559742848400301,-0.292903128693985,0.147526700929778,-7.06378548431807e-05,-3.70941880545317e-05,0.199996735108031,4.45601749943236e-05,-8.83171938359516e-05,0.000102493961234851,0.199996751022503,0.000142444894651205 -0.764000000000001,0.559742707213711,-0.292903203058997,0.147926694604983,-7.0548894531841e-05,-3.72701622064486e-05,0.199996940244473,4.44003583899288e-05,-8.76560490858934e-05,0.00010264179783892,0.199996956160245,0.000142092622715286 -0.766000000000001,0.559742566204722,-0.292903277774634,0.148326688690756,-7.0460253409621e-05,-3.74448122508752e-05,0.199997145675222,4.42409997525317e-05,-8.69931106317611e-05,0.00010278829697119,0.199997161592352,0.00014174096567588 -0.768000000000001,0.559742425372697,-0.292903352838246,0.148726683187684,-7.03719305328309e-05,-3.76181346489756e-05,0.199997351397661,4.40820574487688e-05,-8.63283992902363e-05,0.000102933430876084,0.199997367316203,0.000141389927168991 -0.770000000000001,0.559742284717,-0.292903428247173,0.149126678096347,-7.02839251798259e-05,-3.77901258480362e-05,0.199997557408946,4.39235314786401e-05,-8.56619011835313e-05,0.000103077187410538,0.199997573328949,0.000141039513954067 -0.772000000000001,0.559742144236997,-0.292903503998749,0.14952667341732,-7.01962364069163e-05,-3.79607822537098e-05,0.199997763706411,4.37654495977213e-05,-8.49936267199868e-05,0.000103219578717617,0.19999777962792,0.000140689774298452 -0.774000000000001,0.559742003932055,-0.292903580090302,0.149926669151172,-7.0108863381435e-05,-3.81301003549161e-05,0.19999797028726,4.36078048671184e-05,-8.43235897773908e-05,0.000103360582245915,0.199997986210315,0.000140340722530878 -0.776000000000001,0.559741863801543,-0.292903656519151,0.150326665298469,-7.00218051874479e-05,-3.82980766128193e-05,0.19999817714874,4.34505764701498e-05,-8.36517660696145e-05,0.00010350021187322,0.199998193073374,0.000139992372720642 -0.778000000000001,0.559741723844834,-0.292903733282608,0.150726661859767,-6.99350610755544e-05,-3.84647074191946e-05,0.199998384288108,4.32937852234971e-05,-8.29782111078092e-05,0.00010363844678285,0.199998400214352,0.000139644774371231 -0.780000000000001,0.559741584061299,-0.29290381037798,0.151126658835621,-6.98486300465539e-05,-3.86299894572506e-05,0.199998591702527,4.31374796994177e-05,-8.23029214225279e-05,0.000103775307791487,0.199998607630406,0.000139297981093476 -0.782000000000001,0.559741444450314,-0.292903887802566,0.151526656226577,-6.97625111567567e-05,-3.87939191048847e-05,0.199998799389339,4.29815766311847e-05,-8.1625865788748e-05,0.000103910777551896,0.199998815318874,0.000138951961435976 -0.784000000000001,0.559741305011254,-0.292903965553657,0.151926654033178,-6.96767037400291e-05,-3.89564929204056e-05,0.199999007345637,4.28260898965859e-05,-8.0947092778727e-05,0.000104044810961268,0.199999023276844,0.000138606740431785 -0.786000000000001,0.559741165743499,-0.292904043628538,0.15232665225596,-6.95912067971704e-05,-3.91177074759996e-05,0.199999215568583,4.26710611289848e-05,-8.0266578106336e-05,0.000104177475673817,0.199999231501473,0.000138262393500701 -0.788000000000001,0.559741026646427,-0.292904122024487,0.152726650895453,-6.95060194955132e-05,-3.92775592328309e-05,0.19999942405554,4.25164833894875e-05,-7.95843668743856e-05,0.000104308754342308,0.199999439990119,0.000137918958243779 -0.790000000000001,0.559740887719421,-0.292904200738775,0.153126649952182,-6.94211408636124e-05,-3.94360449434971e-05,0.1999996328036,4.23623497392e-05,-7.89004590828756e-05,0.000104438594925038,0.19999964873987,0.000137576419595914 -0.792000000000001,0.559740748961864,-0.292904279768667,0.153526649426667,-6.93365700965564e-05,-3.95931610691624e-05,0.19999984180992,4.22086324225468e-05,-7.82148200373366e-05,0.000104567032116476,0.199999857747877,0.00013723480218262 -0.794000000000001,0.55974061037314,-0.292904359111419,0.153926649319422,-6.92523063339223e-05,-3.97489042236465e-05,0.200000051071729,4.2055386950679e-05,-7.7527494840579e-05,0.000104694058977728,0.200000067011367,0.000136894170851279 -0.796000000000001,0.559740471952638,-0.292904438764283,0.154326649630954,-6.91683485487537e-05,-3.99032710485247e-05,0.200000260586155,4.19025716902332e-05,-7.68384973703905e-05,0.000104819663365729,0.200000276527463,0.000136554538630173 -0.798000000000001,0.559740333699746,-0.292904518724503,0.154726650361766,-6.90846960471613e-05,-4.0056258213128e-05,0.200000470350382,4.17501866412095e-05,-7.61478310962182e-05,0.000104943836606863,0.200000486293342,0.000136215928242633 -0.800000000000001,0.559740195613854,-0.292904598989316,0.155126651512356,-6.90013478021888e-05,-4.02078623729096e-05,0.200000680361502,4.15982734369713e-05,-7.54554960180619e-05,0.000105066580435853,0.200000696306092,0.00013587838149151 -0.802000000000001,0.559740057694355,-0.292904679555953,0.155526653083212,-6.89183029534134e-05,-4.03580801972003e-05,0.200000890616704,4.14467765663673e-05,-7.47615129526035e-05,0.00010518791046521,0.2000009065629,0.000135541933049303 -0.804000000000001,0.559739919940642,-0.292904760421637,0.155926655074822,-6.88355606959234e-05,-4.050690842472e-05,0.200001101113144,4.12957446016548e-05,-7.40658818998429e-05,0.000105307797204634,0.200001117060915,0.000135206603151987 -0.806000000000001,0.559739782352112,-0.292904841583586,0.156326657487665,-6.87531199750068e-05,-4.06543437247997e-05,0.200001311847893,4.114517060394e-05,-7.33686063292271e-05,0.000105426252797192,0.200001327797206,0.000134872429493866 -0.808000000000001,0.559739644928162,-0.292904923039012,0.156726660322214,-6.86709800135076e-05,-4.08003828500369e-05,0.200001522818155,4.09950129398561e-05,-7.26697139963316e-05,0.000105543254691476,0.20000153876897,0.000134539425556665 -0.810000000000001,0.559739507668192,-0.292905004785118,0.157126663578938,-6.85891399232474e-05,-4.0945022580785e-05,0.200001734020912,4.0845327120561e-05,-7.19692222483914e-05,0.000105658795948593,0.200001749973185,0.000134207640840343 -0.812000000000001,0.559739370571603,-0.292905086819102,0.157526667258298,-6.85075987050254e-05,-4.10882597390305e-05,0.200001945453339,4.06961131460548e-05,-7.12671172076184e-05,0.000105772906058843,0.200001961407023,0.00013387712006319 -0.814000000000001,0.559739233637797,-0.292905169138157,0.157926671360751,-6.84263554706632e-05,-4.12300910496155e-05,0.200002157112536,4.05473155051794e-05,-7.05634092823536e-05,0.000105885539919415,0.200002173067579,0.000133547845689294 -0.816000000000001,0.559739096866181,-0.292905251739466,0.158326675886748,-6.83454094430047e-05,-4.13705133761599e-05,0.200002368995498,4.039895501462e-05,-6.98581401059605e-05,0.000105996728755331,0.200002384951845,0.000133219900210774 -0.818000000000001,0.55973896025616,-0.29290533462021,0.158726680836733,-6.82647596506047e-05,-4.15095236100393e-05,0.200002581099451,4.02510941244216e-05,-6.91512923312042e-05,0.000106106472566592,0.200002597057039,0.000132893323119762 -0.820000000000001,0.559738823807142,-0.292905417777561,0.159126686211146,-6.8184405066507e-05,-4.16471185454847e-05,0.200002793421389,4.01036703845392e-05,-6.84428798358727e-05,0.00010621472798511,0.200002809380154,0.000132568098045836 -0.822000000000001,0.559738687518539,-0.292905501208685,0.159526692010418,-6.81043449690665e-05,-4.17832951293828e-05,0.200003005958363,3.99566352227154e-05,-6.77329199672005e-05,0.000106321514092844,0.200003021918236,0.000132244264543303 -0.824000000000001,0.559738551389762,-0.292905584910741,0.159926698234979,-6.80245785256161e-05,-4.19180502253535e-05,0.200003218707445,3.98100927223588e-05,-6.702141966408e-05,0.000106426808338389,0.200003234668353,0.000131921869716312 -0.826000000000001,0.559738415420225,-0.292905668880885,0.160326704885248,-6.79451045981771e-05,-4.20513808080391e-05,0.200003431665596,3.9664063700151e-05,-6.6308410151537e-05,0.000106530631538426,0.200003447627461,0.000131600982754727 -0.828000000000001,0.559738279609344,-0.292905753116265,0.160726711961641,-6.78659222708155e-05,-4.21832838659597e-05,0.200003644829971,3.95184301948958e-05,-6.55938810212308e-05,0.000106632983692956,0.200003660792712,0.000131281593392823 -0.830000000000001,0.559738143956536,-0.292905837614021,0.161126719464568,-6.77870308773975e-05,-4.2313756332124e-05,0.200003858197531,3.93732338399566e-05,-6.48778600287336e-05,0.000106733830107508,0.200003874161061,0.000130963730682744 -0.832000000000001,0.55973800846122,-0.29290592237129,0.161526727394431,-6.77084293354557e-05,-4.24427953060746e-05,0.200004071765292,3.92285370853784e-05,-6.41603541129409e-05,0.000106833195068212,0.200004087729522,0.00013064746737017 -0.834000000000001,0.559737873122819,-0.292906007385202,0.161926735751629,-6.7630116729056e-05,-4.25703977485758e-05,0.200004285530311,3.90842636033284e-05,-6.34413563349589e-05,0.000106931057758386,0.200004301495147,0.000130332791224469 -0.836000000000001,0.559737737940754,-0.292906092652881,0.162326744536553,-6.75520922810424e-05,-4.26965607314144e-05,0.200004499489523,3.89404342104882e-05,-6.27209187364918e-05,0.000107027412973859,0.200004515454866,0.00013001976077323 -0.838000000000001,0.55973760291445,-0.292906178171445,0.162726753749587,-6.7474354992214e-05,-4.28212814235218e-05,0.200004713639963,3.87970974791152e-05,-6.19990482564336e-05,0.000107122272857696,0.200004729605713,0.000129708434959744 -0.840000000000001,0.559737468043334,-0.292906263938007,0.163126763391113,-6.73969038911259e-05,-4.29445569244402e-05,0.200004927978614,3.86542187147398e-05,-6.12757136697616e-05,0.000107215611389044,0.200004943944664,0.000129398798432258 -0.842000000000001,0.559737333326834,-0.292906349949673,0.163526773461502,-6.73197381173551e-05,-4.30663842782008e-05,0.200005142502409,3.85117493451048e-05,-6.05509392626046e-05,0.000107307426833181,0.200005158468649,0.000129090878596291 -0.844000000000001,0.559737198764381,-0.292906436203544,0.163926783961122,-6.72428568937455e-05,-4.31867606814906e-05,0.200005357208321,3.83697518202552e-05,-5.98248013627955e-05,0.00010739773653734,0.200005373174638,0.000128784775987652 -0.846000000000001,0.559737064355406,-0.292906522696715,0.164326794890335,-6.71662591100741e-05,-4.3305683483652e-05,0.200005572093355,3.82282400179789e-05,-5.90972687453117e-05,0.000107486526623734,0.200005588059631,0.00012848050013003 -0.848000000000001,0.559736930099345,-0.292906609426278,0.164726806249496,-6.70899439336736e-05,-4.34231497564718e-05,0.200005787154428,3.80871445493369e-05,-5.83683310018123e-05,0.000107573772806235,0.200005803120542,0.000128178037274826 -0.850000000000001,0.559736795995631,-0.292906696389314,0.165126818038953,-6.70139105318767e-05,-4.35391568076593e-05,0.200006002388446,3.79465209254803e-05,-5.76380609906834e-05,0.000107659495901524,0.200006018354272,0.000127877486614665 -0.852000000000001,0.559736662043703,-0.292906783582905,0.165526830259049,-6.69381578499716e-05,-4.36537020004346e-05,0.200006217792412,3.78063691464092e-05,-5.69064240174554e-05,0.000107743687235984,0.200006233757821,0.000127578857110786 -0.854000000000001,0.559736528242999,-0.292906871004122,0.165926842910122,-6.68626850552911e-05,-4.37667825037291e-05,0.200006433363195,3.76666406398662e-05,-5.6173451307151e-05,0.000107826322523485,0.200006449328056,0.000127282160092404 -0.856000000000001,0.559736394592962,-0.292906958650035,0.166326855992502,-6.67874912874122e-05,-4.38783958056632e-05,0.200006649097702,3.75273770392148e-05,-5.54391740847925e-05,0.000107907419111264,0.200006665061877,0.000126987468475835 -0.858000000000001,0.559736261093034,-0.292907046517706,0.166726869506513,-6.67125755471342e-05,-4.39885392000682e-05,0.200006864992871,3.73885922222427e-05,-5.47035750031454e-05,0.000107986968325702,0.200006880956221,0.000126694803660853 -0.860000000000001,0.55973612774266,-0.292907134604192,0.167126883452474,-6.66379369185232e-05,-4.40972101056758e-05,0.200007081045575,3.72502514944805e-05,-5.396669916502e-05,0.000108064942411223,0.200007097007956,0.000126404183607628 -0.862000000000001,0.559735994541287,-0.292907222906546,0.167526897830696,-6.65635745411563e-05,-4.42044059967283e-05,0.200007297252641,3.71123617948221e-05,-5.32285396315224e-05,0.000108141353510893,0.200007313213905,0.000126115649974388 -0.864000000000001,0.559735861488362,-0.292907311421816,0.167926912641484,-6.64894874713439e-05,-4.43101242642018e-05,0.200007513610989,3.69749370010552e-05,-5.24891449749098e-05,0.000108216220706669,0.200007529570986,0.00012582927612628 -0.866000000000001,0.559735728583337,-0.292907400147043,0.16832692788514,-6.64156747931521e-05,-4.4414362576628e-05,0.200007730117524,3.68379493576043e-05,-5.17485221340763e-05,0.000108289504099912,0.2000077460761,0.000125545054678148 -0.868000000000001,0.559735595825663,-0.292907489079266,0.168726943561954,-6.63421356739135e-05,-4.45171183527382e-05,0.200007946769005,3.67014127422571e-05,-5.10066468228931e-05,0.000108361208894792,0.200007962726003,0.000125263016531351 -0.870000000000001,0.559735463214794,-0.292907578215517,0.169126959672216,-6.62688691421831e-05,-4.46183891639195e-05,0.200008163562359,3.6565375727271e-05,-5.02635641441706e-05,0.000108431352438543,0.200008179517617,0.000124983241973487 -0.872000000000001,0.559735330750186,-0.292907667552823,0.169526976216204,-6.61958741710044e-05,-4.47181726093148e-05,0.200008380494415,3.64297689237069e-05,-4.95192914451436e-05,0.000108499903506143,0.200008396447768,0.000124705723003822 -0.874000000000001,0.559735198431297,-0.292907757088207,0.169926993194193,-6.61231500664882e-05,-4.48164663297001e-05,0.200008597561973,3.6294585392671e-05,-4.87738668897286e-05,0.000108566856893422,0.200008613513254,0.000124430500424613 -0.876000000000001,0.559735066257586,-0.292907846818688,0.170327010606452,-6.60506958294337e-05,-4.49132680768738e-05,0.200008814761843,3.61598945231023e-05,-4.80272835390316e-05,0.000108632210865656,0.200008830710879,0.000124157622293682 -0.878000000000001,0.559734934228514,-0.292907936741279,0.170727028453241,-6.59785104883958e-05,-4.50085754638562e-05,0.200009032090817,3.60256546816373e-05,-4.72795240458179e-05,0.000108695975831186,0.200009048037435,0.000123887111319674 -0.880000000000001,0.559734802343544,-0.29290802685299,0.171127046734815,-6.59065932107072e-05,-4.5102386173057e-05,0.200009249545746,3.58918450515943e-05,-4.65306682073674e-05,0.000108758137912224,0.200009265489769,0.00012361901223989 -0.882000000000001,0.559734670602141,-0.292908117150824,0.171527065451424,-6.58349431081894e-05,-4.51946981366857e-05,0.200009467123369,3.57585142052308e-05,-4.57807333709148e-05,0.000108818679761535,0.200009483064615,0.000123353362139781 -0.884000000000001,0.559734539003772,-0.292908207631782,0.171927084603308,-6.57635591538863e-05,-4.52855091065407e-05,0.200009684820465,3.5625627448077e-05,-4.50296779030967e-05,0.000108877610052738,0.20000970075875,0.000123090175050334 -0.886000000000001,0.559734407547904,-0.29290829829286,0.172327104190705,-6.56924405983971e-05,-4.53748168482981e-05,0.200009902633809,3.54931778412393e-05,-4.42775503761705e-05,0.000108934927051107,0.200009918568946,0.000122829497565586 -0.888000000000001,0.559734276234009,-0.29290838913105,0.172727124213844,-6.56215864425213e-05,-4.54626193080454e-05,0.200010120560173,3.53612208958686e-05,-4.35243993623935e-05,0.000108990639430262,0.200010136491972,0.00012257140272723 -0.890000000000001,0.559734145061559,-0.292908480143338,0.173127144672946,-6.55509957148137e-05,-4.55489144457477e-05,0.200010338596367,3.52296802841323e-05,-4.27702248617656e-05,0.000109044709026285,0.200010354524634,0.000122315865995689 -0.892000000000001,0.559734014030026,-0.292908571326708,0.173527165568229,-6.54806677213848e-05,-4.56337002074924e-05,0.200010556739009,3.50985907004997e-05,-4.2015019935393e-05,0.000109097101144706,0.200010572663549,0.000122062895457673 -0.894000000000001,0.559733883138888,-0.292908662678139,0.173927186899902,-6.54106013520117e-05,-4.57169745254892e-05,0.200010774984771,3.49679799005464e-05,-4.12588192777452e-05,0.000109147881705018,0.200010790905384,0.000121812601430276 -0.896000000000001,0.559733752387621,-0.292908754194606,0.174327208668168,-6.53407958017826e-05,-4.57987354846034e-05,0.200010993330536,3.48377923731213e-05,-4.05016437055039e-05,0.000109197040298881,0.20001100924702,0.000121564997142745 -0.898000000000001,0.559733621775705,-0.29290884587308,0.174727230873224,-6.52712501825192e-05,-4.58789811003113e-05,0.200011211772932,3.47080697515878e-05,-3.97435140353508e-05,0.000109244517945695,0.200011227685082,0.000121320079603281 -0.900000000000001,0.55973349130262,-0.29290893771053,0.17512725351526,-6.52019635227762e-05,-4.59577095407448e-05,0.200011430308608,3.45787842803702e-05,-3.89844684312024e-05,0.000109290351074653,0.200011446216216,0.000121077916810593 -0.902000000000001,0.559733360967851,-0.292909029703919,0.175527276594459,-6.51329350453977e-05,-4.60349189740361e-05,0.200011648934337,3.44499498372563e-05,-3.82244895458239e-05,0.000109334534481586,0.200011664837192,0.000120838533072582 -0.904000000000001,0.55973323077088,-0.292909121850206,0.175927300110997,-6.50641637234272e-05,-4.61106074989281e-05,0.200011867646746,3.43215872389279e-05,-3.74636051347909e-05,0.00010937705602343,0.200011883544636,0.000120601962855699 -0.906000000000001,0.559733100711196,-0.292909214146349,0.176327324065046,-6.4995648696442e-05,-4.61847733945752e-05,0.200012086442561,3.41936479131277e-05,-3.67018707092548e-05,0.000109417893148778,0.200012102335269,0.000120368219278525 -0.908000000000001,0.559732970788285,-0.2929093065893,0.176727348456768,-6.49273891317747e-05,-4.62574149817651e-05,0.200012305318318,3.40661665543251e-05,-3.59392758608745e-05,0.000109457033714566,0.200012321205626,0.000120137327599305 -0.910000000000001,0.559732841001639,-0.292909399176009,0.177127373286319,-6.48593840302247e-05,-4.63285304980187e-05,0.200012524270696,3.39391154069446e-05,-3.5175834467438e-05,0.000109494524558329,0.200012540152381,0.000119909356571291 -0.912000000000001,0.559732711350749,-0.292909491903422,0.17752739855385,-6.47916326701469e-05,-4.63981183196349e-05,0.200012743296417,3.38124944709861e-05,-3.44115951012025e-05,0.00010953033272032,0.200012759172257,0.000119684319965414 -0.914000000000001,0.559732581835108,-0.292909584768482,0.177927424259505,-6.47241340523407e-05,-4.64661768784235e-05,0.200012962392026,3.36863592576009e-05,-3.36465473538272e-05,0.000109564432179687,0.200012978261795,0.000119462235655134 -0.916000000000001,0.559732452454213,-0.292909677768129,0.178327450403418,-6.46568872331165e-05,-4.65327045090502e-05,0.200013181554145,3.35606681334255e-05,-3.28807155114408e-05,0.000109596826405878,0.200013197417613,0.000119243130856022 -0.918000000000001,0.55973232320756,-0.2929097708993,0.178727476985721,-6.4589891379807e-05,-4.65976997404693e-05,0.200013400779332,3.34353933428843e-05,-3.21141585546414e-05,0.000109627511929444,0.200013416636268,0.000119027039278924 -0.920000000000001,0.559732194094647,-0.292909864158928,0.179127504006536,-6.4523145659745e-05,-4.66611611432688e-05,0.200013620064193,3.33105557026591e-05,-3.13468695445351e-05,0.000109656513036516,0.200013635914364,0.000118814015178301 -0.922000000000001,0.559732065114977,-0.292909957543944,0.179527531465978,-6.44566491569964e-05,-4.67230872186474e-05,0.200013839405384,3.31861829683255e-05,-3.0578848481122e-05,0.000109683814114581,0.200013855248554,0.000118604080002167 -0.924000000000001,0.559731936268051,-0.292910051051277,0.179927559364157,-6.43904009278717e-05,-4.67834765371932e-05,0.20001405879945,3.30622473843078e-05,-2.98101404672124e-05,0.000109709364856658,0.200014074635379,0.000118397218776572 -0.926000000000001,0.559731807553373,-0.292910144677851,0.180327587701176,-6.43244001674592e-05,-4.68423277805163e-05,0.200014278242844,3.29387420117122e-05,-2.90407906056167e-05,0.000109733184344707,0.200014294071293,0.000118193485398912 -0.928000000000001,0.55973167897045,-0.292910238420588,0.180727616477129,-6.42586459598248e-05,-4.68996396996157e-05,0.200014497732187,3.28156737894325e-05,-2.82708023657818e-05,0.000109755295130132,0.200014513552914,0.000117992929965154 -0.930000000000001,0.559731550518789,-0.292910332276409,0.181127645692105,-6.41931374723014e-05,-4.69554109899794e-05,0.200014717264024,3.2693021900787e-05,-2.75001757477078e-05,0.00010977567119208,0.200014733076784,0.000117795548866805 -0.932000000000001,0.5597314221979,-0.292910426242232,0.181527675346185,-6.41278738722217e-05,-4.70096404026065e-05,0.200014936834872,3.25708487958209e-05,-2.6728952384758e-05,0.000109794295183319,0.200014952639417,0.000117601378995608 -0.934000000000001,0.559731294007294,-0.292910520314971,0.181927705439444,-6.40628540771181e-05,-4.70623267995185e-05,0.200015156441205,3.24491336578525e-05,-2.59571600325081e-05,0.000109811165369122,0.200015172237287,0.000117410444475026 -0.936000000000001,0.559731165946484,-0.292910614491539,0.18232773597195,-6.39980773375903e-05,-4.71134690427366e-05,0.200015376079533,3.23277793423671e-05,-2.51848125687459e-05,0.000109826304300897,0.200015391866901,0.000117222767488764 -0.938000000000001,0.559731038014984,-0.292910708768847,0.182727766943762,-6.39335429597487e-05,-4.71630660497934e-05,0.200015595746422,3.22068621771976e-05,-2.44119308101531e-05,0.000109839668610556,0.200015611524823,0.00011703835754853 -0.940000000000001,0.559730910212312,-0.292910803143803,0.183127798354935,-6.38692498888815e-05,-4.72111167659772e-05,0.200015815438208,3.20864307346014e-05,-2.36385355734114e-05,0.000109851260032823,0.200015831207387,0.000116857257935229 -0.942000000000001,0.559730782537985,-0.292910897613314,0.183527830205515,-6.38051972368103e-05,-4.72576201920871e-05,0.200016035151462,3.19664086867455e-05,-2.28646719613312e-05,0.000109861101119102,0.200016050911163,0.000116679501401612 -0.944000000000001,0.559730654991523,-0.292910992174284,0.183927862495541,-6.37413842541345e-05,-4.73025754538225e-05,0.200016254882612,3.18468168503116e-05,-2.20903365044655e-05,0.000109869155440201,0.200016270632576,0.000116505081552355 -0.946000000000001,0.559730527572448,-0.292911086823616,0.184327895225045,-6.3677809969409e-05,-4.73459815381049e-05,0.200016474628084,3.17276621641937e-05,-2.13155465500492e-05,0.000109875436873907,0.20001649036805,0.000116334039297896 -0.948000000000001,0.559730400280283,-0.29291118155821,0.184727928394053,-6.36144736054777e-05,-4.73878376400227e-05,0.20001669438436,3.16088960561345e-05,-2.05403714870211e-05,0.000109879935011881,0.200016710114067,0.000116166386226513 -0.950000000000001,0.559730273114553,-0.292911276374967,0.185127962002583,-6.35513743851845e-05,-4.7428143024053e-05,0.200016914147824,3.14905948539667e-05,-1.97647974375936e-05,0.000109882634241609,0.200016929867009,0.000116002146919275 -0.952000000000001,0.559730146074786,-0.292911371270782,0.185527996050645,-6.34885112260619e-05,-4.74668968297731e-05,0.200017133914897,3.1372730802115e-05,-1.89888521573422e-05,0.000109883543236711,0.200017149623294,0.00011584134762031 -0.954000000000001,0.559730019160509,-0.292911466242554,0.185928030538242,-6.3425883461976e-05,-4.75040984326824e-05,0.200017353681997,3.12552345116401e-05,-1.82125738101835e-05,0.00010988263944578,0.200017369379339,0.000115683974606426 -0.956000000000001,0.559729892371252,-0.292911561287176,0.186328065465373,-6.33634902880153e-05,-4.75397471250138e-05,0.200017573445454,3.11382031270568e-05,-1.74359658655643e-05,0.000109879917664646,0.200017589131474,0.000115530069270456 -0.958000000000001,0.559729765706547,-0.292911656401542,0.186728100832024,-6.33013306494678e-05,-4.75738422961447e-05,0.200017793201668,3.10216158316834e-05,-1.66590699568481e-05,0.00010987538830165,0.200017808876095,0.000115379661184604 -0.960000000000001,0.559729639165929,-0.292911751582545,0.187128136638179,-6.32394038246886e-05,-4.76063834048412e-05,0.200018012947008,3.09054101754746e-05,-1.58819034312696e-05,0.000109869034009558,0.200018028609571,0.000115232737834818 -0.962000000000001,0.559729512748932,-0.292911846827076,0.187528172883812,-6.31777090087659e-05,-4.76373699098697e-05,0.200018232677804,3.07896139140062e-05,-1.51045009832984e-05,0.000109860853053645,0.200018248328231,0.000115089327518248 -0.964000000000001,0.559729386455093,-0.292911942132025,0.187928209568891,-6.31162453690325e-05,-4.76668014087744e-05,0.20001845239042,3.06742339861721e-05,-1.43268764907223e-05,0.000109850850638082,0.200018468028437,0.000114949455877482 -0.966000000000001,0.559729260283951,-0.292912037494282,0.188328246693374,-6.30550120728212e-05,-4.76946774158326e-05,0.200018672081206,3.05592981475477e-05,-1.35490403618821e-05,0.000109839018089253,0.200018687706539,0.000114813139896136 -0.968000000000001,0.559729134235045,-0.292912132910735,0.188728284257215,-6.29940081764424e-05,-4.77209975702219e-05,0.200018891746492,3.04447855814516e-05,-1.27710411690351e-05,0.000109825318977963,0.200018907358864,0.000114680360787072 -0.970000000000001,0.559729008307918,-0.292912228378272,0.18912832226036,-6.29332329304954e-05,-4.77457615805088e-05,0.200019111382482,3.03306685323079e-05,-1.1992910137204e-05,0.000109809753304213,0.200019126981615,0.000114551129492065 -0.972000000000001,0.559728882502113,-0.292912323893781,0.189528360702745,-6.28726855023131e-05,-4.77689692107707e-05,0.200019330985505,3.02169747556924e-05,-1.12146576747296e-05,0.000109792355762472,0.200019346571121,0.000114425501939126 -0.974000000000001,0.559728756817176,-0.292912419454149,0.189928399584302,-6.28123650314726e-05,-4.77906202112077e-05,0.200019550551905,3.01036764960294e-05,-1.04362941899527e-05,0.000109773100331889,0.200019566123724,0.000114303460715202 -0.976000000000001,0.559728631252653,-0.292912515056262,0.190328438904953,-6.2752270796329e-05,-4.78107143875306e-05,0.200019770077907,2.9990766814425e-05,-9.65787172457765e-06,0.000109751971399952,0.200019785635648,0.000114185006925346 -0.978000000000001,0.559728505808093,-0.292912610697006,0.190728478664614,-6.26924019642149e-05,-4.7829251698106e-05,0.200019989559791,2.98783012220305e-05,-8.87941109528611e-06,0.000109728965497213,0.200020005103173,0.000114070165663078 -0.980000000000001,0.559728380483045,-0.292912706373269,0.191128518863192,-6.26327575914409e-05,-4.78462320319117e-05,0.200020208993769,2.97662450243763e-05,-8.10092271041895e-06,0.000109704068745886,0.20002022452251,0.000113958926771945 -0.982000000000001,0.559728255277062,-0.292912802081934,0.191528559500589,-6.25733369841174e-05,-4.78616553889477e-05,0.200020428376066,2.96545635269929e-05,-7.32246555057436e-06,0.000109677295023758,0.200020443889882,0.000113851309537678 -0.984000000000001,0.559728130189697,-0.29291289781989,0.191928600576696,-6.25141393373329e-05,-4.7875521894114e-05,0.200020647702949,2.95432914243498e-05,-6.54403614630538e-06,0.000109648623514147,0.200020663201556,0.000113747312565244 -0.986000000000001,0.559728005220505,-0.292912993584022,0.1923286420914,-6.245516381842e-05,-4.78878315335329e-05,0.20002086697056,2.94324425942349e-05,-5.76563796705897e-06,0.00010961805248233,0.200020882453674,0.000113646947553592 -0.988000000000001,0.559727880369042,-0.292913089371216,0.192728684044579,-6.2396409566956e-05,-4.78985844459822e-05,0.200021086175159,2.93219892810725e-05,-4.98735427956198e-06,0.000109585604479712,0.200021101642493,0.000113550242039782 -0.990000000000001,0.559727755634867,-0.29291318517836,0.193128726436101,-6.23378758612957e-05,-4.79077809506511e-05,0.200021305312978,2.92119106681809e-05,-4.20919896160221e-06,0.000109551241342376,0.200021320764247,0.000113457161785582 -0.992000000000001,0.559727631017539,-0.29291328100234,0.193528769265831,-6.22795619242832e-05,-4.79154212418286e-05,0.200021524380124,2.91022483889236e-05,-3.4311546659449e-06,0.000109514923171683,0.200021539815041,0.00011336768534698 -0.994000000000001,0.559727506516619,-0.292913376840045,0.193928812533622,-6.22214668677401e-05,-4.79215055693149e-05,0.20002174337267,2.89929885655127e-05,-2.65327690374128e-06,0.000109476691600996,0.200021758790948,0.00011328185763037 -0.996000000000001,0.559727382131671,-0.292913472688362,0.194328856239321,-6.21635899700212e-05,-4.79260343494436e-05,0.200021962286891,2.88841034419423e-05,-1.87560383923987e-06,0.000109436572304222,0.20002197768824,0.000113199702645828 -0.998000000000001,0.559727257862259,-0.292913568544182,0.194728900382769,-6.21059304539723e-05,-4.79290079846719e-05,0.20002218111896,2.87756138357555e-05,-1.09815975790528e-06,0.000109394486177975,0.200022196503094,0.000113121154134059 -1,0.559727133707949,-0.292913664404394,0.195128944963797,-6.20484875146782e-05,-4.79304269884752e-05,0.200022399864835,6.47104314581583e-05,1.17001859593062e-05,-0.0248962396563058,0.200022415231465,0.0248963265032917 -1.002,0.559727009668309,-0.29291376026589,0.195528989982229,-6.18470887281397e-05,-4.78822072408347e-05,0.199922596160334,0.000208561473647326,6.05797356726379e-05,-0.124918854036565,0.199922611460676,0.124919042830152 -1.004,0.559726886319594,-0.292913855933223,0.195928635348438,-6.12142416200889e-05,-4.76881080457847e-05,0.199522724448689,0.000459439777888981,0.00014546347806843,-0.299958929307967,0.199522739538045,0.299959316434937 -1.006,0.559726764811343,-0.292913951018322,0.196327080880023,-6.00093296165837e-05,-4.73003533285609e-05,0.198722760443103,0.000743686244530916,0.000241945054268999,-0.500005546889836,0.198722775133022,0.500006158489317 -1.008,0.559726646282276,-0.292914045134637,0.196723526390211,-5.82394966419652e-05,-4.67203278287087e-05,0.19752270226113,0.00102349224928577,0.000337419873291944,-0.700052949590204,0.197522716372489,0.700053779090221 -1.01,0.559726531853356,-0.292914137899634,0.197117171689068,-5.59153606194406e-05,-4.59506738353932e-05,0.195922548644742,0.00129711919677477,0.00043125516352438,-0.900100815097448,0.195922562012248,0.900101853035709 -1.012,0.559726422620833,-0.292914228937332,0.19750721658479,-5.30510198548661e-05,-4.49953071746112e-05,0.19392229900074,0.00156282526503348,0.000522810704450815,-1.10014880104481,0.193922311477356,1.10015003531072 -1.014,0.559726319649277,-0.292914317880862,0.197892860885071,-4.96640595593067e-05,-4.38594310175899e-05,0.191521953440562,0.00181886569694533,0.000611439794628854,-1.30019654553948,0.191521964901829,1.30019796152887 -1.016,0.559726223964595,-0.292914404375056,0.198273304398552,-4.57755570670848e-05,-4.25495479960957e-05,0.188721512818582,0.00206349316800302,0.000696490178031526,-1.5002436680768,0.188721523166807,1.50024524885338 -1.018,0.559726136547048,-0.292914488079054,0.198647746936345,-4.14100868872946e-05,-4.10734703054638e-05,0.185520978768255,0.002294958015292,0.000777305032839679,-1.70028977063662,0.185520987936559,1.70029149711657 -1.02,0.559726058324248,-0.292914568668937,0.199015388313625,-3.65957250059168e-05,-3.9440327864737e-05,0.181920353736035,0.00251150856361848,0.000853224046970524,-1.90033443890278,0.181920361692227,1.90033629006723 -1.022,0.559725990164148,-0.292914645840366,0.199375428351289,-3.13640526328207e-05,-3.76605741175817e-05,0.177919641012644,0.0027113915071486,0.000923584472789506,-2.1003772436648,0.177919647762946,2.10037919680164 -1.024,0.559725932868037,-0.292914719311234,0.199727066877676,-2.57501589773223e-05,-3.5745989973579e-05,0.173518844761376,0.00289285265187011,0.000987722077738781,-2.30041774234613,0.173518850353986,2.30041977332231 -1.026,0.559725887163512,-0.292914788824326,0.200069503730335,-1.97926420253402e-05,-3.37096858066266e-05,0.16871797004326,0.00305413732498704,0.0010449723482353,-2.50045548068518,0.168717974571798,2.5004575642491 -1.028,0.559725853697469,-0.292914854149977,0.200401938757849,-1.35336096773741e-05,-3.15661005806378e-05,0.163517022838636,0.00319349088839793,0.0010946715374438,-2.70048999475615,0.16351702644553,2.7004921048705 -1.03,0.559725833029074,-0.292914915088728,0.200723571821689,-7.01867847174852e-06,-2.93309996568513e-05,0.157916010064235,0.00330915968238531,0.00113615756733298,-2.90052081296579,0.15791601294415,2.90052292317085 -1.032,0.559725825622755,-0.292914971473976,0.201033602798106,-2.96970947832875e-07,-2.70214703113058e-05,0.151914939586772,0.00339939158766622,0.00116877119787917,-3.10054745819577,0.151914941990249,3.10054954200276 -1.034,0.55972583184119,-0.292915023174609,0.201331231580036,6.57888787891636e-06,-2.46559148653347e-05,0.145513820231452,0.00346243698989834,0.00119185697075585,-3.30056945031797,0.145513822469026,3.30057148163242 -1.036,0.559725851938307,-0.292915070097635,0.201615658079031,1.35527770117605e-05,-2.22540424282824e-05,0.1387126617855,0.00349654961928624,0.00120476418424831,-3.50058630869085,0.138712664232718,3.50058826226534 -1.038,0.559725886052298,-0.292915112190779,0.201886082227178,2.05650863560613e-05,-1.98368581283414e-05,0.131511474996689,0.00349998735549306,0.00120684794102654,-3.70059755486033,0.13151147810069,3.70059940677611 -1.04,0.559725934198652,-0.292915149445068,0.202141703979018,2.75527264337327e-05,-1.74266506641762e-05,0.123910271566059,0.00347101347664135,0.0011974700987738,-3.90060271541342,0.123910275854811,3.90060444359073 -1.042,0.559725996263204,-0.292915181897382,0.202381723313443,3.44491402626268e-05,-1.50469777332462e-05,0.115909064135035,0.00340789761688053,0.00117600024163167,-4.10060132499494,0.115909070230998,4.10060290973083 -1.044,0.559726071995213,-0.292915209632978,0.202605340235558,4.11843169012549e-05,-1.27226496976496e-05,0.107507866266079,0.00329489877742839,0.00113709665294781,-4.2801538298609,0.107507874907371,4.28015524912587 -1.046,0.559726161000471,-0.29291523278798,0.202811754778507,4.76287353723403e-05,-1.0498591121455e-05,0.0987884488155912,0.00307491947182381,0.00106157932097672,-4.34575395964232,0.0987884608550388,4.34575517716166 -1.048,0.559726262510155,-0.292915251627343,0.203000494030821,5.34839947885501e-05,-8.47633241374268e-06,0.09012485042751,0.0027336183941462,0.00094485897925156,-4.23827163429507,0.0901248666959733,4.2382726211864 -1.05,0.55972637493645,-0.29291526669331,0.203172254180217,5.85632089489251e-05,-6.71915520444876e-06,0.0818353622784109,0.00235898493794462,0.000816859906105359,-4.04471370012322,0.0818353835088176,4.04471447052008 -1.052,0.55972649676299,-0.292915278503964,0.203327835479934,6.29199345403286e-05,-5.20889278932124e-06,0.0739459956270171,0.00201515260034313,0.000699331623860154,-3.84465549212696,0.073946022579457,3.84465608384504 -1.054,0.559726626616189,-0.292915287528881,0.203468038162725,6.66238193502977e-05,-3.92182870900814e-06,0.0664567403099031,0.00170527032078426,0.00059338974012224,-3.6446023173979,0.0664567738212738,3.64460276464259 -1.056,0.559726763258268,-0.292915294191279,0.203593662441174,6.97410158234656e-05,-2.83533382883227e-06,0.0593675863574255,0.00142758697224687,0.00049848331365454,-3.4445540629778,0.0593676273886264,3.44455439487709 -1.058,0.559726905580252,-0.292915298870217,0.203705508508155,7.23341672392851e-05,-1.92789545438998e-06,0.0526785240579918,0.00118034510637743,0.000414047972990827,-3.24451058548481,0.0526785737551517,3.2445108266075 -1.06,0.559727052594937,-0.29291530190286,0.203804376537406,7.44623962489754e-05,-1.17914193686896e-06,0.0463895440154862,0.000961782119224263,0.000339507880142697,-3.04447171429848,0.0463896037922676,3.04447188514757 -1.062,0.559727203429837,-0.292915303586784,0.203891066684217,7.61812957161822e-05,-5.69863933819192e-07,0.0405006372007979,0.000770131187988187,0.000274277423689683,-2.84443725456946,0.0405007088528734,2.84443737204967 -1.064,0.55972735732012,-0.292915304182316,0.203966379086209,7.75429210009281e-05,-8.20322421102303e-08,0.0350117949972084,0.000398973742932185,0.00021421258517007,-2.64440716362356,0.0350118808668982,2.64440720239729 -1.066,0.559727513601521,-0.292915303914913,0.204031113864205,7.77771906879109e-05,2.86986406861089e-07,0.0299230085463037,-0.00115535531142186,0.000141854781393613,-2.44438170280032,0.0299231096284446,2.4443819799601 -1.068,0.559727668428882,-0.292915303034371,0.204086071120394,7.29214997552407e-05,4.85386883464222e-07,0.0252342681860071,-0.00401650483705662,5.56669155216126e-05,-2.24435995451258,0.0252343735540246,2.24436354916703 -1.07,0.55972780528752,-0.292915301973366,0.20413205093695,6.17111713396845e-05,5.0965406894754e-07,0.0209455687282534,-0.00626615419968556,-1.15940174127971e-05,-2.04434003984142,0.0209456596429523,2.04434964311923 -1.072,0.559727915273568,-0.292915300995754,0.204169853395307,4.78568829564984e-05,4.39010813813034e-07,0.0170569080266414,-0.00686163004193217,-4.41210054602514e-05,-1.84432192848992,0.0170569751686306,1.84433469300511 -1.074,0.559727996715052,-0.292915300217322,0.204200278569056,3.42646511719557e-05,3.33170047106534e-07,0.0135682810142937,-0.00631623352231879,-5.29287308426162e-05,-1.64430670730048,0.0135683242834244,1.64431883930115 -1.076,0.559728052332172,-0.292915299663074,0.204224126519365,2.25919488672232e-05,2.27295890442569e-07,0.0104796811974395,-0.00517427409252313,-4.84691488333854e-05,-1.44429475571785,0.0104797055515784,1.44430402507772 -1.078,0.559728087082847,-0.292915299308139,0.204242197293846,1.35675548018632e-05,1.39293451772993e-07,0.00779110199142229,-0.00383122115282796,-3.79535396166641e-05,-1.24428593613682,0.00779111380604149,1.24429183496633 -1.08,0.559728106602392,-0.2929152991059,0.20425529092733,7.26706425591139e-06,7.54817319759126e-08,0.00550253745289219,-0.00255506633906055,-2.60802768270451e-05,-1.04427983641311,0.00550254225212326,1.04428296250737 -1.082,0.559728116151104,-0.292915299006212,0.204264207443657,3.34728944562101e-06,3.4972344464812e-08,0.00361398264576984,-0.00150598635645371,-1.56192385414311e-05,-0.844275922229352,0.00361398419607713,0.844277265532532 -1.084,0.559728119991549,-0.292915298966011,0.204269746857913,1.24311883009653e-06,1.30047778101883e-08,0.00212543376397478,-0.000754516962797601,-7.88577883548135e-06,-0.644273639664044,0.00212543412755078,0.644274081524305 -1.086,0.55972812112358,-0.292915298954193,0.204272709178713,3.29221594430606e-07,3.42922912288656e-09,0.00103688808711366,-0.000298867597336993,-3.12933359780664e-06,-0.444272481940274,0.00103688813938479,0.44427258247725 -1.088,0.559728121308436,-0.292915298952294,0.204273894410262,4.764844074856e-08,4.87443418961675e-10,0.000348343836213682,-8.15881945337436e-05,-8.50951253905661e-07,-0.233204047222351,0.000348343839472835,0.233204061496026 -1.09,0.559728121314173,-0.292915298952243,0.204274102554058,2.8688162956314e-09,2.54241072639161e-11,0.000104071898224256,-2.23898122264643e-05,-2.31009655848879e-07,-0.122135968994713,0.0001040718982638,0.122135971047168 +0,0.559809314390095,-0.292914056655931,0.00429163619695835,-1.48048240333765e-10,-0.000200533342459286,1.95850280437782e-10,-5.19473353222111e-07,-0.150532426496097,6.98751820293886e-07,0.000200533342459436,0.150532426498615 +0.002,0.559809314389799,-0.292914457722616,0.00429163619735005,-1.18699494677799e-09,-0.00050159819545148,1.59335392102555e-09,-1.63173641265502e-06,-0.275996818305951,2.19903956155676e-06,0.000501598195455415,0.275996818319535 +0.004,0.559809314385347,-0.292916063048713,0.00429163620333177,-6.67499389095383e-09,-0.00130452061568309,8.99200852666482e-09,-5.4644760938416e-06,-0.501856364427261,7.37314809901024e-06,0.00130452061573116,0.501856364511173 +0.006,0.559809314363099,-0.292919675805079,0.00429163623331809,-2.30448993221444e-08,-0.00250902365316052,3.10859463170665e-08,-1.33372132782306e-05,-0.702644430184557,1.80062275853543e-05,0.00250902365345893,0.702644430541854 +0.008,0.559809314293167,-0.292926099143326,0.00429163632767555,-6.00238470038761e-08,-0.00411509833642132,8.1016918868082e-08,-2.68721919716164e-05,-0.903428334621259,3.6288504319526e-05,0.0041150983376566,0.903428335749722 +0.01,0.559809314123003,-0.292936136198425,0.00429163655738576,-1.3053366720861e-07,-0.00612273699164556,1.76239963595171e-07,-4.76606254462553e-05,-1.10420872436942,6.43673517264354e-05,0.00612273699557351,1.10420872727408 +0.012,0.559809313771032,-0.292950590091292,0.00429163703263541,-2.50666348788897e-07,-0.008531933233899,3.38486325773824e-07,-7.72818961602972e-05,-1.30498624207123,0.000104371435105666,0.00853193324429563,1.30498624853333 +0.014,0.559809313120338,-0.29297026393136,0.00429163791133107,-4.39661251849799e-07,-0.0113426819599305,5.93725704017833e-07,-0.000117304062297041,-1.50576152713189,0.000158410287491584,0.0113426819839906,1.50576154003368 +0.016,0.559809312012387,-0.292995960819132,0.00429163940753823,-7.19882597977061e-07,-0.0145549793424265,9.72127475740158e-07,-0.000169284392059765,-1.70653521658001,0.000228573638315354,0.0145549793926933,1.70653524028392 +0.018,0.559809310240807,-0.29302848384873,0.00429164179984097,-1.11679882008886e-06,-0.0181688228262505,1.50802025727925e-06,-0.000234770286544616,-1.90730794599747,0.000316930657065505,0.0181688229231573,1.90730798677801 +0.02,0.559809307545192,-0.293068636110437,0.00429164543961925,-1.65896374415553e-06,-0.0221842111264164,2.23985010400218e-06,-0.000315300119346773,-2.10808035058102,0.00042552910587551,0.0221842113015204,2.10808041710817 +0.022,0.559809303604953,-0.293117220693236,0.00429165075924139,-2.37799929747595e-06,-0.0266011442285746,3.21013668078129e-06,-0.000412404152494438,-2.30885306660294,0.00055639424925008,0.026601144528559,2.30885317047524 +0.024,0.559809298033195,-0.293175040687351,0.00429165828016598,-3.30858035413328e-06,-0.0314196233928282,4.4654271010025e-06,-0.000527605584221823,-2.50962673270169,0.000711527754250484,0.0314196238843481,2.5096268890276 +0.026,0.559809290370631,-0.293242899186807,0.00429166862094979,-4.48842163436324e-06,-0.0366396511593814,6.05624769778323e-06,-0.000662421756336683,-2.71040199126618,0.000892906439758921,0.0366396519348263,2.71040221929221 +0.028,0.559809280079508,-0.293321599291989,0.00429168250515677,-5.95826737948001e-06,-0.0422612313578929,8.03705286003819e-06,-0.00081836540322122,-2.91117949030236,0.00110248076119756,0.0422612325421363,2.91117981408619 +0.03,0.559809266537562,-0.293411944112238,0.00429170076916123,-7.76188324724813e-06,-0.0482843691205908,1.04661707425735e-05,-0.000996945970221928,-3.11195988508816,0.00134217326749203,0.048284370878795,3.11196033421512 +0.032,0.559809249031975,-0.293514736768471,0.00429172436983974,-9.94605126036773e-06,-0.0547090708982456,1.34057459300063e-05,-0.00119967105693952,-3.31274383990817,0.00161387693660419,0.0547090734447881,3.31274445024951 +0.034,0.559809226753357,-0.293630780395831,0.00429175439214495,-1.25605674750062e-05,-0.0615353444802235,1.69216784889903e-05,-0.00142804802705232,-3.51353203015269,0.00191945323611136,0.0615353480888085,3.51353284466373 +0.036,0.559809198789706,-0.293760878146392,0.00429179205655369,-1.5658243368577e-05,-0.0687631990188564,2.10835588744518e-05,-0.00168358572916194,-3.71432514430919,0.00226073012046801,0.0687632040338718,3.71432621386559 +0.038,0.559809164120383,-0.293905833191907,0.00429183872638045,-1.9294910391654e-05,-0.0763926450574603,2.59645989708623e-05,-0.00196779620376119,-3.91512388604742,0.00263949989122436,0.0763926519066401,3.91512527031774 +0.04,0.559809121610064,-0.294066448726622,0.00429189591494958,-2.35294281836218e-05,-0.084423694563046,3.16415584393492e-05,-0.00228219662612438,-4.11592897643642,0.00305751677275034,0.0844237037714924,4.11593074478898 +0.042,0.55980907000267,-0.294243527970159,0.00429196529261421,-2.84236968961515e-05,-0.092856360963206,3.81946660618637e-05,-0.00261357001318796,-4.29626424078982,0.00349670714605565,0.0928563731688259,4.29626645872883 +0.044,0.559809007915276,-0.294437874170475,0.00429204869361383,-3.39837082363736e-05,-0.101608751526205,4.56283870235718e-05,-0.00288772374068546,-4.36225239731913,0.0038572361771641,0.101608767454173,4.3622550584687 +0.046,0.559808934067837,-0.294649962976264,0.0042921478061623,-3.99745918588934e-05,-0.110305370552482,5.36236107705201e-05,-0.00303774098930897,-4.25441403652595,0.00404940771105838,0.110305390830095,4.25441704817031 +0.048,0.559808848016909,-0.294879095652685,0.00429226318805691,-4.61346721936095e-05,-0.118626407672309,6.18260178678053e-05,-0.00310089159905046,-4.06006826159446,0.00412365942694531,0.118626432754683,4.06007153987494 +0.05,0.559808749529149,-0.295124468606953,0.00429239511023377,-5.23781582550952e-05,-0.12654564359886,7.01182484783013e-05,-0.00312642041422339,-3.85916540013298,0.00414641766061096,0.126545673864775,3.85916889405835 +0.052,0.559808638504276,-0.29538527822708,0.00429254366105082,-5.86403538505031e-05,-0.134063069272841,7.84116885102492e-05,-0.00312051303547056,-3.65825827682019,0.00412620355765003,0.134063105028709,3.65826193472814 +0.054,0.559808514967733,-0.295660720884044,0.00429270875698781,-6.48602103969775e-05,-0.141178676706141,8.66230627089014e-05,-0.00308498261658573,-3.4573474206427,0.00406568018140873,0.141178718179827,3.4573511875275 +0.056,0.559808379063434,-0.295949992933905,0.00429289015330166,-7.0980284316846e-05,-0.147892458955412,9.46744092358841e-05,-0.00302162195264798,-3.25643334895056,0.00396749686229491,0.147892506291978,3.25643716773224 +0.058,0.559808231046596,-0.296252290719866,0.00429308745462476,-7.69466982075694e-05,-0.154204410101943,0.000102493050158081,-0.00293220538127858,-3.05551657049305,0.00383428709008898,0.154204463361175,3.05552038319882 +0.06,0.559808071276642,-0.296566810574312,0.00429330012550229,-8.27091058419604e-05,-0.160114525237384,0.00011001155759624,-0.00281849053818117,-2.85459758819772,0.00366866630997775,0.160114584392965,2.85460133706016 +0.062,0.559807900210173,-0.296892748820816,0.00429352750085514,-8.82206603602941e-05,-0.165622800454734,0.000117167715397992,-0.00268221993920958,-2.65367690171006,0.00347323011930938,0.16562286539493,2.65368053018836 +0.064,0.559807718394,-0.297229301776131,0.00429376879636388,-9.34379855987987e-05,-0.170729232844224,0.000123904478073478,-0.00252512262488569,-2.45275500989141,0.00325055254414563,0.17072930337395,2.45275846362489 +0.066,0.55980752645823,-0.297575665752193,0.00429402311876743,-9.83211508598369e-05,-0.1754338204943,0.000130169925574575,-0.00234891571471162,-2.25183241297013,0.003003184162291,0.175433896338434,2.25183564067696 +0.068,0.559807325109397,-0.297931037058109,0.00429428947606618,-0.000102833648457645,-0.179736562496105,0.000135917214722642,-0.00215530572555966,-2.05090961432133,0.00273365064005353,0.179736643303932,2.05091256866407 +0.07,0.559807115123636,-0.298294612002177,0.00429456678762632,-0.000106942373762076,-0.183637458951585,0.000141104528134789,-0.00194598987618422,-1.84998712236123,0.00244445128548542,0.183637544302232,1.84998976081519 +0.0720000000000001,0.559806897339902,-0.298665586893915,0.00429485389417872,-0.000110617607962382,-0.18713651098555,0.000145695019864583,-0.00172265736397836,-1.64906545206331,0.00213805762244029,0.187136600394308,1.64906773785298 +0.0740000000000001,0.559806672653204,-0.299043158046119,0.00429514956770578,-0.000113833003217989,-0.190233720759839,0.00014965675862455,-0.00148699047519684,-1.44814512612404,0.00181691214851121,0.190233813685239,1.44814702935415 +0.0760000000000001,0.559806442007889,-0.299426521776954,0.00429545252121322,-0.000116565569863169,-0.192929091490046,0.000152962668458628,-0.00124066566048464,-1.24722667619106,0.00148342722133821,0.192929187341585,1.24722817543953 +0.0780000000000001,0.559806206390925,-0.299814874412079,0.00429576141837962,-0.000118795665859928,-0.195222627464603,0.000155590467509903,-0.000985354492444301,-1.04631064382077,0.00113998410451038,0.195222725610998,1.04631172881693 +0.0800000000000001,0.559805966825225,-0.300207412286813,0.00429607488308326,-0.000120506987832947,-0.197114334065329,0.00015752260487667,-0.000722724449730982,-0.845397581043793,0.000788931927599156,0.197114433843195,0.845398258088408 +0.0820000000000001,0.559805724362974,-0.300603331748341,0.00429639150879912,-0.000121686563658851,-0.198604217788778,0.000158746195220299,-0.000454439749719704,-0.644488050958319,0.000432586931553569,0.198604318511624,0.644488356353311 +0.0840000000000001,0.559805480078971,-0.301001829157968,0.00429670986786414,-0.000122324746831825,-0.199692286269162,0.000159252952602884,-0.000182162090966997,-0.443582628208844,7.32319292012783e-05,0.199692387236599,0.443582671657277 +0.0860000000000001,0.559805235063986,-0.301402100893417,0.00429702852060954,-0.000122415212022719,-0.200378548301613,0.000159039122937105,8.97599772287093e-05,-0.244625409286797,-0.00028339325504334,0.200378648808671,0.24462558990695 +0.0880000000000001,0.559804990418123,-0.301803343351174,0.00429734602435589,-0.000121965706922911,-0.200670787906309,0.000158119379582711,0.000308309683338947,-0.0847491499374186,-0.000568534682006405,0.200670887266467,0.0847516176934726 +0.0900000000000001,0.559804747201159,-0.302204784045043,0.00429766099812787,-0.000121181973289364,-0.200717544901363,0.000156764984209079,0.000408280242947256,-0.0102937339103992,-0.000698170054755364,0.200717642701287,0.0103254584401973 +0.0920000000000001,0.559804505690229,-0.30260621353078,0.00429797308429272,-0.000120332585951122,-0.200711962841951,0.000155326699363689,0.000422885892970015,0.00278007059387186,-0.000716394179221812,0.200712059015355,0.00290186932948522 +0.0940000000000001,0.559804265870815,-0.30300763189641,0.00429828230492532,-0.000119490429717484,-0.200706424618988,0.000153899407492192,0.000419284718056189,0.00275823999057456,-0.000710919849898627,0.200706519192396,0.00287907876817917 +0.0960000000000001,0.559804027728511,-0.303409039229256,0.00429858868192269,-0.000118655447078897,-0.200700929881989,0.000152483019964095,0.000415712131385248,0.00273658394556175,-0.000705489672757353,0.200701022881503,0.00285647053995914 +0.0980000000000001,0.559803791249027,-0.303810435615938,0.00429889223700518,-0.000117827581191943,-0.200695478283205,0.000151077448801162,0.00041216788315701,0.00271510106411576,-0.000700103288710229,0.200695569734508,0.00283404318370973 +0.1,0.559803556418186,-0.304211821142388,0.0042991929917179,-0.000117006775546269,-0.200690069477732,0.000149682606809254,0.000408651792960235,0.00269378994111058,-0.000694760330863242,0.200690159406099,0.00281179523650983 +0.102,0.559803323221924,-0.304613195893849,0.00429949096743241,-0.000116192974020102,-0.200684703123441,0.000148298407477709,0.000405163569361377,0.00267264921652299,-0.000689460472221014,0.200684791553741,0.00278972527236533 +0.104,0.55980309164629,-0.305014559954882,0.00429978618534781,-0.000115386121268823,-0.200679378880866,0.00014692476492037,0.000401702983376939,0.00265167754767703,-0.000684203359767321,0.200679465837573,0.00276783188456693 +0.106,0.559802861677439,-0.305415913409373,0.0043000786664921,-0.000114586162086594,-0.20067409641325,0.00014556159403864,0.000398269854595675,0.0026308735676106,-0.000678988625740783,0.200674181920447,0.00274611364654072 +0.108,0.559802633301641,-0.305817256340535,0.00430036843172396,-0.000113793041850441,-0.200668855386596,0.000144208810417406,0.000394863905461836,0.00261023594752552,-0.000673815951819645,0.20066893946798,0.00272456916640368 +0.11,0.559802406505272,-0.306218588830919,0.00430065550173377,-0.000113006706464747,-0.20066365546946,0.000142866330231361,0.000391484948625287,0.00258976334127636,-0.00066868502141687,0.200663738148352,0.00270319704917831 +0.112,0.559802181274816,-0.306619910962413,0.00430093989704489,-0.000112227102055939,-0.200658496333231,0.000141534070331739,0.000388132755102521,0.00256945444088163,-0.000663595454628018,0.200658577632576,0.00268199591475319 +0.114,0.559801957596864,-0.307021222816252,0.00430122163801509,-0.000111454175444336,-0.200653377651696,0.000140211948412849,0.000384807054276681,0.00254930799040154,-0.000658546926192438,0.200653457594077,0.00266096444037566 +0.116,0.559801735458114,-0.30742252447302,0.00430150074483854,-0.000110687873838833,-0.200648299101269,0.000138899882626969,0.000381507651858736,0.00252932264022121,-0.000653539148146031,0.200648377708904,0.00264010123383968 +0.118,0.559801514845368,-0.307823816012657,0.0043017772375456,-0.000109928144836902,-0.200643260361136,0.000137597791820265,0.000378234339681871,0.00250949709276749,-0.000648571764003118,0.200643337655891,0.00261940493383233 +0.12,0.559801295745534,-0.308225097514464,0.00430205113600582,-0.000109174936480105,-0.200638261112898,0.000136305595570957,0.000374986930395948,0.00248983011985615,-0.000643644451105145,0.200638337116289,0.00259887425689453 +0.122,0.559801078145622,-0.308626369057109,0.00430232245992788,-0.000108428197115318,-0.200633301040656,0.000135023214015845,0.000371765188078481,0.00247032045496559,-0.000638756883323983,0.200633375773855,0.00257850787496998 +0.124,0.559800862032746,-0.309027630718627,0.00430259122886188,-0.000107687875727791,-0.200628379831078,0.000133750568037661,0.000368568821296121,0.00245096682168628,-0.000633908744072817,0.200628453314918,0.00255830444491083 +0.126,0.559800647394119,-0.309428882576433,0.00430285746220004,-0.000106953921830133,-0.200623497173369,0.000132487579039553,0.000365397670454389,0.00243176797119077,-0.000629099721101539,0.200623569428352,0.00253826267009008 +0.128,0.559800434217059,-0.309830124707321,0.00430312117917804,-0.000106226285045974,-0.200618652759193,0.000131234169153255,0.00036225158983614,0.00241272269541764,-0.000624329496087931,0.200618723805493,0.00251838129341202 +0.13,0.559800222488979,-0.31023135718747,0.00430338239887665,-0.000105504915470789,-0.200613846282588,0.000129990261055202,0.00035913029494683,0.00239382978023394,-0.000619597779333291,0.200613916140055,0.00249865903909242 +0.132,0.559800012197397,-0.310632580092451,0.00430364114022226,-0.000104789763866186,-0.200609077440072,0.000128755778035922,0.000356033591497606,0.0023750879839246,-0.000614904244709835,0.200609146128241,0.00247909460888847 +0.134,0.559799803329924,-0.31103379349723,0.00430389742198879,-0.000104080781104798,-0.200604345930652,0.000127530644076362,0.00035296130601603,0.00235649609547916,-0.000610248576497773,0.200604413468744,0.00245968673954035 +0.136,0.559799595874272,-0.311434997476173,0.00430415126279857,-0.000103377918642122,-0.200599651455691,0.00012631478372993,0.000349913188701922,0.00233805293875511,-0.000605630498008713,0.200599717862621,0.00244043419992575 +0.138,0.559799389818249,-0.311836192103053,0.00430440268112371,-0.000102681128349991,-0.200594993718897,0.000125108122084327,0.000346889045266252,0.00231975735495715,-0.00060104969785979,0.200595059013278,0.00242133577491864 +0.14,0.559799185149759,-0.312237377451049,0.0043046516952869,-0.000101990362461057,-0.200590372426271,0.000123910584938491,0.00034388868141999,0.00230160815406499,-0.000596505854259798,0.200590436626418,0.00240239021689513 +0.142,0.559798981856799,-0.312638553592758,0.00430489832346347,-0.000101305573624311,-0.200585787286281,0.000122722098667288,0.000340911930629683,0.00228360414952777,-0.000591998696591877,0.200585850410217,0.00238359629823353 +0.144,0.559798779927464,-0.313039720600194,0.00430514258368157,-0.000100626714738539,-0.200581238009673,0.000121542590152124,0.000337958584728515,0.00226574420336689,-0.000587527939494014,0.200581300075131,0.00236495282823325 +0.146,0.55979857934994,-0.313440878544797,0.00430538449382407,-9.99537392853965e-05,-0.200576724309467,0.000120371986909312,0.000335028407794091,0.00224802717066485,-0.000583093279389601,0.2005767853339,0.00234645860107222 +0.148,0.559798380112507,-0.313842027497432,0.00430562407162921,-9.92866011073622e-05,-0.20057224590099,0.000119210217034565,0.00033212120553738,0.00223045190997362,-0.00057869442311037,0.200572305901568,0.00232811242280342 +0.15,0.559798182203536,-0.314243167528401,0.00430586133469221,-9.8625254463247e-05,-0.200567802501827,0.000118057209216871,0.000329236818363828,0.00221301729025347,-0.000574331096570013,0.200567861495449,0.00230991311914852 +0.152,0.559797985611489,-0.314644298707439,0.00430609630046608,-9.79696538339069e-05,-0.200563393831829,0.000116912892648285,0.000326375079739982,0.00219572222209807,-0.000570003008334984,0.200563451835121,0.00229185955040912 +0.154,0.559797790324921,-0.315045421103728,0.00430632898626281,-9.73197541442871e-05,-0.200559019612939,0.000115777197183531,0.000323535739865654,0.0021785655883455,-0.000565709873043274,0.200559076642263,0.00227395053995293 +0.156,0.559797596332473,-0.315446534785891,0.00430655940925481,-9.66755108744443e-05,-0.200554679569476,0.000114650053156112,0.000320718604451819,0.00216154624407827,-0.000561451435690526,0.200554735640932,0.00225618490004019 +0.158,0.559797403622877,-0.315847639822006,0.00430678758647543,-9.60368797264798e-05,-0.200550373427963,0.000113531391440769,0.000317923562476174,0.00214466313805395,-0.000557227393567501,0.200550428557394,0.00223856153264702 +0.16,0.559797212184954,-0.316248736279603,0.00430701353482058,-9.54038166245396e-05,-0.200546100916923,0.000112421143581842,0.000315150405771902,0.00212791521209121,-0.000553037466516359,0.200546155119919,0.00222107932489549 +0.162,0.559797022007611,-0.316649824225673,0.00430723727104976,-9.47762781033922e-05,-0.200541861767114,0.000111319241574703,0.000312398870661035,0.0021113013455587,-0.000548881411675805,0.200541915059014,0.00220373710551678 +0.164,0.559796833079842,-0.317050903726671,0.00430745881178687,-9.41542211418955e-05,-0.200537655711541,0.000110225617935139,0.000309668818365693,0.00209482048721399,-0.000544758954959534,0.20053770810744,0.0021865337796906 +0.166,0.559796645390726,-0.31745197484852,0.0043076781735215,-9.35376028299294e-05,-0.200533482485166,0.000109140205754865,0.000306960123985789,0.00207847159969243,-0.000540669813607619,0.200533533999917,0.00216946826567984 +0.168,0.55979645892943,-0.317853037656612,0.00430789537260989,-9.29263806459523e-05,-0.200529341825142,0.000108062938680709,0.000304272586293397,0.00206225361787382,-0.000536613734350432,0.200529392473362,0.00215253945169348 +0.17,0.559796273685204,-0.31825409221582,0.00430811042527622,-9.23205124847558e-05,-0.200525233470694,0.000106993750817463,0.000301605990182807,0.0020461655252102,-0.000532590453510006,0.200525283266764,0.00213574626794544 +0.172,0.55979608964738,-0.318655138590495,0.00430832334761316,-9.17199566852211e-05,-0.200521157163041,0.000105932576866668,0.000298960148303884,0.00203020627392858,-0.000528599694397946,0.200521206121114,0.00211908761543511 +0.174,0.559795906805377,-0.319056176844472,0.00430853415558369,-9.11246718915403e-05,-0.200517112645598,0.000104879352039872,0.000296334908000961,0.00201437483707267,-0.00052464121155088,0.2005171607796,0.0021025624277983 +0.176,0.559795725148693,-0.319457207041077,0.00430874286502132,-9.05346170532172e-05,-0.200513099663693,0.000103834012020465,0.000293730123557267,0.00199867022585009,-0.000520714748229733,0.200513146987326,0.00208616967339874 +0.178,0.559795544666909,-0.319858229243127,0.00430894949163177,-8.99497513973112e-05,-0.200509117964695,0.000102796493046953,0.000291145607622667,0.00198309141677399,-0.000516820065042666,0.200509164491443,0.00206990828583089 +0.18,0.559795365349687,-0.320259243512936,0.00430915405099351,-8.93700346227266e-05,-0.200505167298026,0.000101766731760294,0.000288581172847024,0.0019676374141131,-0.000512956903515882,0.200505213041158,0.00205377722053879 +0.182,0.55979518718677,-0.320660249912319,0.00430935655855881,-8.87954267059231e-05,-0.200501247415038,0.000100744665432889,0.00028603665963578,0.00195230724295281,-0.000509124986093623,0.20050129238761,0.00203777545205708 +0.184,0.55979501016798,-0.321061248502596,0.00430955702965524,-8.82258879841834e-05,-0.200497358069054,9.97302318159198e-05,0.000283511859822116,0.00193709991450075,-0.000505324082925026,0.200497402283912,0.00202190194673298 +0.186,0.559794834283218,-0.321462239344595,0.00430975547948607,-8.76613792666347e-05,-0.20049349901538,9.8723369101189e-05,0.000281006648505943,0.00192201445731177,-0.000501553958955066,0.200493542485167,0.0020061556979366 +0.188,0.559794659522463,-0.321863222498658,0.00430995192313165,-8.71018613901597e-05,-0.200489670011225,9.77240159800995e-05,0.000278520914664959,0.00190704991381851,-0.000497814368720372,0.200489712748379,0.00199053571166294 +0.19,0.559794485875773,-0.32226419802464,0.00431014637554999,-8.65472956079748e-05,-0.200485870815725,9.67321116263075e-05,0.000276054436254558,0.00189220531604528,-0.000494105065022842,0.200485912832487,0.00197504096796169 +0.192,0.559794313333281,-0.322665165981921,0.00431033885157815,-8.59976436451414e-05,-0.200482101189961,9.57475957200082e-05,0.000273606998169029,0.00187747970642471,-0.000490425815409534,0.200482142498374,0.00195967046155822 +0.194,0.559794141885198,-0.3230661264294,0.00431052936593287,-8.54528676152987e-05,-0.200478360896899,9.47704083646694e-05,0.000271178468569388,0.00186287217249226,-0.000486776368345542,0.200478401508817,0.00194442323727149 +0.196,0.55979397152181,-0.323467079425508,0.00431071793321161,-8.49129297708639e-05,-0.200474649701271,9.3800490246626e-05,0.000268768736433333,0.00184838178790558,-0.000483156481836943,0.200474689628354,0.00192929833190361 +0.198,0.559793802233479,-0.323868025028205,0.00431090456789386,-8.43777926695654e-05,-0.200470967369748,9.28377824373216e-05,0.000266377621349619,0.00183400759856678,-0.000479565950319005,0.200471006623472,0.00191429475511529 +0.2,0.55979363401064,-0.324268963294987,0.00431108928434136,-8.38474192854654e-05,-0.200467313670877,9.188222644535e-05,0.000264004956784799,0.00181974869548074,-0.00047600453093044,0.200467352262534,0.00189941155239645 +0.202,0.559793466843802,-0.324669894282889,0.00431127209679964,-8.33217728424262e-05,-0.200463688374966,9.09337643135998e-05,0.000261650603960994,0.00180560417659126,-0.000472471993820389,0.200463726315667,0.00188464778301452 +0.204,0.559793300723548,-0.325070818048487,0.00431145301939861,-8.28008168696214e-05,-0.20046009125417,8.99923384700684e-05,0.000259314368589169,0.00179157312943379,-0.000468968112607437,0.200460128554849,0.00187000248943913 +0.206,0.559793135640534,-0.325471734647905,0.00431163206615352,-8.22845153680695e-05,-0.200456522082448,8.90578918631701e-05,0.000256996104952556,0.00177765465195212,-0.000465492651369192,0.200456558753862,0.00185547472848501 +0.208,0.559792971585487,-0.325872644136817,0.00431180925096607,-8.17728324498112e-05,-0.200452980635562,8.81303678645916e-05,0.000254695695089957,0.00176384788719286,-0.000462045401938837,0.200453016688298,0.00184106361098819 +0.21,0.559792808549205,-0.326273546570448,0.00431198458762498,-8.12657325877097e-05,-0.200449466690899,8.72097102554147e-05,0.000252412965529025,0.00175015194350814,-0.000458626139669682,0.200449502135373,0.00182676820273421 +0.212,0.559792646522556,-0.32667444200358,0.00431215808980709,-8.07631805876951e-05,-0.200445980027788,8.62958633059129e-05,0.000250147749736307,0.00173656593618898,-0.000455234633843504,0.20044601487425,0.00181258757561688 +0.214,0.559792485496482,-0.327075330490559,0.0043123297710782,-8.02651415887645e-05,-0.200442520427155,8.53887717200407e-05,0.000247899922811712,0.00172308902562923,-0.000451870672824039,0.200442554685689,0.00179852085529537 +0.216,0.55979232546199,-0.327476212085289,0.00431249964489397,-7.97715808964482e-05,-0.200439087671686,8.44883806146168e-05,0.000245669345977362,0.00170972030283378,-0.00044853403977085,0.200439121352218,0.00178456709773215 +0.218,0.559792166410159,-0.327877086841246,0.00431266772460066,-7.9282464204855e-05,-0.200435681545943,8.35946355609573e-05,0.000243455838822015,0.00169645893860482,-0.000445224530853934,0.200435714658239,0.00177072543289546 +0.22,0.559792008332133,-0.328277954811473,0.00431283402343621,-7.87977575411602e-05,-0.200432301835931,8.2707482491201e-05,0.000241259276445582,0.00168330412109174,-0.000441941941375923,0.200432334389599,0.00175699501482008 +0.222,0.559791851219128,-0.328678816048589,0.00431299855453063,-7.83174270990727e-05,-0.200428948329459,8.18268677954536e-05,0.000239079506192396,0.00167025499681061,-0.000438686018934542,0.200428980333954,0.00174337494184718 +0.224,0.559791695062425,-0.329079670604791,0.00431316133090739,-7.78414395163906e-05,-0.200425620815944,8.09527384154628e-05,0.00023691638928458,0.00165731073309416,-0.000435456567506043,0.200425652280571,0.0017298643483594 +0.226,0.55979153985337,-0.329480518531853,0.00431332236548429,-7.73697615419344e-05,-0.200422319086527,8.00850415254294e-05,0.000234769786944255,0.00164447047992788,-0.000432253422291695,0.200422350020439,0.00171646236000064 +0.228,0.559791385583379,-0.329881359881137,0.0043134816710735,-7.69023603686136e-05,-0.200419042934025,7.92237247262961e-05,0.000232639546515756,0.00163173344627787,-0.000429076345634382,0.200419073346231,0.00170316813870148 +0.23,0.559791232243929,-0.330282194703589,0.0043136392603832,-7.64392033558713e-05,-0.200415792152741,7.83687361428919e-05,0.000230525556976779,0.00161909884111022,-0.00042592513023465,0.200415822052106,0.00168998085973461 +0.232,0.559791079826565,-0.330683023049748,0.00431379514601807,-7.59802581407065e-05,-0.20041256653866,7.75200242053575e-05,0.000228427637916084,0.00160656583522711,-0.000422799581803468,0.200412595933905,0.00167689965563122 +0.234,0.559790928322896,-0.331083844969744,0.00431394934048002,-7.5525492804207e-05,-0.200409365889401,7.6677537815678e-05,0.000226345678311368,0.00159413361677796,-0.000419699497378189,0.200409394789108,0.00166392368284465 +0.236,0.559790777724594,-0.331484660513305,0.00431410185616933,-7.5074875427461e-05,-0.200406190004193,7.58412262158447e-05,0.000224279553262541,0.00158180142595388,-0.000416624698282297,0.200406218416808,0.00165105215192621 +0.238,0.559790628023395,-0.331885469729761,0.00431425270538488,-7.46283745911568e-05,-0.200403038683697,7.50110390225488e-05,0.000222229047663891,0.00156956849947654,-0.000413574965073271,0.200403066617528,0.00163828424759258 +0.24,0.559790479211096,-0.33228627266804,0.00431440190032542,-7.41859592368054e-05,-0.200399911730195,7.41869263555516e-05,0.000220194126820949,0.00155743400814812,-0.000410550101727358,0.20039993919342,0.00162561912182746 +0.242,0.559790331279558,-0.332687069376681,0.0043145494530903,-7.3747598083873e-05,-0.200396808947664,7.33688386156394e-05,0.00021817467277252,0.00154539718522084,-0.000407549933037487,0.200396835948329,0.00161305598038219 +0.244,0.559790184220703,-0.333087859903831,0.00431469537567988,-7.33132605457153e-05,-0.200393730141454,7.25567266234017e-05,0.000216170456535103,0.00153345731251919,-0.000404574252571565,0.200393756687474,0.00160059405266951 +0.246,0.559790038026516,-0.333488644297247,0.0043148396799968,-7.28829162577326e-05,-0.200390675118414,7.17505416053531e-05,0.000214181360147377,0.001521613619479,-0.000401622875581416,0.20039070121758,0.00158823253845551 +0.248,0.559789892689038,-0.333889422604304,0.0043149823778463,-7.24565351051258e-05,-0.200387643686976,7.0950235121076e-05,0.000212207300342868,0.00150986533310749,-0.000398695618186609,0.200387669346953,0.00157597064008179 +0.25,0.559789748200375,-0.334290194871995,0.00431512348093728,-7.20340870563612e-05,-0.200384635657082,7.01557591326067e-05,0.000210248166099396,0.00149821171822885,-0.000395792292169756,0.200384660885412,0.00156380759127438 +0.252,0.55978960455269,-0.334690961146933,0.00431526300088284,-7.16155424407283e-05,-0.200381650840103,6.9367065952397e-05,0.000208303777005217,0.00148665203446307,-0.000392912704976052,0.200381675644209,0.00155174261036405 +0.254,0.559789461738205,-0.335091721475356,0.00431540094920109,-7.12008719483403e-05,-0.200378689048944,6.85841083127025e-05,0.000206374001221476,0.00147518555704273,-0.000390056673592317,0.200378713436127,0.00153977493962642 +0.256,0.559789319749202,-0.335492475903129,0.00431553733731609,-7.07900464358423e-05,-0.200375750097875,6.78068392580277e-05,0.000204458748542802,0.00146381157195563,-0.000387224028883248,0.200375774075321,0.00152790384075421 +0.258,0.55978917857802,-0.335893224475747,0.00431567217655812,-7.03830369541691e-05,-0.200372833802656,6.70352121971695e-05,0.000202557914885659,0.00145252933986262,-0.000384414595641646,0.200372857377436,0.00151612854777782 +0.26,0.559789038217055,-0.336293967238339,0.00431580547816487,-6.99798147762997e-05,-0.200369939980516,6.62691808754611e-05,0.000200671319838808,0.00144133813911873,-0.000381628176109039,0.200369963159587,0.00150444829578301 +0.262,0.55978889865876,-0.336694704235669,0.00431593725328162,-6.95803516748138e-05,-0.200367068450099,6.55086994927334e-05,0.000198798838502156,0.00143023728971236,-0.000378864578598481,0.200367091240308,0.00149286236873671 +0.264,0.559788759895648,-0.33709543551214,0.00431606751296285,-6.91846194222911e-05,-0.200364219031357,6.47537225610672e-05,0.000196940394547873,0.00141922606999856,-0.000376123665199457,0.200364241439439,0.00148137003084688 +0.266,0.559788621920283,-0.337496161111795,0.00431619626817187,-6.87925900966224e-05,-0.20036139154582,6.40042048319355e-05,0.00019509585613697,0.00140830377567957,-0.000373405254633363,0.200361413578403,0.00146997054455624 +0.268,0.559788484725287,-0.337896881078323,0.00431632352978217,-6.84042359977432e-05,-0.200358585816254,6.32601015425338e-05,0.000193265091430469,0.00139746975796884,-0.000370709154345894,0.200358607479862,0.00145866322265249 +0.27,0.559788348303339,-0.33829759545506,0.00431644930857804,-6.80195297309005e-05,-0.200355801666788,6.2521368214552e-05,0.000191447968589386,0.00138672329175193,-0.000368035218620277,0.200355822967837,0.00144744731673011 +0.272,0.559788212647168,-0.33869830428499,0.00431657361525503,-6.76384441233857e-05,-0.200353038923087,6.17879606680526e-05,0.000189644383530307,0.00137606371089505,-0.000365383272249442,0.200353059867892,0.00143632213109962 +0.274,0.559788077749562,-0.339099007610752,0.00431669646042071,-6.72609521967792e-05,-0.200350297411944,6.10598351255542e-05,0.00018785423910872,0.00136549036314215,-0.00036275311140338,0.200350318006719,0.00142528697700429 +0.276,0.55978794360336,-0.339499705474638,0.00431681785459553,-6.68870271669508e-05,-0.200347576961635,6.03369482224391e-05,0.00018607740348564,0.00135500255113441,-0.000360144592967404,0.200347597212494,0.00141434113335941 +0.278,0.559787810201454,-0.339900397918598,0.0043169378082136,-6.65166425828367e-05,-0.20034487740174,5.96192567536846e-05,0.000184313744822084,0.0013445996156769,-0.000357557602449765,0.2003448973147,0.00140348392298538 +0.28,0.559787677536789,-0.340301084984245,0.00431705633162255,-6.61497721876624e-05,-0.200342198563172,5.89067178126401e-05,0.000182563172912431,0.00133428091492194,-0.000354991931683646,0.200342218144152,0.00139271466696853 +0.282,0.559787545602365,-0.340701766712851,0.00431717343508485,-6.5786389891187e-05,-0.20033954027808,5.819928902695e-05,0.000180825569795484,0.00132404581049128,-0.000352447373369591,0.200339559532906,0.0013820326862873 +0.284,0.55978741439123,-0.341102443145357,0.00431728912877866,-6.54264699084805e-05,-0.20033690237993,5.74969283191617e-05,0.000179100803632259,0.00131389364319001,-0.000349923818220019,0.200336921314333,0.00137143730517823 +0.286,0.559787283896485,-0.341503114322371,0.00431740342279813,-6.50699866766579e-05,-0.200334284703507,5.67995937540699e-05,0.000177388763400454,0.00130382377117044,-0.000347421126589696,0.200334303323127,0.00136092785953213 +0.288,0.559787154111283,-0.341903780284171,0.00431751632715367,-6.47169148548787e-05,-0.200331687084845,5.61072438128029e-05,0.000175689351955555,0.00129383556299323,-0.000344939112863207,0.200331705395232,0.00135050368531131 +0.29,0.559787025028826,-0.34230444107071,0.00431762785177338,-6.43672292688357e-05,-0.200329109361255,5.54198373026171e-05,0.000174002506847515,0.00128392839415792,-0.000342477620915438,0.20032912736787,0.00134016413716457 +0.292,0.559786896642366,-0.342705096721616,0.00431773800650288,-6.40209048274886e-05,-0.200326551371269,5.47373333291412e-05,0.000172328082359563,0.00127410162628627,-0.000340036491151829,0.200326569079484,0.00132990854474655 +0.294,0.559786768945207,-0.343105747276195,0.0043178468011067,-6.36779169393975e-05,-0.20032401295475,5.40596913380098e-05,0.000170665891141564,0.00126435466610286,-0.000337615570916716,0.200324030369854,0.00131973627737539 +0.296,0.559786641930698,-0.343506392773435,0.00431795424526824,-6.33382412629224e-05,-0.200321493952604,5.33868710454743e-05,0.000169015898499047,0.00125468691686281,-0.000335214715360687,0.200321511079799,0.00130964672285563 +0.298,0.559786515592241,-0.343907033252006,0.00431806034859088,-6.30018533454013e-05,-0.200318994207082,5.2718832476567e-05,0.000167378041981969,0.00124509772977954,-0.00033283379264476,0.200319011051487,0.00129963921884736 +0.3,0.559786389923285,-0.344307668750263,0.00431816512059814,-6.26687290949945e-05,-0.200316513561685,5.20555358748953e-05,0.000165752175873557,0.00123558651504707,-0.000330472633633394,0.200316530128338,0.0012897131392498 +0.302,0.559786264917325,-0.344708299306252,0.00431826857073438,-6.2338844641907e-05,-0.200314051861022,5.13969419420334e-05,0.000164138168334826,0.00122615268285941,-0.000328131064854242,0.200314068154879,0.0012798678586909 +0.304,0.559786140567906,-0.345108924957708,0.00431837070836591,-6.20121764216552e-05,-0.200311608950954,5.07430116154783e-05,0.000162535929160157,0.00121679564341059,-0.000325808979621811,0.200311624976892,0.00127010277430339 +0.306,0.559786016868619,-0.345509545742056,0.00431847154278084,-6.16887009252664e-05,-0.200309184678449,5.00937060235462e-05,0.000160945382021715,0.00120751481036407,-0.000323506223545709,0.200309200441268,0.00126041727610958 +0.308,0.559785893813103,-0.345910161696421,0.00431857108319,-6.13683948935683e-05,-0.200306778891712,4.94489867212955e-05,0.000159366388141624,0.0011983096112611,-0.000321222626623039,0.200306794396135,0.0012508107554561 +0.31,0.55978577139504,-0.346310772857623,0.00431866933872772,-6.10512353726999e-05,-0.200304391440004,4.8808815517054e-05,0.000157798878130943,0.00118917947364294,-0.00031895807349469,0.200304406690677,0.00124128262660868 +0.312,0.559785649608161,-0.346711379262181,0.00431876631845207,-6.07371993810446e-05,-0.200302022173818,4.81731544273167e-05,0.000156242740967372,0.00118012379035637,-0.000316712425382781,0.200302037175314,0.00123183225930397 +0.314,0.559785528446242,-0.347111980946318,0.00431886203134543,-6.04262644088304e-05,-0.200299670944842,4.75419658155229e-05,0.000154697879506393,0.00117114200975932,-0.000314485521825391,0.200299685701661,0.00122245907268287 +0.316,0.559785407903103,-0.347512577945961,0.00431895648631533,-6.0118407863019e-05,-0.200297337605779,4.69152123400151e-05,0.000153164196603495,0.00116223360449585,-0.000312277230116176,0.200297352122347,0.00121316251631414 +0.318,0.559785287972611,-0.347913170296742,0.00431904969219479,-5.98136076224164e-05,-0.200295022010424,4.62928568950582e-05,0.000151641546541903,0.00115339800557668,-0.000310087420150873,0.200295036291099,0.00120394199446722 +0.32,0.559785168648673,-0.348313758034002,0.00431914165774291,-5.95118416768514e-05,-0.200292724013756,4.56748626594116e-05,0.000150129908504937,0.00114463462666525,-0.000307915954018967,0.200292738062824,0.00119479690856991 +0.322,0.559785049925244,-0.348714341192797,0.00431923239164543,-5.92130879883967e-05,-0.200290443471918,4.50611930789823e-05,0.000148629178409187,0.0011359429299973,-0.000305762704218282,0.200290457293597,0.00118572669877013 +0.324,0.559784931796321,-0.34911491980789,0.00431932190251523,-5.89173249632146e-05,-0.200288180242036,4.44518118425385e-05,0.000147139217476777,0.00112732241597246,-0.000303627509419535,0.200288193840479,0.00117673082875821 +0.326,0.559784814255944,-0.349515493913765,0.0043194101988928,-5.86245311184896e-05,-0.200285934182254,4.38466830413042e-05,0.000145659928563191,0.00111877253988757,-0.000301510240385827,0.200285947561543,0.00116780873256413 +0.328,0.559784697298197,-0.349916063544619,0.0043194972892474,-5.83346852489618e-05,-0.200283705151877,4.32457708809952e-05,0.000144191235340596,0.00111029272928387,-0.000299410805176814,0.200283718316033,0.00115895982989329 +0.33,0.559784580917203,-0.350316628734372,0.00431958318197632,-5.80477661771272e-05,-0.200281493011337,4.26490398205969e-05,0.000142733075358947,0.00110188248109155,-0.000297329047667383,0.200281505964315,0.00115018359209506 +0.332,0.559784465107132,-0.350717189516664,0.00431966788540668,-5.77637529475261e-05,-0.200279297621952,4.20564546903257e-05,0.000141285330657048,0.00109354128183248,-0.000295264823008121,0.200279310367644,0.00114147947657271 +0.334,0.559784349862191,-0.35111774592486,0.00431975140779509,-5.7482624854499e-05,-0.200277118846209,4.14679805285645e-05,0.000139847883273701,0.00108526859374236,-0.000293218014105199,0.200277131388444,0.00113284692470583 +0.336,0.559784235176632,-0.351518297992049,0.00431983375732879,-5.72043614144313e-05,-0.200274956547577,4.08835826339049e-05,0.000138420670758864,0.00107706393803752,-0.000291188509068951,0.200274968890124,0.00112428544263814 +0.338,0.559784121044746,-0.35191884575105,0.00431991494212562,-5.69289421714636e-05,-0.200272810590457,4.03032264922886e-05,0.000137003630662491,0.0010689267700148,-0.000289176182999284,0.200272822737024,0.00111579447000019 +0.34,0.559784007460864,-0.352319389234411,0.00431999497023476,-5.66563468917813e-05,-0.200270680840497,3.97268779019077e-05,0.000135596617267808,0.00106085656578769,-0.000287180884975257,0.200270692794734,0.00110737344942187 +0.342,0.559783894419358,-0.352719928474412,0.00432007384963723,-5.63865557023923e-05,-0.200268567164194,3.91545029523876e-05,0.000134199568124771,0.00105285287779755,-0.000285202501372483,0.200268578929691,0.00109902191659702 +0.344,0.559783781914641,-0.353120463503068,0.00432015158824657,-5.61195486192822e-05,-0.200266469428986,3.85860678964178e-05,0.000132812413844441,0.00104491520991345,-0.000283240929842276,0.200266481009278,0.00109073936277994 +0.346,0.559783669941164,-0.353520994352128,0.00432022819390881,-5.58553060470146e-05,-0.200264387503354,3.80215392330185e-05,0.000131435022587833,0.00103704304171837,-0.000281296036810929,0.200264398901917,0.00108252524028989 +0.348,0.559783558493417,-0.353921521053081,0.0043203036744035,-5.55938085289309e-05,-0.200262321256819,3.74608837491741e-05,0.000130067345782692,0.00102923587361192,-0.000279367689572096,0.200262332477076,0.0010743790317879 +0.35,0.55978344756593,-0.354322043637156,0.00432037803744381,-5.53350366638838e-05,-0.20026027055986,3.69040684747301e-05,0.000128709293223395,0.00102149326150491,-0.000277455780572921,0.200260281605177,0.00106630027461093 +0.352,0.55978333715327,-0.354722562135321,0.0043204512906774,-5.50789713560373e-05,-0.200258235283773,3.63510606268824e-05,0.000127360760826534,0.00101381473008311,-0.000275560203127911,0.200258246157465,0.001058288474793 +0.354,0.559783227250044,-0.355123076578291,0.00432052344168632,-5.48255936205777e-05,-0.200256215300939,3.58018276622185e-05,0.00012602167920317,0.0010061997693378,-0.000273680828000167,0.200256226006266,0.00105034310349499 +0.356,0.559783117850896,-0.355523586996525,0.00432059449798805,-5.45748846392246e-05,-0.200254210484696,3.52563373148817e-05,0.000124691965086576,0.000998647924771444,-0.000271817539830576,0.200254221024867,0.0010424636870256 +0.358,0.559783008950506,-0.355924093420229,0.00432066446703558,-5.43268257602314e-05,-0.20025222070924,3.47145575028962e-05,0.00012337154214892,0.000991158755764276,-0.000269970220657943,0.200252231087413,0.00103464976517781 +0.36,0.559782900543593,-0.356324595879361,0.00432073335621806,-5.40813984706289e-05,-0.200250245849673,3.417645643225e-05,0.00012206032018458,0.000983731790471508,-0.000268138773337754,0.200250256068955,0.00102690085164777 +0.362,0.559782792624912,-0.356725094403628,0.00432080117286131,-5.3838584479493e-05,-0.200248285782078,3.36420024095452e-05,0.000120758188171255,0.00097636655357891,-0.000266323083378261,0.200248295845529,0.00101921644987611 +0.364,0.559782685189255,-0.35712558902249,0.0043208679242277,-5.35983657179439e-05,-0.200246340383459,3.31111640987369e-05,0.000119465111414474,0.000969062600997272,-0.000264523009399497,0.200246350294086,0.00101159609528637 +0.366,0.559782578231449,-0.357526079765162,0.0043209336175177,-5.33607240338351e-05,-0.200244409531674,3.25839103719472e-05,0.000118180999708617,0.000961819485167935,-0.000262738469002099,0.200244419292441,0.00100403932882851 +0.368,0.559782471746359,-0.357926566660617,0.00432099825986919,-5.31256417191094e-05,-0.200242493105518,3.20602102227285e-05,0.000116905748970275,0.000954636765471139,-0.000260969360704743,0.200242502719338,0.000996545691555656 +0.37,0.559782365728882,-0.358327049737584,0.00432106185835859,-5.2893101037954e-05,-0.200240590984613,3.15400329291282e-05,0.000115639331443873,0.000947514015164907,-0.000259215560474701,0.200240600454355,0.000989114740918829 +0.372,0.559782260173954,-0.358727529024555,0.0043211244200009,-5.2663084393334e-05,-0.200238703049457,3.10233479808297e-05,0.00011438162222932,0.000940450779751688,-0.000257476964228564,0.200238712377945,0.000981746001645908 +0.374,0.559782155076544,-0.359128004549782,0.00432118595175052,-5.24355745490368e-05,-0.200236829181494,3.05101250722139e-05,0.000113132503365421,0.000933446632489509,-0.000255753471352371,0.200236838371504,0.000974439026893176 +0.376,0.559782050431656,-0.359528476341281,0.00432124646050119,-5.22105543798723e-05,-0.200234969262927,3.00003340954202e-05,0.000111891974852174,0.000926501170922522,-0.000254044974293266,0.200234978317195,0.000967193405023416 +0.378,0.559781946234327,-0.359928944426834,0.0043213059530869,-5.19880066496281e-05,-0.20023312317681,2.94939451750409e-05,0.000110659995056217,0.000919613930144835,-0.000252351358559499,0.200233132098025,0.000960008657871947 +0.38,0.55978184247963,-0.360329408833988,0.00432136443628189,-5.17679143996474e-05,-0.200231290807207,2.89909286611822e-05,0.00010943640438299,0.000912784483414474,-0.000250672540016982,0.200231299598018,0.000952884338251538 +0.382,0.559781739162669,-0.360729869590062,0.00432142191680154,-5.15502610320961e-05,-0.200229472038876,2.84912550149729e-05,0.000108221133443553,0.00090601247684785,-0.000249008417184392,0.200229480701889,0.000945820074726932 +0.384,0.559781636278585,-0.361130326722144,0.00432147840130195,-5.13350298658732e-05,-0.200227666757299,2.79948949924447e-05,0.00010701413366565,0.000899297473294646,-0.000247358872967895,0.200227675295079,0.000938815414399599 +0.386,0.55978153382255,-0.361530780257092,0.00432153389638151,-5.11222044974335e-05,-0.200225874848983,2.75018195231014e-05,0.000105815300965872,0.000892639028665653,-0.00024572381282506,0.200225883264054,0.000931869897359575 +0.388,0.559781431789767,-0.36193123022154,0.00432158840858005,-5.09117686620097e-05,-0.200224096201185,2.70119997411444e-05,0.000104624621466431,0.000886036757852259,-0.00024410314308082,0.200224104496031,0.000924983130734973 +0.39,0.559781330175475,-0.362331676641896,0.00432164194438048,-5.07037060115678e-05,-0.200222330701952,2.65254069507781e-05,0.000103441998022813,0.000879490275745853,-0.000242496770060108,0.200222338879019,0.000918154712222321 +0.392,0.559781228974943,-0.362732119544347,0.00432169451020785,-5.04980006699184e-05,-0.200220578240082,2.6042012660904e-05,0.000102267326551608,0.000872999204176716,-0.000240904574934364,0.200220586301776,0.000911384238835355 +0.394,0.559781128183473,-0.363132558954857,0.00432174611243112,-5.02946367053613e-05,-0.200218838705135,2.55617886510406e-05,0.000101100572358348,0.000866563112933426,-0.000239326491784097,0.200218846653826,0.000904671279580456 +0.396,0.559781027796397,-0.363532994899168,0.00432179675736245,-5.00935983804851e-05,-0.20021711198763,2.50847066937676e-05,9.99416313596234e-05,0.000860181609968479,-0.000237762428668963,0.200217119825649,0.000898015425420606 +0.398,0.559780927809079,-0.363933427402807,0.0043218464512579,-4.98948701799228e-05,-0.200215397978695,2.46107389363648e-05,9.87904757998591e-05,0.000853854337928838,-0.000236212258086787,0.200215405708337,0.000891416299704642 +0.4,0.559780828216916,-0.364333856491083,0.0043218952003182,-4.96984364772856e-05,-0.200213696570278,2.41398576614204e-05,9.76470085345387e-05,0.00084758090129755,-0.00023467590110765,0.200213704193801,0.000884873494393859 +0.402,0.559780729015333,-0.364734282189088,0.00432194301068854,-4.95042821457847e-05,-0.20021200765509,2.36720353319342e-05,9.65111462969386e-05,0.000841360935782686,-0.00023315327186274,0.200212015174717,0.000878386631160201 +0.404,0.559780630199787,-0.365134704521703,0.00432198988845953,-4.93123918920979e-05,-0.200210331126535,2.32072445739695e-05,9.53828543925893e-05,0.000835194052806187,-0.000231644292289501,0.200210338544454,0.000871955315887922 +0.406,0.559780531765766,-0.365535123513594,0.00432203583966684,-4.91227507282143e-05,-0.200208666878879,2.27454581627762e-05,9.42620564936546e-05,0.000829079881137228,-0.000230148842692013,0.200208674197244,0.000865579155475342 +0.408,0.559780433708784,-0.365935539189219,0.00432208087029218,-4.89353436661232e-05,-0.20020701480701,2.22866492032014e-05,9.3148690150091e-05,0.000823018077300563,-0.000228666841538272,0.200207022027943,0.000859257795111154 +0.41,0.559780336024391,-0.366335951572822,0.00432212498626365,-4.8750155967614e-05,-0.200205374806569,2.18307907966231e-05,9.2042644339596e-05,0.000817008249248684,-0.000227198238521298,0.200205381932155,0.000852990836588885 +0.412,0.559780238708161,-0.366736360688445,0.00432216819345536,-4.85671730887649e-05,-0.200203746774013,2.13778562491162e-05,9.0943891306594e-05,0.000811050046567451,-0.000225742901802105,0.200203753806307,0.000846777908968512 +0.414,0.559780141755699,-0.367136766559919,0.00432221049768865,-4.83863804023876e-05,-0.200202130606383,2.09278191894146e-05,8.98523894177216e-05,0.000805143136189956,-0.000224300742909799,0.200202137547407,0.000840618667951027 +0.416,0.559780045162639,-0.36753716921087,0.00432225190473212,-4.8207763531094e-05,-0.200200526201469,2.0480653277477e-05,8.87680554062519e-05,0.00079928713994648,-0.000222871720211015,0.200200533053213,0.000834512734123481 +0.418,0.559779948924644,-0.367937568664724,0.00432229242030176,-4.80313081807626e-05,-0.200198933457823,2.00363323085706e-05,8.76907921276703e-05,0.000793481707422882,-0.000221455724418173,0.200198940222248,0.000828459735241423 +0.42,0.559779853037406,-0.368337964944702,0.00432233205006136,-4.78570003625833e-05,-0.200197352274639,1.95948303798043e-05,8.66205440708256e-05,0.000787726509021702,-0.000220052642774249,0.200197358953673,0.000822459322558006 +0.422,0.559779757496643,-0.368738358073823,0.00432237079962328,-4.76848260044793e-05,-0.200195782551787,1.91561217374736e-05,8.55572696023542e-05,0.000782021215145478,-0.000218662425839622,0.20019578914733,0.000816511165749427 +0.424,0.559779662298102,-0.369138748074909,0.00432240867454831,-4.75147712841739e-05,-0.200194224189778,1.87201806764459e-05,8.45008993333173e-05,0.000776365475380069,-0.000217285012031609,0.200194230703699,0.000810614908451502 +0.426,0.559779567437558,-0.369539134970582,0.00432244568034598,-4.7346822407146e-05,-0.200192677089886,1.82869816893472e-05,8.34513985692453e-05,0.000770758928902992,-0.000215920288593185,0.200192683524025,0.000804770174293888 +0.428,0.559779472910813,-0.369939518783268,0.00432248182247506,-4.71809656898969e-05,-0.200191141154063,1.78564995220731e-05,8.24086424100478e-05,0.000765201273872362,-0.000214568159247197,0.200191147510233,0.000798976638483556 +0.43,0.559779378713695,-0.370339899535198,0.00432251710634407,-4.70171878375058e-05,-0.20018961628479,1.74287090523584e-05,8.13726169779371e-05,0.000759692180690718,-0.000213228579758195,0.200189622564776,0.000793233975206656 +0.432,0.559779284842061,-0.370740277248408,0.00432255153731127,-4.68554752219851e-05,-0.20018810238534,1.70035852030403e-05,8.03432737006559e-05,0.0007542313197606,-0.000211901476400432,0.200188108590897,0.000787541847165875 +0.434,0.559779191291794,-0.37114065194474,0.00432258512068488,-4.66958147427032e-05,-0.200186599359511,1.65811031467566e-05,7.932048073922e-05,0.000748818389240124,-0.00021058674942731,0.200186605492369,0.000781899928228182 +0.436,0.559779098058802,-0.371541023645846,0.00432261786172386,-4.65381932990283e-05,-0.200185107111783,1.61612382053311e-05,7.83042519714171e-05,0.000743453038715147,-0.000209284314704738,0.200185113173645,0.000776307864853255 +0.438,0.559779005139021,-0.371941392373187,0.0043226497656377,-4.63825977348175e-05,-0.200183625547356,1.57439658879377e-05,7.72944902527328e-05,0.000738134966343784,-0.000207994117588928,0.200183631539899,0.000770765346726463 +0.44,0.559778912528411,-0.372341758148035,0.00432268083758741,-4.62290153380173e-05,-0.200182154571918,1.53292617349754e-05,7.62911539498034e-05,0.000732863866814703,-0.000206716086956215,0.200182160496792,0.000765272061465778 +0.442,0.55977882022296,-0.372742120991474,0.00432271108268464,-4.60774331190183e-05,-0.200180694091889,1.49171015401128e-05,7.52942153070534e-05,0.000727639448694361,-0.00020545011698847,0.20018069995072,0.000759827701999285 +0.444,0.559778728218679,-0.373142480924403,0.00432274050599357,-4.59278384767891e-05,-0.200179244014123,1.45074612670215e-05,7.43035424854987e-05,0.000722461399732532,-0.000204196168654413,0.200179249808512,0.000754431949099801 +0.446,0.559778636511606,-0.373542837967531,0.00432276911252971,-4.57802189490763e-05,-0.20017780424629,1.41003168654952e-05,7.33191007906697e-05,0.000717329383392861,-0.000202954170830382,0.200177809977813,0.000749084461377342 +0.448,0.559778545097803,-0.373943192141388,0.00432279690726103,-4.56345620736264e-05,-0.200176374696589,1.36956445837e-05,7.23408555280968e-05,0.000712243142936275,-0.000201724023769778,0.200176380366799,0.000743784966105224 +0.45,0.559778453973357,-0.374343543466317,0.00432282389510805,-4.54908555269639e-05,-0.200174955273718,1.32934207704161e-05,7.13687511866289e-05,0.000707202369581993,-0.000200505673696172,0.200174960884143,0.000738533151167015 +0.452,0.559778363134381,-0.374743891962483,0.00432285008094412,-4.53490870688799e-05,-0.200173545887111,1.28936218889153e-05,7.04027253162209e-05,0.000702206778835367,-0.000199299051220625,0.200173551439256,0.000733328722873811 +0.454,0.559778272577009,-0.375144237649866,0.0043228754695956,-4.5209244625699e-05,-0.200172146446603,1.24962245655336e-05,6.94427432224031e-05,0.000697256089671194,-0.000198104087821559,0.200172151941952,0.000728171393822518 +0.456,0.559778182297402,-0.375544580548269,0.00432290006584238,-4.50713160959903e-05,-0.200170756862752,1.2101205537629e-05,6.84887285773427e-05,0.000692349993308693,-0.000196920721916293,0.200170762302764,0.000723060847990487 +0.458,0.559778092291745,-0.375944920677317,0.00432292387441775,-4.49352897113897e-05,-0.20016937704663,1.17085416778684e-05,6.75405772976311e-05,0.000687488229539345,-0.000195748862431844,0.200169382432744,0.000717996805341223 +0.46,0.559778002556244,-0.376345258056456,0.00432294690000909,-4.48011537867998e-05,-0.200168006909834,1.13182100879017e-05,6.65983101999501e-05,0.000682670513868499,-0.000194588455591782,0.200168012243467,0.000712978984596716 +0.462,0.55977791308713,-0.376745592704956,0.0043229691472581,-4.46688964705899e-05,-0.200166646364574,1.09301878555013e-05,6.56618648342543e-05,0.000677896579148739,-0.000193439468436363,0.200166651647121,0.000708007118935973 +0.464,0.559777823880658,-0.377145924641914,0.00432299062076051,-4.45385063274628e-05,-0.200165295323518,1.05444522141562e-05,6.47311301782416e-05,0.000673166147824311,-0.000192301787341197,0.200165300556353,0.000703080905163405 +0.466,0.559777734933104,-0.37754625388625,0.00432301132506696,-4.44099719498769e-05,-0.200163953699983,1.01609807061365e-05,6.38061270485934e-05,0.000668478942339456,-0.00019117535852986,0.200163958884461,0.00069820006868321 +0.468,0.55977764624077,-0.377946580456714,0.00432303126468334,-4.42832818192684e-05,-0.200162621407748,9.77975078003678e-06,6.28867791174769e-05,0.000663834719832889,-0.00019006014730788,0.200162626545202,0.000693364364413166 +0.47,0.559777557799977,-0.378346904371881,0.00432305044407008,-4.4158424833407e-05,-0.200161298361104,9.40074011690494e-06,6.19729892403775e-05,0.000659233199279407,-0.000188956073877977,0.200161303452848,0.000688573496645732 +0.472,0.559777469607071,-0.378747225650158,0.0043230688676438,-4.40353898623069e-05,-0.200159984474951,9.02392648452487e-06,6.10647574172951e-05,0.000654674123939935,-0.000187863074055383,0.200159989522281,0.000683827206060488 +0.474,0.559777381658418,-0.379147544309781,0.00432308653977602,-4.39141658037378e-05,-0.200158679664608,8.64928782068341e-06,6.01620281370784e-05,0.000650157237075399,-0.000186781072379626,0.200158684668798,0.000679125225234909 +0.476,0.559777293950407,-0.379547860368817,0.00432310346479509,-4.37947417497586e-05,-0.200157383846003,8.27680219500637e-06,5.92647389496825e-05,0.000645682299293959,-0.000185710030686791,0.20015738880831,0.00067446731307158 +0.478,0.559777206479451,-0.379948173845165,0.0043231196469848,-4.36771068479391e-05,-0.200156096935411,7.90644769793624e-06,5.8372834343956e-05,0.000641249074673222,-0.000184649890863642,0.200156101857073,0.000669853227024074 +0.48,0.55977711924198,-0.380348484756558,0.00432313509058588,-4.35612504123827e-05,-0.200154818849704,7.5382026315518e-06,5.74862518698537e-05,0.000636857264840751,-0.000183600593062216,0.200154823731942,0.000665282663765774 +0.482,0.559777032234449,-0.380748793120564,0.00432314979979533,-4.34471618404597e-05,-0.200153549506352,7.17204532568737e-06,5.6604977649588e-05,0.000632506651221387,-0.0001825620696283,0.200153554350368,0.00066075539846525 +0.484,0.559776945453333,-0.381149098954584,0.00432316377876718,-4.33348305017844e-05,-0.200152288823099,6.8079543530386e-06,5.57289700497953e-05,0.000628197032587207,-0.000181534262448657,0.200152293630078,0.000656271223121174 +0.486,0.559776858895127,-0.381549402275856,0.00432317703161274,-4.32242459602605e-05,-0.200151036718221,6.44590827589275e-06,5.48581249870672e-05,0.000623928134851903,-0.00018051713856354,0.200151041489331,0.000651829861736971 +0.488,0.559776772556349,-0.381949703101457,0.00432318956240028,-4.31153980018361e-05,-0.20014979311056,6.08588579878444e-06,5.39924147058279e-05,0.000619699708215293,-0.000179510629451373,0.200149797846951,0.000647431058332788 +0.49,0.559776686433535,-0.382350001448298,0.00432320137515593,-4.30082763014372e-05,-0.200148557919388,5.72786575808725e-06,5.31318392060775e-05,0.000615511541041113,-0.000178514665723217,0.200148562622196,0.000643074595494565 +0.492,0.559776600523244,-0.382750297333134,0.00432321247386332,-4.29028706450118e-05,-0.200147331064396,5.37182713589157e-06,5.22763013433014e-05,0.000611363414754206,-0.000177529196204728,0.200147335734737,0.000638760246169998 +0.494,0.559776514822052,-0.383150590772556,0.00432322286246448,-4.2799171096064e-05,-0.200146112465729,5.01774897326834e-06,5.14257386674543e-05,0.000607255107309967,-0.000176554196609779,0.200146117104707,0.000634487790498806 +0.496,0.559776429326559,-0.383550881782997,0.00432323254485921,-4.2697167690342e-05,-0.200144902043966,4.66561034945245e-06,5.05801581172812e-05,0.000603186375153219,-0.000175589591477883,0.200144906652667,0.000630256979494094 +0.498,0.559776344033382,-0.383951170380732,0.00432324152490587,-4.25968504635948e-05,-0.200143699720229,4.31539060735681e-06,4.97394903041435e-05,0.000599157004566039,-0.000174635327900007,0.200143704299724,0.000626067592810079 +0.5,0.559776258939157,-0.384351456581878,0.00432324980642164,-4.24982097291254e-05,-0.200142505415948,3.96706903785243e-06,4.89036658392448e-05,0.000595166781136588,-0.000173691378987945,0.200142509967294,0.000621919416834478 +0.502,0.559776174040543,-0.384751740402396,0.00432325739318203,-4.24012358002379e-05,-0.200141319053104,3.62062509140504e-06,4.80726708442044e-05,0.000591215483514151,-0.00017275765540338,0.200141323577342,0.000617812218339391 +0.504,0.559776089334214,-0.38515202185809,0.004323264288922,-4.23059190457486e-05,-0.200140140554014,3.27603841623891e-06,4.72464567469188e-05,0.000587302938920286,-0.000171834141533822,0.20014014505217,0.000613745828447094 +0.506,0.559776004816867,-0.385552300964612,0.00432327049733569,-4.22122499732502e-05,-0.200138969841348,2.93328852526975e-06,4.6424974975423e-05,0.000583428923228712,-0.000170920798348017,0.200138974314436,0.000609720022690092 +0.508,0.559775920485214,-0.385952577737456,0.00432327602207611,-4.21202191458469e-05,-0.200137806838321,2.59235522284684e-06,4.56081422628429e-05,0.000579593199823147,-0.000170017520027821,0.200137811287339,0.000605734543375489 +0.51,0.55977583633599,-0.386352852191965,0.00432328086675658,-4.20298174041988e-05,-0.200136651468549,2.25321844515846e-06,4.47959586091767e-05,0.000575795581353455,-0.000169124308307955,0.200136655894481,0.000601789216482418 +0.512,0.559775752365945,-0.38675312434333,0.00432328503494989,-4.19410353114102e-05,-0.200135503655996,1.91585798961502e-06,4.39884101366399e-05,0.000572035890877842,-0.000168241134565483,0.200135508059814,0.00059788386835458 +0.514,0.559775668571849,-0.387153394206589,0.00432328853018854,-4.18538637636523e-05,-0.200134363324986,1.58025390689653e-06,4.31853650062485e-05,0.000568313927168384,-0.000167367905125337,0.200134367707649,0.000594018275165266 +0.516,0.55977558495049,-0.38755366179663,0.00432329135596551,-4.17682938513852e-05,-0.200133230400287,1.24638636911367e-06,4.23868370957869e-05,0.000564629488997159,-0.000166504594834027,0.200133234762741,0.000590192243175917 +0.518,0.559775501498673,-0.38795392712819,0.00432329351573402,-4.16843164152691e-05,-0.20013210480703,9.14235527560424e-07,4.15927917107872e-05,0.000560982399422371,-0.00016565117246653,0.200132109150206,0.000586405596599754 +0.52,0.559775418213224,-0.388354190215858,0.00432329501290762,-4.16019226845421e-05,-0.200130986470689,5.83781679247552e-07,4.08031455845225e-05,0.000557372464154992,-0.000164807571235992,0.200130990795509,0.000582658129630494 +0.522,0.559775335090983,-0.388754451074073,0.00432329585086073,-4.1521103832931e-05,-0.200129875317173,2.55005242616457e-07,4.00178848392052e-05,0.000553799492375439,-0.000163973757315305,0.200129879624544,0.000578949654072266 +0.524,0.559775252128809,-0.389154709717127,0.00432329603292859,-4.14418511451852e-05,-0.20012877127272,-7.21133500136694e-08,3.92369678414716e-05,0.000550263303672471,-0.000163149712489874,0.200128775563538,0.000575279994176282 +0.526,0.559775169323578,-0.389554966159164,0.00432329556240733,-4.13641559615651e-05,-0.200127674263958,-3.97593607343038e-07,3.84603390801707e-05,0.000546763728043187,-0.000162335370840205,0.200127678539108,0.000571648969746008 +0.528,0.559775086672185,-0.389955220414183,0.00432329444255417,-4.12880097888646e-05,-0.200126584217808,-7.2145483337449e-07,3.76879707997268e-05,0.000543300619770814,-0.00016153068292668,0.200126588478162,0.000568056430395002 +0.53,0.559775004171539,-0.390355472496035,0.004323292676588,-4.12134040783662e-05,-0.200125501061479,-1.04371633904976e-06,3.69198144278826e-05,0.000539873760280196,-0.000160735640943044,0.2001255053079,0.000564502166520057 +0.532,0.559774921818568,-0.390755722418428,0.00432329026768881,-4.1140330531153e-05,-0.200124424722767,-1.36439739714666e-06,3.61558144534868e-05,0.000536482972629537,-0.000159950194582314,0.200124428956104,0.000560985995892179 +0.534,0.559774839610217,-0.391155970194926,0.00432328721899841,-4.10687808205523e-05,-0.200123355129589,-1.68351711737902e-06,3.53959639376455e-05,0.000533128156204876,-0.000159174296139597,0.200123359350683,0.00055750781317666 +0.536,0.559774757543445,-0.391556215838947,0.00432328353362034,-4.09987466754025e-05,-0.200122292210142,-2.00109458170505e-06,3.46401796136319e-05,0.000529809095900502,-0.000158407920461401,0.200122296419822,0.000554067405111938 +0.538,0.55977467561523,-0.391956459363767,0.00432327921462008,-4.09302201020978e-05,-0.200121235893205,-2.31714879922462e-06,3.38884545425522e-05,0.000526525618244067,-0.000157651023312277,0.20012124009229,0.000550664597708975 +0.54,0.559774593822565,-0.39235670078252,0.00432327426502514,-4.08631928572322e-05,-0.200120186107669,-2.63169867495416e-06,3.31407817855122e-05,0.000523277639968844,-0.000156903563058863,0.200120190296967,0.000547299303863329 +0.542,0.559774512162459,-0.392756940108197,0.00432326868782539,-4.07976569749557e-05,-0.200119142782645,-2.94476305146007e-06,3.2397029503528e-05,0.000520064945969123,-0.000156165517149753,0.200119146962956,0.000543971308336358 +0.544,0.559774430631937,-0.39315717735365,0.00432326248597294,-4.07336047392181e-05,-0.200118105847885,-3.25636074355317e-06,3.16571768799178e-05,0.000516887366242002,-0.000155436852625201,0.200118110019997,0.00054068044294447 +0.546,0.55977434922804,-0.393557412531589,0.00432325566238241,-4.0671028267436e-05,-0.20011707523318,-3.56651046196088e-06,3.0921272486939e-05,0.000513744827929096,-0.000154717517443503,0.200117079397874,0.000537426630825073 +0.548,0.559774267947824,-0.393957645654583,0.00432324821993109,-4.06099196492704e-05,-0.200116050868573,-3.87523081332719e-06,3.01892261189707e-05,0.000510637136741376,-0.000154007500328956,0.200116055026618,0.000534209682569182 +0.55,0.559774186788361,-0.394357876735063,0.00432324016145916,-4.05502713629602e-05,-0.200115032684633,-4.1825404632767e-06,2.946096144818e-05,0.000507564146962069,-0.000153306779597517,0.20011503683679,0.000531029453233383 +0.552,0.559774105746738,-0.394758105785322,0.00432323148976924,-4.04920758034777e-05,-0.200114020611985,-4.48845793171726e-06,2.87364923523547e-05,0.000504525744099426,-0.000152615293666502,0.200114024759007,0.000527885821284025 +0.554,0.559774024820058,-0.395158332817511,0.00432322220762743,-4.04353253935507e-05,-0.200113014581657,-4.79300163794271e-06,2.8015742503662e-05,0.0005015217546811,-0.000151933019984507,0.200113018724287,0.000524778615044528 +0.556,0.559773944005437,-0.395558557843648,0.00432321231776268,-4.0380012833463e-05,-0.200112014524967,-5.09619001165528e-06,2.72986772076322e-05,0.000498552078093128,-0.000151259939469572,0.20011201866394,0.000521707735841248 +0.558,0.559773863300006,-0.395958780875611,0.00432320182286738,-4.03261306847202e-05,-0.200111020373345,-5.398041395821e-06,2.65853172809471e-05,0.000495616592904868,-0.000150596020896676,0.200111024509388,0.000518673064432182 +0.56,0.559773782700914,-0.396359001925142,0.0043231907255971,-4.02736715643392e-05,-0.200110032058595,-5.69857409524199e-06,2.58755586401982e-05,0.000492715146460653,-0.000149941222632455,0.200110036192427,0.000515674442193135 +0.562,0.55977370220532,-0.396759221003845,0.004323179028571,-4.02226284501594e-05,-0.200109049512759,-5.99780628635082e-06,2.51693596520219e-05,0.000489847613860394,-0.000149295537738014,0.200109053645089,0.000512711750458771 +0.564,0.5597736218104,-0.397159438123193,0.00432316673437196,-4.01729941257311e-05,-0.20010807266814,-6.29575624619404e-06,2.4466748071994e-05,0.000487013880612341,-0.000148658944529311,0.200108076799672,0.000509784879590283 +0.566,0.559773541513344,-0.397559653294518,0.00432315384554602,-4.01247614578714e-05,-0.200107101457236,-6.59244206446807e-06,2.37676267555997e-05,0.000484213828755297,-0.000148031376219493,0.200107105588666,0.000506893697322126 +0.568,0.559773461311354,-0.397959866529021,0.0043231403646037,-4.00779236187087e-05,-0.200106135812825,-6.88788175107202e-06,2.30719748861574e-05,0.000481447347266961,-0.00014741283888009,0.200106139944839,0.000504038103006338 +0.57,0.559773381201649,-0.398360077837769,0.00432312629401901,-4.00324735583268e-05,-0.200105175667847,-7.18209341998843e-06,2.2379792463667e-05,0.000478714332063923,-0.000146803322102762,0.200105179801127,0.000501217998693714 +0.572,0.55977330118146,-0.398760287231693,0.00432311163623002,-3.99884044488541e-05,-0.200104220955496,-7.47509503948306e-06,2.1690982343614e-05,0.000476014647837752,-0.000146202772978443,0.200104225090715,0.000498433239760067 +0.574,0.559773221248031,-0.399160494721591,0.00432309639363886,-3.99457096289524e-05,-0.200103271609256,-7.7669045119022e-06,2.10055306482104e-05,0.000473348200913381,-0.000145611168955728,0.200103275747081,0.000495683733998203 +0.576,0.559773141398622,-0.39956070031813,0.00432308056861197,-3.99043823262612e-05,-0.200102327562693,-8.05753971530598e-06,2.03234165607746e-05,0.000470714901085189,-0.000145028507432532,0.200102331703783,0.000492969397967511 +0.578,0.559773061630502,-0.399960904031842,0.00432306416347999,-3.98644159627093e-05,-0.200101388749651,-8.34701854163233e-06,1.96445915090493e-05,0.000468114637330874,-0.000144454771928981,0.200101392894661,0.000490290123082269 +0.58,0.559772981940958,-0.400361105873129,0.0043230471805378,-3.9825803960225e-05,-0.200100455104143,-8.6353588030219e-06,1.89690207985649e-05,0.000465547295158685,-0.000143889917342265,0.200100459253719,0.000487645789577218 +0.582,0.559772902327286,-0.400761305852259,0.00432302962204478,-3.9788539879515e-05,-0.200099526560471,-8.92257821100139e-06,1.82966697348519e-05,0.00046301278783245,-0.000143333922855704,0.200099530715254,0.000485036311280886 +0.584,0.559772822786798,-0.40116150397937,0.00432301149022496,-3.97526172812856e-05,-0.200098603052992,-9.20869449444472e-06,1.76275105623347e-05,0.000460511059841017,-0.00014278680061236,0.200098607213617,0.00048246164178475 +0.586,0.559772743316817,-0.401561700264471,0.00432299278726681,-3.97180298372657e-05,-0.200097684516231,-9.49372541345083e-06,1.6961494708756e-05,0.000458041982814849,-0.000142248515050403,0.200097688683328,0.0004799216502282 +0.588,0.559772663914679,-0.401961894717435,0.00432297351532331,-3.96847713024506e-05,-0.200096770885061,-9.77768855464633e-06,1.62985874796462e-05,0.000455605476956666,-0.000141719028005915,0.200096775059252,0.000477416251803062 +0.59,0.559772584577732,-0.402362087348011,0.00432295367651259,-3.96528354873471e-05,-0.200095862094324,-1.00606015254745e-05,1.56387611194297e-05,0.000453201497163657,-0.000141198351621963,0.200095866276227,0.000474945409896222 +0.592,0.559772505303337,-0.402762278165813,0.0043229332729172,-3.96222162579729e-05,-0.200094958079072,-1.03424819611342e-05,1.49819809336371e-05,0.000450829915066286,-0.000140686468551309,0.2000949622693,0.000472508999390286 +0.594,0.559772426088867,-0.403162467180327,0.00432291230658474,-3.95929075636125e-05,-0.200094058774663,-1.06233473996797e-05,1.4328198350011e-05,0.00044849066474506,-0.000140183346701572,0.200094062973823,0.000470106949878298 +0.596,0.559772346931707,-0.403562654400911,0.00432289077952761,-3.95649034645728e-05,-0.200093164116413,-1.09032153479405e-05,1.36773786740818e-05,0.000446183708036063,-0.000139688980001218,0.200093168325107,0.000467739225571242 +0.598,0.559772267829253,-0.403962839836793,0.00432286869372335,-3.95381980489162e-05,-0.200092274039831,-1.11821033196846e-05,1.30295010891679e-05,0.000443908944325333,-0.000139203362378715,0.200092278258656,0.000465405731430333 +0.6,0.559772188778914,-0.404363023497071,0.00432284605111433,-3.95127854602162e-05,-0.200091388480636,-1.14600287974553e-05,1.23845378396936e-05,0.000441666318101718,-0.000138726479088913,0.200091392710184,0.000463106412526795 +0.602,0.559772109778111,-0.404763205390715,0.00432282285360816,-3.94886598975574e-05,-0.200090507374559,-1.17370092360403e-05,1.17424195367199e-05,0.000439455763445728,-0.000138258312784578,0.200090511615419,0.000460841202038492 +0.604,0.559772030824275,-0.405163385526569,0.00432279910307738,-3.94658157820693e-05,-0.200089630657582,-1.20130620485936e-05,1.11031323024591e-05,0.000437277224846211,-0.000137798837444858,0.200089634910337,0.000458610041884479 +0.606,0.559771951914848,-0.405563563913346,0.00432277480135997,-3.94442473683476e-05,-0.200088758265659,-1.22882045858197e-05,1.0466676136911e-05,0.000435130646792014,-0.000137348073886434,0.200088762530888,0.000456412888169588 +0.608,0.559771873047285,-0.405963740559631,0.00432274995025904,-3.94239490775216e-05,-0.200087890134995,-1.25624543441394e-05,9.83294695666713e-06,0.000433015956424753,-0.000136906007364156,0.200087894413272,0.000454249667271965 +0.61,0.559771794219052,-0.406363915473886,0.00432272455154259,-3.94049155805209e-05,-0.200087026201834,-1.28358286152763e-05,9.20192394504581e-06,0.000430933143336087,-0.000136472588438408,0.200087030493731,0.000452120356569896 +0.612,0.559771715427623,-0.406764088664439,0.00432269860694458,-3.93871413817415e-05,-0.200086166402422,-1.3108344697893e-05,8.57362791872873e-06,0.000428882134667629,-0.000136047863079358,0.200086170708505,0.000450024903290041 +0.614,0.559771636670486,-0.407164260139495,0.0043226721181638,-3.9370621068846e-05,-0.200085310673295,-1.33800200675937e-05,7.94798254988293e-06,0.000426862878377676,-0.000135631794857816,0.200085314994129,0.000447963247532474 +0.616,0.559771557945139,-0.407564429907132,0.00432264508686431,-3.93553494515419e-05,-0.200084458950908,-1.36508718773243e-05,7.3249392662511e-06,0.000424875364057886,-0.000135224369895992,0.200084463287051,0.000445935376408223 +0.618,0.559771479249088,-0.407964597975299,0.00432261751467629,-3.9341321311781e-05,-0.200083611171839,-1.39209175471777e-05,6.70449112893933e-06,0.000422919539666555,-0.000134825635898783,0.200083615523848,0.000443941256619139 +0.62,0.559771400579854,-0.408364764351819,0.00432258940319412,-3.93285314870262e-05,-0.200082767272749,-1.41901744209194e-05,6.08660344347811e-06,0.000420995370509214,-0.000134435532150867,0.200082771641178,0.000441980837861049 +0.622,0.559771321934962,-0.40876492904439,0.0043225607539786,-3.93169748980071e-05,-0.200081927190357,-1.44586596757812e-05,5.47122069871619e-06,0.000419102808013605,-0.000134054059519605,0.200081931575754,0.000440054074875473 +0.624,0.559771243311954,-0.409165092060581,0.00432253156855542,-3.93066466042313e-05,-0.200081090861517,-1.47263906589978e-05,4.85829432239626e-06,0.000417241866057516,-0.000133681232750149,0.20008109526443,0.000438160986173439 +0.626,0.559771164708376,-0.409565253407836,0.00432250184841597,-3.92975417207175e-05,-0.200080258222893,-1.49933846067818e-05,4.24783819230612e-06,0.000415412537702053,-0.000133317048373049,0.200080262643864,0.000436301565429543 +0.628,0.559771086121787,-0.409965413093472,0.00432247159501699,-3.92896552514621e-05,-0.200079429211367,-1.525965885249e-05,3.63982455287015e-06,0.000413614715394361,-0.00013296150291886,0.200079433650939,0.000434475709760599 +0.63,0.559771007549755,-0.410365571124681,0.00432244080978056,-3.9282982422506e-05,-0.200078603764031,-1.55252306184572e-05,3.0341423817859e-06,0.000411848447706697,-0.000132614592050773,0.200078608222742,0.000432683463889252 +0.632,0.559770928989857,-0.410765727508528,0.00432240949409451,-3.92775186819349e-05,-0.200077781817576,-1.57901172206931e-05,2.43078474015945e-06,0.000410113751986296,-0.000132276333116021,0.200077786295962,0.000430924850275951 +0.634,0.55977085043968,-0.411165882251952,0.00432237764931168,-3.92732592835454e-05,-0.200076963309023,-1.60543359509213e-05,1.82975856688472e-06,0.000408410558844218,-0.000131946710502095,0.200076967807618,0.000429199798467082 +0.636,0.55977077189682,-0.411566035361765,0.00432234527675071,-3.92701996476674e-05,-0.20007614817534,-1.63179040627015e-05,1.23101528970437e-06,0.00040673889603604,-0.000131625725076356,0.200076152694676,0.000427508334946977 +0.638,0.559770693358882,-0.411966186844653,0.00432231237769543,-3.92683352223866e-05,-0.200075336353439,-1.65808388512267e-05,6.34478580785469e-07,0.000405098739275633,-0.000131313380308251,0.200075340894044,0.000425850437329535 +0.64,0.559770614823479,-0.412366336707178,0.00432227895339531,-3.92676617333442e-05,-0.200074527780383,-1.68431575839345e-05,4.01068067645837e-08,0.000403490126726913,-0.000131009690075567,0.200074532342786,0.000424226145903753 +0.642,0.559770536288235,-0.412766484955775,0.00432224500506509,-3.92681747951595e-05,-0.200073722392932,-1.7104877611529e-05,-5.52058398994859e-07,0.000401913065328774,-0.000130714663051922,0.200073726977658,0.00042263546939085 +0.644,0.55977045775078,-0.41316663159675,0.00432221053388486,-3.92698699669402e-05,-0.200072920128122,-1.73660162361422e-05,-1.1420933643258e-06,0.000400367503039511,-0.000130428297502593,0.200072924735695,0.00042107835690851 +0.646,0.559770379208755,-0.413566776636287,0.00432217554100015,-3.92727431686168e-05,-0.20007212092292,-1.76265908015394e-05,-1.73008135595509e-06,0.000398853519656405,-0.00013015059342758,0.200072125553863,0.000419554883529381 +0.648,0.559770300659807,-0.413966920080442,0.00432214002752165,-3.9276790292364e-05,-0.200071324714043,-1.78866186098525e-05,-2.31600155720102e-06,0.000397371132526691,-0.000129881557765776,0.200071329368877,0.000418065066558332 +0.65,0.559770222101594,-0.414367061935143,0.00432210399452571,-3.92820071748456e-05,-0.20007053143839,-1.81461170326025e-05,-2.899895601427e-06,0.000395920313894793,-0.000129621226946375,0.200070536117634,0.000416608889516455 +0.652,0.559770143531779,-0.414767202206195,0.00432206744305352,-3.92883898747698e-05,-0.200069741032788,-1.8405103517638e-05,-3.48184675535989e-06,0.000394501129680202,-0.000129369581887417,0.200069745736961,0.000415186407886275 +0.654,0.559770064948034,-0.415167340899274,0.00432203037411164,-3.92959345618671e-05,-0.200068953433871,-1.86635953601521e-05,-4.06182726342407e-06,0.000393113621516283,-0.000129126632129883,0.200068958163491,0.000413797661892345 +0.656,0.559769986348041,-0.415567478019931,0.00432199278867208,-3.93046371838235e-05,-0.200068168578302,-1.89216100461575e-05,-4.63988569787687e-06,0.000391757789403035,-0.000128892422776583,0.200068173333885,0.000412442663586661 +0.658,0.559769907729486,-0.415967613573588,0.00432195468767145,-3.93144941046586e-05,-0.200067386402713,-1.91791650512585e-05,-5.21612614212685e-06,0.000390433692321057,-0.000128666934745558,0.200067391184777,0.000411121461575776 +0.66,0.559769829090064,-0.416367747565542,0.00432191607201188,-3.9325501688392e-05,-0.200066606843532,-1.94362777851398e-05,-5.7905416572801e-06,0.000389141382312052,-0.000128450187986129,0.200066611652591,0.000409834108627005 +0.662,0.559769750427479,-0.416767880000962,0.00432187694256031,-3.93376562712877e-05,-0.200065829837184,-1.9692965803203e-05,-6.36314612112442e-06,0.00038788088366215,-0.000128242207651785,0.200065834673752,0.000408580632632646 +0.664,0.559769671739439,-0.41716801088489,0.00432183730014866,-3.93509542728765e-05,-0.200065055319998,-1.99492466157469e-05,-6.93400892259887e-06,0.000386652255351949,-0.000128042989405719,0.20006506018459,0.000407361085751254 +0.666,0.559769593023662,-0.417568140222242,0.00432179714557385,-3.93653923069781e-05,-0.200064283228163,-2.02051377608259e-05,-7.50317863396077e-06,0.000385455556362046,-0.000127852567075037,0.200064288121294,0.000406175531670317 +0.668,0.55976951427787,-0.417968268017803,0.00432175647959762,-3.93809669874123e-05,-0.200063513497772,-2.04606568840471e-05,-8.0706483163162e-06,0.000384290863020275,-0.000127670960609061,0.200063518419957,0.000405024044900984 +0.67,0.559769435499794,-0.418368394276233,0.00432171530294631,-3.93976749002433e-05,-0.200062746064711,-2.07158216032621e-05,-8.63649429749812e-06,0.000383158227368341,-0.000127498179548768,0.200062751016462,0.000403906674891676 +0.672,0.55976935668717,-0.418768519002062,0.00432167361631121,-3.94155129646023e-05,-0.200061980864863,-2.09706496022421e-05,-9.20077902755167e-06,0.000382057701447946,-0.000127334255119182,0.200061985846696,0.000402823477589327 +0.674,0.559769277837742,-0.419168642199692,0.0043216314203479,-3.94344780163535e-05,-0.200061217833905,-2.12251586237388e-05,-9.76349556758293e-06,0.000380989375464713,-0.000127179198596005,0.200061222846334,0.000401774537045607 +0.676,0.559769198949258,-0.419568763873397,0.00432158871567671,-3.94545669468726e-05,-0.200060456907361,-2.14793663966262e-05,-1.03246786120614e-05,0.000379953332685367,-0.000127033018652856,0.200060461950901,0.000400759930426205 +0.678,0.559769120019474,-0.419968884027322,0.00432154550288232,-3.94757767308018e-05,-0.200059698020574,-2.17332906983503e-05,-1.08844183666079e-05,0.000378949645968296,-0.000126895756055734,0.200059703095741,0.000399779736414482 +0.68,0.559769041046151,-0.42036900266548,0.00432150178251392,-3.94981046203391e-05,-0.200058941108777,-2.19869494208491e-05,-1.14427564645858e-05,0.000377978402049672,-0.000126767441162301,0.200058946216087,0.000398834042216018 +0.682,0.559768962027056,-0.420769119791757,0.00432145755508463,-3.95215477566601e-05,-0.200058186106966,-2.22403604629995e-05,-1.19996651504195e-05,0.000377039725829586,-0.000126648086982983,0.200058191246936,0.000397922963340671 +0.684,0.55976888295996,-0.421169235409908,0.00432141282107207,-3.95461032809407e-05,-0.200057432949874,-2.2493541768781e-05,-1.25552485075175e-05,0.000376133700574765,-0.000126537724742803,0.200057438123022,0.000397046585120988 +0.686,0.559768803842642,-0.421569349523556,0.00432136758091756,-3.95717687506902e-05,-0.200056681572164,-2.27465113619707e-05,-1.31095620470312e-05,0.000375260402613042,-0.000126436386534145,0.20005668677901,0.000396204985110207 +0.688000000000001,0.559768724672885,-0.421969462136196,0.00432132183502662,-3.95985415291289e-05,-0.200055931908263,-2.29992873149176e-05,-1.36625780133848e-05,0.000374419946436166,-0.000126344089704244,0.200055937149327,0.000395398269255731 +0.690000000000001,0.559768645448476,-0.422369573251189,0.0043212755837683,-3.96264190627437e-05,-0.200055183892378,-2.32518877207877e-05,-1.42144004899869e-05,0.000373612460413674,-0.00012626085940659,0.200055189168181,0.000394626563190746 +0.692000000000001,0.559768566167209,-0.422769682871766,0.00432122882747574,-3.96553991310888e-05,-0.200054437458422,-2.3504330752544e-05,-1.4765071110201e-05,0.000372838079853998,-0.00012618674855025,0.200054442769487,0.000393890005742453 +0.694000000000001,0.55976848682688,-0.423169791001023,0.00432118156644529,-3.96854793471845e-05,-0.200053692540059,-2.37566347149887e-05,-1.53146037518148e-05,0.000372096901901653,-0.000126121787492883,0.20005369788691,0.000393188691054668 +0.696000000000001,0.559768407425292,-0.423569897641926,0.00432113380093688,-3.97166575460961e-05,-0.200052949070814,-2.40088179025155e-05,-1.58630886204491e-05,0.000371389058395621,-0.000126065992714364,0.200052954453979,0.000392522744303593 +0.698000000000001,0.55976832796025,-0.423970002797306,0.00432108553117368,-3.97489317016663e-05,-0.200052206983825,-2.42608986858461e-05,-1.64105187772101e-05,0.000370714712399911,-0.000126019405848054,0.200052212403832,0.000391892324199623 +0.700000000000001,0.559768248429565,-0.424370106469861,0.00432103675734213,-3.97822996212049e-05,-0.200051466211964,-2.45128955259077e-05,-1.69569497332489e-05,0.000370073964528483,-0.000125982074598851,0.200051471669344,0.00039129753447513 +0.702000000000001,0.559768168831051,-0.424770208662154,0.00432098747959157,-3.98167595005993e-05,-0.200050726687967,-2.47648269842415e-05,-1.7502492510868e-05,0.000369466967437004,-0.000125954018916074,0.20005073218325,0.000390738521459428 +0.704000000000001,0.559768089162527,-0.425170309376613,0.0043209376980342,-3.98523095912484e-05,-0.200049988344095,-2.5016711601572e-05,-1.80471471100674e-05,0.000368893908475609,-0.000125935276096277,0.200049993877818,0.000390215464832788 +0.706000000000001,0.559768009421813,-0.425570408615531,0.00432088741274517,-3.98889480890396e-05,-0.200049251112333,-2.52685680886266e-05,-1.85909482253166e-05,0.000368354922952729,-0.000125925908589508,0.200049256685032,0.000389728504312323 +0.708000000000001,0.559767929606734,-0.425970506381063,0.00432083662376184,-3.99266733841496e-05,-0.200048514924403,-2.552041523593e-05,-1.91339860622364e-05,0.000367850177401818,-0.000125925941549254,0.200048520536618,0.000389277799511152 +0.710000000000001,0.559767849715119,-0.426370602675228,0.00432078533108422,-3.99654840332886e-05,-0.200047779711623,-2.57722718548236e-05,-1.96762675597206e-05,0.000367379824478542,-0.000125935424415136,0.200047785363898,0.00038886350055204 +0.712000000000001,0.559767769744798,-0.426770697499909,0.00432073353467442,-4.00053784543885e-05,-0.200047045405105,-2.60241569335906e-05,-2.02178551678145e-05,0.000366944061941376,-0.000125954388412175,0.200047051097983,0.000388485796679761 +0.714000000000001,0.559767689693605,-0.427170790856849,0.00432068123445649,-4.00463554539598e-05,-0.200046311935376,-2.62760894084723e-05,-2.07588668477143e-05,0.000366543091018245,-0.00012598288211263,0.200046317669406,0.00038814488881269 +0.716000000000001,0.559767609559376,-0.427570882747651,0.00432062843031679,-4.00884139217794e-05,-0.200045579232741,-2.65280884620411e-05,-2.12992679049506e-05,0.000366177053956473,-0.000126020982711694,0.200045585008476,0.000387840923241926 +0.718000000000001,0.55976752933995,-0.42797097317378,0.00432057512210264,-4.01315525255796e-05,-0.20004484722716,-2.67801733393191e-05,-2.18391277284624e-05,0.000365846183209006,-0.000126068702352433,0.200044853045153,0.000387574115468724 +0.720000000000001,0.559767449033166,-0.428371062136559,0.00432052130962343,-4.01757704326932e-05,-0.200044115848008,-2.70323632714508e-05,-2.23785850961278e-05,0.000365550669595427,-0.000126126091341827,0.200044121708818,0.00038734465793365 +0.722000000000001,0.559767368636868,-0.428771149637172,0.00432046699264955,-4.02210668659641e-05,-0.200043385024481,-2.72846777046864e-05,-2.29175983745833e-05,0.000365290707404764,-0.000126193229477156,0.20004339092867,0.000387152745563564 +0.724000000000001,0.559767288148899,-0.429171235676657,0.00432041217091261,-4.02674408261916e-05,-0.200042654685178,-2.75371361893595e-05,-2.34562022582985e-05,0.000365066560314986,-0.00012627015145289,0.200042660633311,0.000386998627946577 +0.726000000000001,0.559767207567105,-0.429571320255912,0.0043203568441048,-4.03148916749973e-05,-0.20004192475824,-2.7789758310498e-05,-2.3994563280727e-05,0.000364878412206781,-0.000126356896300306,0.200041930750887,0.000386882488624415 +0.728000000000001,0.559767126889332,-0.42997140337569,0.00432030101187937,-4.03634190793145e-05,-0.20004119517153,-2.80425637745607e-05,-2.45327230752323e-05,0.000364726488594202,-0.000126453540347238,0.200041201209263,0.00038680455510515 +0.730000000000001,0.559767046113428,-0.430371485036598,0.0043202446738497,-4.04130225672983e-05,-0.200040465852286,-2.82955724718869e-05,-2.50706816418145e-05,0.000364611056624664,-0.00012656014604373,0.200040471935684,0.000386765086657973 +0.732000000000001,0.559766965237242,-0.430771565239099,0.00432018782958948,-4.04637018058817e-05,-0.200039736727303,-2.85488043587356e-05,-2.56084944916246e-05,0.000364532345281665,-0.000126676764564126,0.200039742856949,0.00038676430604066 +0.734000000000001,0.559766884258621,-0.431171643983508,0.00432013047863226,-4.05154565452647e-05,-0.200039007722905,-2.88022795301434e-05,-2.61462726469652e-05,0.000364490621712621,-0.000126803452286938,0.200039013899384,0.000386802477243448 +0.736000000000001,0.559766803175416,-0.431571721269991,0.00432007262047136,-4.05682868964696e-05,-0.200038278764816,-2.90560181678834e-05,-2.66840924356692e-05,0.000364486142656606,-0.000126940282070553,0.20003828498872,0.000386879857565574 +0.738000000000001,0.559766721985474,-0.431971797098567,0.00432001425455959,-4.06221929150074e-05,-0.200037549778334,-2.93100406584257e-05,-2.72219469188428e-05,0.000364519161383247,-0.000127087303354589,0.200037556050259,0.000386996687432256 +0.740000000000001,0.559766640686644,-0.432371871469104,0.00431995538030873,-4.0677174684145e-05,-0.200036820688171,-2.95643673813017e-05,-2.77599540576823e-05,0.00036458996932609,-0.000127244556905048,0.200036827008718,0.000387153248784568 +0.742000000000001,0.559766559276775,-0.43277194438132,0.00431989599709007,-4.07332327312382e-05,-0.200036091418457,-2.98190188860459e-05,-2.82981971189144e-05,0.000364698850979783,-0.000127412175428276,0.200036097788232,0.000387349844860587 +0.744000000000001,0.559766477753713,-0.433172015834778,0.00431983610423319,-4.07903674726206e-05,-0.200035361892767,-3.00740160830148e-05,-2.8836703858115e-05,0.000364846111655659,-0.000127590213568063,0.200035368312383,0.000387586768708631 +0.746000000000001,0.559766396115305,-0.433572085828891,0.00431977570102574,-4.08485795466706e-05,-0.20003463203401,-3.03293797403181e-05,-2.93755506031168e-05,0.000365032046256708,-0.000127778690406366,0.200034638504084,0.000387864295198155 +0.748000000000001,0.559766314359395,-0.433972154362914,0.00431971478671422,-4.09078696750331e-05,-0.200033901764582,-3.05851308446403e-05,-2.99148275595406e-05,0.000365256960094262,-0.000127977724771744,0.200033908285736,0.00038818274284539 +0.750000000000001,0.559766232483826,-0.434372221435949,0.00431965336050236,-4.09682388569088e-05,-0.20003317100617,-3.08412906394051e-05,-3.04546110552195e-05,0.000365521200113017,-0.000128187372175348,0.200033177579034,0.000388542447426026 +0.752000000000001,0.55976615048644,-0.434772287046939,0.00431959142155167,-4.1029688119254e-05,-0.200032439679781,-3.10978803333417e-05,-3.09949427235167e-05,0.000365825068154857,-0.000128407716751266,0.20003244630499,0.000388943708891419 +0.754000000000001,0.559766068365074,-0.435172351194668,0.00431952896898102,-4.10922186278028e-05,-0.200031707705897,-3.13549215064101e-05,-3.15359405256288e-05,0.000366168900756136,-0.000128638859113461,0.200031714384092,0.000389386871247357 +0.756000000000001,0.559765986117565,-0.435572413877762,0.00431946600186564,-4.11558318813565e-05,-0.200030975004178,-3.16124357697955e-05,-3.20776530338129e-05,0.000366553083025467,-0.000128880829619593,0.200030981736007,0.000389872295428987 +0.758000000000001,0.559765903741746,-0.435972475094685,0.00431940251923794,-4.12205292399381e-05,-0.200030241493565,-3.18704448248885e-05,-3.26201426981143e-05,0.000366977965376991,-0.000129133735822517,0.200030248279682,0.000390400336205492 +0.760000000000001,0.559765821235448,-0.436372534843737,0.00431933852008634,-4.12863124521489e-05,-0.200029507092317,-3.21289707130856e-05,-3.31635552353049e-05,0.000367443901694297,-0.000129397680070919,0.200029513933383,0.000390971356874016 +0.762000000000001,0.559765738596496,-0.436772593123054,0.00431927400335509,-4.13531834608793e-05,-0.200028771717958,-3.23880355451722e-05,-3.37079184009603e-05,0.000367951277085998,-0.000129672724814844,0.200028778614642,0.000391585727054816 +0.764000000000001,0.559765655822714,-0.437172649930608,0.00431920896794416,-4.14211441257528e-05,-0.200028035287208,-3.2647661612345e-05,-3.42533154618074e-05,0.000368500487069045,-0.000129958973270339,0.200028042240186,0.000392243844213941 +0.766000000000001,0.55976557291192,-0.437572705264203,0.00431914341270864,-4.14901967227265e-05,-0.20002729771601,-3.29078714382536e-05,-3.47998782568304e-05,0.000369091927160392,-0.000130256517377747,0.200027304725964,0.000392946106407789 +0.768000000000001,0.559765489861927,-0.437972759121472,0.00431907733645841,-4.15603436387801e-05,-0.2000265589195,-3.3168687681856e-05,-3.53476692360743e-05,0.000369726027571459,-0.000130565437801711,0.200026565987121,0.000393692934572805 +0.770000000000001,0.559765406670545,-0.438372811499881,0.00431901073795792,-4.16315873996708e-05,-0.2000258188119,-3.34301331894604e-05,-3.58967647273722e-05,0.000370403215044223,-0.000130885818676318,0.200025825937887,0.000394484748708438 +0.772000000000001,0.559765323335578,-0.43877286239672,0.00431894361592565,-4.17039306976896e-05,-0.20002507730664,-3.36922309565612e-05,-3.64472549363448e-05,0.000371123905912318,-0.000131217778830128,0.200025084491699,0.000395321971831977 +0.774000000000001,0.559765239854823,-0.439172911809108,0.00431887596903409,-4.17773764194162e-05,-0.200024334316276,-3.39550043047809e-05,-3.6999285579764e-05,0.000371888554673294,-0.000131561429285443,0.200024341561123,0.000396205065472456 +0.776000000000001,0.559765156226072,-0.439572959733985,0.00431880779590843,-4.18519278400087e-05,-0.200023589752421,-3.4218476673703e-05,-3.75529052298873e-05,0.000372697626233042,-0.000131916848972178,0.200023597057779,0.000397134481421216 +0.778000000000001,0.559765072447111,-0.439973006168117,0.00431873909512739,-4.19275880403358e-05,-0.200022843525771,-3.44826717006696e-05,-3.81081763367579e-05,0.000373551599375243,-0.000132284154116811,0.200022850892373,0.000398110698085239 +0.780000000000001,0.55976498851572,-0.440373051108088,0.00431866986522163,-4.20043605453557e-05,-0.200022095546023,-3.47476132901703e-05,-3.86653001283027e-05,0.000374450966761363,-0.000132663459211091,0.20002210297461,0.000399134219721116 +0.782000000000001,0.559764904429669,-0.440773094550301,0.00431860010467423,-4.2082249240849e-05,-0.200021345721904,-3.5013325537514e-05,-3.92243598712482e-05,0.000375396200236188,-0.000133054853593273,0.200021353213226,0.000400205511735914 +0.784000000000001,0.559764820186723,-0.441173136490976,0.00431852981191948,-4.21612579848407e-05,-0.200020593961222,-3.52798327045433e-05,-3.97853902600609e-05,0.000376387830625102,-0.000133458454357194,0.20002060151604,0.000401325099339958 +0.786000000000001,0.559764735784637,-0.441573176926146,0.00431845898534342,-4.22413908018892e-05,-0.200019840170581,-3.55471593549428e-05,-4.03485300726203e-05,0.000377426416509063,-0.000133874379464049,0.200019847789665,0.000402493544339258 +0.788000000000001,0.55976465122116,-0.441973215851658,0.00431838762328206,-4.23226521051312e-05,-0.200019084255556,-3.58153302223995e-05,-4.09139250256984e-05,0.000378512450549539,-0.000134302752079207,0.200019091939685,0.0004037113494789 +0.790000000000001,0.559764566494029,-0.442373253263168,0.00431831572402253,-4.2405046501992e-05,-0.200018326120779,-3.60843703632596e-05,-4.14816514471283e-05,0.00037964649132749,-0.00013474369883748,0.200018333870743,0.000404979073731545 +0.792000000000001,0.559764481600974,-0.442773289156141,0.0043182432858006,-4.24885787109197e-05,-0.200017565669591,-3.63543050177495e-05,-4.2051848114788e-05,0.00038082913558779,-0.000135197334230619,0.200017573486191,0.000406297314322256 +0.794000000000001,0.559764396539714,-0.443173323525847,0.00431817030680245,-4.25732538944512e-05,-0.200016802804237,-3.66251597001821e-05,-4.26246399287677e-05,0.00038206095231974,-0.000135663793567053,0.200016810688285,0.000407666648257529 +0.796000000000001,0.559764311307958,-0.443573356367358,0.0043180967851618,-4.26590772706347e-05,-0.200016037425782,-3.68969601920177e-05,-4.32001240335822e-05,0.000383342552146004,-0.000136143170521852,0.2000160453781,0.000409087675055202 +0.798000000000001,0.559764225903405,-0.44397338767555,0.00431802271896169,-4.27460543905855e-05,-0.200015269434028,-3.71697323822695e-05,-4.37784392071094e-05,0.000384674583853161,-0.000136635626424297,0.200015277455451,0.000410561057104024 +0.800000000000001,0.559764140323741,-0.444373417445094,0.00431794810623227,-4.28341910274632e-05,-0.200014498727446,-3.74435026977149e-05,-4.43597172883336e-05,0.000386057633777747,-0.000137141322603673,0.200014506818819,0.000412087397867805 +0.802000000000001,0.559764054566641,-0.44477344567046,0.0043178729449509,-4.29234932597388e-05,-0.200013725203493,-3.77182976726842e-05,-4.49440970551329e-05,0.000387492340298001,-0.000137660350132962,0.200013733365674,0.000413667327267277 +0.804000000000001,0.559763968629768,-0.445173472345908,0.00431779723304158,-4.30139674156837e-05,-0.200012948758085,-3.79941440982467e-05,-4.55316964687035e-05,0.000388979435467229,-0.000138192853861574,0.200012956991943,0.000415301578925639 +0.806000000000001,0.559763882510771,-0.445573497465492,0.0043177209683745,-4.31056200456136e-05,-0.200012169285751,-3.82710690881305e-05,-4.61226681847115e-05,0.000390519575010906,-0.000138738989914622,0.200012177592169,0.000416990822812331 +0.808000000000001,0.559763796207287,-0.445973521023051,0.00431764414876523,-4.31984580884226e-05,-0.200011386679785,-3.85491000579052e-05,-4.67171301643532e-05,0.000392113438940633,-0.000139298940438071,0.200011395059658,0.000418735756839336 +0.810000000000001,0.559763709716939,-0.446373543012211,0.00431756677197427,-4.32924885662711e-05,-0.200010600831996,-3.88282648498827e-05,-4.73152420021882e-05,0.000393761787065294,-0.000139872837270904,0.200010609286232,0.000420537141861206 +0.812000000000001,0.559763623037333,-0.446773563426379,0.00431748883570583,-4.33877190564313e-05,-0.200009811632637,-3.91085914069888e-05,-4.79171979872461e-05,0.000395465372254877,-0.000140460775822915,0.200009820162159,0.000422395724481691 +0.814000000000001,0.559763536166062,-0.447173582258742,0.00431741033760864,-4.348415735822e-05,-0.200009018970507,-3.93901079531744e-05,-4.85230952640414e-05,0.000397224919623795,-0.000141062956454663,0.20000902757625,0.000424312249679247 +0.816000000000001,0.559763449100704,-0.447573599502261,0.00431733127527402,-4.35818114374875e-05,-0.200008222732958,-3.96728432328075e-05,-4.91330934271339e-05,0.000399041227144847,-0.00014167958473088,0.200008231415873,0.00042628753976688 +0.818000000000001,0.559763361838817,-0.447973615149674,0.00431725164623571,-4.36806897319286e-05,-0.200007422805598,-3.99568262920979e-05,-4.97473520710833e-05,0.000400915124015854,-0.000142310789888467,0.200007431566649,0.000428322421338659 +0.820000000000001,0.559763274377945,-0.448373629193484,0.00431717144796885,-4.37808008457718e-05,-0.200006619072462,-4.02420863923613e-05,-5.03660654849191e-05,0.00040284741514851,-0.000142956720246279,0.20000662791263,0.000430417709101101 +0.822000000000001,0.559763186715613,-0.448773641625963,0.00431709067789014,-4.38821539938683e-05,-0.200005811415937,-4.0528653173083e-05,-5.09894210187766e-05,0.000404838947087871,-0.000143617574430155,0.200005820336216,0.000432574273183516 +0.824000000000001,0.559763098849329,-0.449173652439147,0.00431700933335616,-4.39847585298469e-05,-0.200004999716674,-4.0816556690082e-05,-5.16175227560644e-05,0.000406890590665121,-0.00014429353458606,0.200005008718075,0.000434792991607066 +0.826000000000001,0.559763010776579,-0.44957366162483,0.00431692741166338,-4.40886240848925e-05,-0.200004183853575,-4.11058273114273e-05,-5.22505996802813e-05,0.000409003254875362,-0.000144984756839106,0.200004192937124,0.000437074784655079 +0.828000000000001,0.559762922494832,-0.449973669174562,0.00431684491004691,-4.4193760928568e-05,-0.200003363703655,-4.13964957174384e-05,-5.28888460804566e-05,0.000411177879938717,-0.000145691432008876,0.200003372870396,0.000439420609839158 +0.830000000000001,0.559762834001535,-0.450373675079645,0.00431676182568051,-4.43001794692143e-05,-0.200002539142055,-4.16885930394628e-05,-5.35324007344684e-05,0.000413415364441949,-0.000146413790813591,0.200002548393049,0.000441831392756865 +0.832000000000001,0.559762745294115,-0.45077367933113,0.00431667815567476,-4.44078905315059e-05,-0.200001710042197,-4.19821508806928e-05,-5.41814926258155e-05,0.000415716683299649,-0.00014715198590892,0.200001719378521,0.000444308115921554 +0.834000000000001,0.559762656369973,-0.451173681919813,0.00431659389707699,-4.45169054397176e-05,-0.200000876275322,-4.22772009830985e-05,-5.48363160435272e-05,0.000418082873876457,-0.000147906198573466,0.200000885698072,0.000446851826182323 +0.836000000000001,0.559762567226493,-0.451573682836231,0.00431650904687082,-4.462723579568e-05,-0.200000037710701,-4.25737756749866e-05,-5.54970791544207e-05,0.000420514928434201,-0.000148676686413662,0.200000047220991,0.000449463555712407 +0.838000000000001,0.55976247786103,-0.451973682070656,0.00431642360197429,-4.47388937563353e-05,-0.199999194215608,-4.28719077287531e-05,-5.61639831864191e-05,0.000423013891276413,-0.000149463628106028,0.199999203814569,0.000452144358968233 +0.840000000000001,0.559762388270918,-0.452373679613094,0.00431633755923991,-4.48518917284257e-05,-0.199998345655136,-4.31716301874107e-05,-5.68372224285518e-05,0.000425580865687225,-0.000150267230082657,0.199998355343921,0.000454895354481879 +0.842000000000001,0.559762298453463,-0.452773675453277,0.00431625091545354,-4.49662426460495e-05,-0.199997491892145,-4.34729766490837e-05,-5.75170744365749e-05,0.000428216979236895,-0.000151087725663856,0.199997501671924,0.000457717703398929 +0.844000000000001,0.559762208405947,-0.453173669580662,0.00431616366733331,-4.5081960026172e-05,-0.199996632787219,-4.37759810900662e-05,-5.82037404384116e-05,0.000430923345617895,-0.000151925323883806,0.199996642659184,0.000460612536994751 +0.846000000000001,0.559762118125623,-0.453573661984426,0.00431607581152918,-4.51990576078031e-05,-0.199995768198763,-4.4080677944619e-05,-5.88974077841974e-05,0.000433701095869931,-0.000152780264134344,0.199995778164126,0.000463581011594993 +0.848000000000001,0.559762027609717,-0.453973652653457,0.00431598734462153,-4.53175496573088e-05,-0.199994897982836,-4.43871021466035e-05,-5.95983609685823e-05,0.000436551478993907,-0.000153652782337865,0.19999490804283,0.000466624405664852 +0.850000000000001,0.559761936855425,-0.454373641576357,0.00431589826312059,-4.54374510516775e-05,-0.199994021992847,-4.46952890739704e-05,-6.03068844862164e-05,0.000439475768276853,-0.000154543098804249,0.199994032148727,0.000469744016041699 +0.852000000000001,0.559761845859913,-0.454773628741429,0.00431580856346524,-4.55587771952537e-05,-0.199993140079763,-4.50052745418205e-05,-6.10231795650228e-05,0.000442475184964097,-0.000155451462466316,0.199993150332806,0.000472941090344404 +0.854000000000001,0.559761754620316,-0.455173614136676,0.00431571824202243,-4.56815437699376e-05,-0.199992252092107,-4.53170949238357e-05,-6.17474613107127e-05,0.000445551033567692,-0.00015637814047148,0.199992262443614,0.00047621696243296 +0.856000000000001,0.559761663133737,-0.455573597749797,0.00431562729508554,-4.58057670404965e-05,-0.199991357875628,-4.56307871037065e-05,-6.24800905457689e-05,0.000448704674110844,-0.000157323378283114,0.199991368326922,0.000479573030557326 +0.858000000000001,0.559761571397248,-0.455973579568179,0.00431553571887401,-4.59314641321206e-05,-0.199990457273411,-4.59463884369681e-05,-6.32213378870538e-05,0.000451937497841781,-0.000158287428303483,0.199990467825839,0.000483010713627192 +0.860000000000001,0.559761479407881,-0.456373559578891,0.00431544350953179,-4.60586523920447e-05,-0.199989550125637,-4.62639368169204e-05,-6.39714115013845e-05,0.000455250923764305,-0.000159270549006385,0.199989560780572,0.000486531451047646 +0.862000000000001,0.559761387162638,-0.456773537768681,0.00431535066312674,-4.61873497781262e-05,-0.199988636269715,-4.65834706329937e-05,-6.473066527235e-05,0.00045864637088222,-0.00016027303963162,0.199988647028554,0.000490136715399684 +0.864000000000001,0.559761294658482,-0.45717351412397,0.00431525717564926,-4.63175750531341e-05,-0.199987715540153,-4.69050289754469e-05,-6.54993836946005e-05,0.00046212537269108,-0.000161295195082178,0.199987726404319,0.000493828076668226 +0.866000000000001,0.559761201892338,-0.457573488630842,0.00431516304301084,-4.64493473129046e-05,-0.199986787768225,-4.72286514133224e-05,-6.62778512627859e-05,0.000465689476564223,-0.00016233726949505,0.199986798739168,0.000497607105272604 +0.868000000000001,0.559761108861093,-0.457973461275043,0.00431506826104361,-4.65826864581853e-05,-0.199985852782247,-4.75543780534271e-05,-6.70664287993893e-05,0.000469340261100015,-0.000163399551701693,0.199985863861444,0.000501475423184076 +0.870000000000001,0.559761015561592,-0.458373432041971,0.00431497282549863,-4.67176130281021e-05,-0.19998491040718,-4.78822496201292e-05,-6.78654146768486e-05,0.000473079391632991,-0.00016448235742178,0.199984921596137,0.00050543473485716 +0.872000000000001,0.55976092199064,-0.458773400916671,0.00431487673204513,-4.68541481168927e-05,-0.199983960464681,-4.82123074831142e-05,-6.86751489009651e-05,0.000476908505742113,-0.000165586005844431,0.199983971764931,0.000509486726281712 +0.874000000000001,0.559760828145,-0.45917336788383,0.0043147799762687,-4.6992313623706e-05,-0.199983002773157,-4.85445936435069e-05,-6.94959992331156e-05,0.000480829310395281,-0.000166710797076808,0.199983014186265,0.000513633146814528 +0.876000000000001,0.559760734021386,-0.459573332927764,0.00431468255367055,-4.71321321138251e-05,-0.199982037147439,-4.88791506714214e-05,-7.03282779235258e-05,0.000484843633991038,-0.000167857047705944,0.199982048674997,0.000517875858253578 +0.878000000000001,0.559760639616471,-0.459973296032419,0.00431458445966601,-4.72736267354001e-05,-0.199981063398621,-4.92160218343307e-05,-7.11723735502545e-05,0.000488953318805718,-0.000169025109880705,0.199981075042255,0.000522216758155162 +0.880000000000001,0.559760544926879,-0.460373257181358,0.00431448568958322,-4.74168216080261e-05,-0.199980081334164,-4.95552511109442e-05,-7.20287024469356e-05,0.000493160200176756,-0.000170215312331188,0.19998009309553,0.000526657734744845 +0.882000000000001,0.559760449949185,-0.460773216357756,0.00431438623866157,-4.75617415451878e-05,-0.199979090757821,-4.98968830836554e-05,-7.28976046193707e-05,0.000497466200177765,-0.000171427987256943,0.199979102638608,0.000531200749146227 +0.884000000000001,0.559760354679913,-0.46117317354439,0.00431428610205088,-4.77084120265036e-05,-0.199978091469363,-5.0240963059972e-05,-7.37794825234061e-05,0.000501873348435211,-0.000172663504154069,0.199978103471295,0.000535847884737594 +0.886000000000001,0.559760259115537,-0.461573128723633,0.00431418527480933,-4.78568594752815e-05,-0.199977083264427,-5.05875371002717e-05,-7.46747663704638e-05,0.000506383723147818,-0.000173922239457558,0.199977095389261,0.000540601277448973 +0.888000000000001,0.559760163252475,-0.461973081877447,0.00431408375190248,-4.80071110919855e-05,-0.199976065934471,-5.09366520178022e-05,-7.55838724941781e-05,0.000510999399044864,-0.000175204550520447,0.199976078183999,0.000545463052945385 +0.890000000000001,0.559760067087092,-0.462373032987371,0.00431398152820126,-4.81591949652582e-05,-0.199975039266831,-5.12883553023535e-05,-7.65072449837589e-05,0.000515722544530694,-0.000176510802502028,0.199975051642884,0.000550435432064587 +0.892000000000001,0.559759970615695,-0.462772982034515,0.00431387859848127,-4.83131400719205e-05,-0.199974003044292,-5.16426952278104e-05,-7.74453418062037e-05,0.000520555442501402,-0.000177841407399126,0.199974015548736,0.000555520760972092 +0.894000000000001,0.559759873834532,-0.463172928999548,0.00431377495742034,-4.8468976332483e-05,-0.199972957045061,-5.199972093195e-05,-7.83986556229798e-05,0.000525500365444742,-0.000179196770269674,0.1999729696798,0.000560721379796713 +0.896000000000001,0.55975977673979,-0.463572873862695,0.00431367059959754,-4.86267346944124e-05,-0.199971901042831,-5.2359482308889e-05,-7.93675958288275e-05,0.000530559658706852,-0.000180577293569517,0.199971913809811,0.00056603968559428 +0.898000000000001,0.559759679327593,-0.46397281660372,0.00431356551949111,-4.87864467157983e-05,-0.199970834806426,-5.27220301062281e-05,-8.03526481463201e-05,0.000535735841106221,-0.000181983399703822,0.199970847707631,0.000571478256172252 +0.900000000000001,0.559759581594003,-0.464372757201921,0.00431345971147712,-4.89481452869977e-05,-0.199969758099466,-5.30874159077043e-05,-8.13544370759089e-05,0.000541031427991889,-0.000183415519751373,0.199969771136924,0.000577039689616361 +0.902000000000001,0.559759483535012,-0.464772695636118,0.00431335316982748,-4.91118644641019e-05,-0.199968670680714,-5.34556921852336e-05,-8.23733997679098e-05,0.00054644895206013,-0.000184874097801379,0.199968683856493,0.00058272657931041 +0.904000000000001,0.559759385146545,-0.465172631884644,0.00431324588870838,-4.92776388860694e-05,-0.199967572303658,-5.38269122989099e-05,-8.34100566393658e-05,0.000551991140296248,-0.000186359577943051,0.199967585619872,0.000588541713734249 +0.906000000000001,0.559759286424456,-0.465572565925332,0.00431313786217828,-4.94455046906594e-05,-0.199966462716153,-5.42011304970058e-05,-8.44650460685141e-05,0.000557660768257806,-0.00018787241467394,0.19996647617496,0.000594487948285463 +0.908000000000001,0.559759187364526,-0.465972497735508,0.00431302908418639,-4.96154990703434e-05,-0.199965341660585,-5.45784019576056e-05,-8.55389092890828e-05,0.000563460670482962,-0.000189413131013172,0.199965355264191,0.000600568202940922 +0.910000000000001,0.55975908796246,-0.466372427291975,0.00431291954857045,-4.97876603278157e-05,-0.199964208873471,-5.49587830210585e-05,-8.66322222292658e-05,0.000569393764776604,-0.000190982200540257,0.199964222624128,0.000606785466386642 +0.912000000000001,0.559758988213885,-0.466772354571002,0.00431280924905431,-4.99620279592605e-05,-0.199963064085526,-5.53423307597666e-05,-8.77456163284051e-05,0.000575463062618686,-0.000192580066477044,0.199963077985537,0.000613142815033848 +0.914000000000001,0.559758888114348,-0.467172279548317,0.00431269817924741,-5.01386427931294e-05,-0.19996190702122,-5.57291032869666e-05,-8.88797438425292e-05,0.000581671710797593,-0.000194207294343385,0.199961921072937,0.000619643495232452 +0.916000000000001,0.559758787659314,-0.467572202199087,0.00431258633264116,-5.03175469346306e-05,-0.199960737398683,-5.61191599371402e-05,-9.00352223331956e-05,0.000588022942837884,-0.000195864417566749,0.19996075160451,0.000626290821131106 +0.918000000000001,0.559758686844161,-0.467972122497911,0.00431247370260766,-5.04987836824621e-05,-0.199959554929449,-5.65125609572336e-05,-9.12127456897948e-05,0.000594520079000293,-0.000197551956564179,0.199959569291845,0.000633088196741276 +0.920000000000001,0.559758585664179,-0.468372040418805,0.00431236028239733,-5.06823979173898e-05,-0.199958359318367,-5.69093677633969e-05,-9.24130355572927e-05,0.000601166505465044,-0.000199270471651356,0.199958373839844,0.000640039106075653 +0.922000000000001,0.559758484114569,-0.468771955935185,0.00431224606513661,-5.08684358246913e-05,-0.199957150263427,-5.7309642843839e-05,-9.36367719472918e-05,0.000607965768006923,-0.000201020542225925,0.199957164946555,0.000647147184592317 +0.924000000000001,0.559758382190436,-0.469171869019858,0.00431213104382595,-5.10569450051789e-05,-0.199955927455295,-5.77134499323006e-05,-9.48847181381217e-05,0.000614921544239699,-0.000202802730338292,0.199955942302701,0.000654416199936212 +0.926000000000001,0.559758279886789,-0.469571779645006,0.00431201521133688,-5.12479746972438e-05,-0.19995469057725,-5.81208537651922e-05,-9.61576443470057e-05,0.000622037598513314,-0.000204617627529158,0.199954705591622,0.000661850013240339 +0.928000000000001,0.559758177198537,-0.469971687782167,0.00431189856041089,-5.1441575582567e-05,-0.199953439304901,-5.85319204424172e-05,-9.74563416078488e-05,0.000629317799261119,-0.000206465890391361,0.199953454488988,0.000669452608391021 +0.930000000000001,0.559758074120487,-0.470371593402226,0.00431178108365511,-5.16378000636752e-05,-0.199952173306053,-5.89467173267577e-05,-9.87816634046012e-05,0.000636766160633239,-0.000208348126945479,0.19995218866267,0.000677228102159284 +0.932000000000001,0.559757970647337,-0.470771496475392,0.00431166277354159,-5.18367022361854e-05,-0.199950892240258,-5.93653129501991e-05,-0.000100134428526743,0.000644386845966016,-0.000210264971232943,0.199950907772281,0.000685180756558441 +0.934000000000001,0.559757866773678,-0.471171396971187,0.00431154362240331,-5.20383377777822e-05,-0.199949595758669,-5.97877772116894e-05,-0.000101515490458226,0.000652184115740306,-0.000212217098928546,0.199949611469045,0.000693314944815549 +0.936000000000001,0.559757762493986,-0.471571294858426,0.00431142362243274,-5.22427641980183e-05,-0.199948283503795,-6.02141813459133e-05,-0.000102925792888619,0.000660162424725996,-0.000214205214330021,0.199948299395539,0.000701635246911119 +0.938000000000001,0.559757657802621,-0.471971190105202,0.00431130276567793,-5.24500409493366e-05,-0.19994695510897,-6.06445980690095e-05,-0.000104366272568601,0.000668326380348638,-0.000216230014796204,0.199946971185169,0.000710146385485797 +0.940000000000001,0.559757552693822,-0.472371082678862,0.00431118104404046,-5.26602292882927e-05,-0.199945610198274,-6.10791014050981e-05,-0.000105837907882211,0.000676680638606041,-0.000218292190747038,0.199945626462087,0.000718853135174567 +0.942000000000001,0.559757447161704,-0.472770972545995,0.00431105844927231,-5.28733925808655e-05,-0.199944248386416,-6.15177668319977e-05,-0.000107341698030172,0.00068523006713228,-0.000220392495919874,0.19994426484108,0.000727760494459476 +0.944000000000001,0.559757341200251,-0.473170859672408,0.00431093497297313,-5.30895960804134e-05,-0.199942869278005,-6.19606713887776e-05,-0.000108878663029888,0.000693979800708843,-0.000222531699664574,0.199942885926835,0.000736873723517582 +0.946000000000001,0.559757234803319,-0.473570744023107,0.00431081060658675,-5.3308907232985e-05,-0.199941472467213,-6.2407893630656e-05,-0.000110449906165488,0.000702935040036711,-0.000224710578269893,0.199941489313604,0.00074619816152934 +0.948000000000001,0.559757127964623,-0.473970625562277,0.00431068534139861,-5.35313957050753e-05,-0.199940057537845,-6.28595137018572e-05,-0.000112056516843317,0.000712101159289213,-0.00022692992970863,0.199940074585276,0.000755739318168415 +0.950000000000001,0.559757020677736,-0.474370504253258,0.00431055916853194,-5.37571333003583e-05,-0.199938624062576,-6.33156133494905e-05,-0.00011369969549202,0.000721483775500963,-0.000229190587515415,0.19993864131461,0.000765502961765507 +0.952000000000001,0.559756912936089,-0.474770380058527,0.00431043207894521,-5.39861944870434e-05,-0.199937171602743,-6.37762760519189e-05,-0.00011538062172356,0.000731088637545562,-0.000231493406041561,0.199937189063034,0.000775494990866004 +0.954000000000001,0.559756804732959,-0.475170252939669,0.00431030406342774,-5.42186557872526e-05,-0.199935699708026,-6.42415869736567e-05,-0.000117100468211007,0.000740921757974577,-0.000233839238771016,0.199935717380317,0.000785721554168131 +0.956000000000001,0.559756696061466,-0.475570122857359,0.00431017511259732,-5.44545963598874e-05,-0.199934207915711,-6.47116330070029e-05,-0.000118860601916459,0.000750989354036945,-0.000236228997300969,0.199934225803841,0.000796189042709451 +0.958000000000001,0.559756586914573,-0.475969989771332,0.00431004521689571,-5.46940981949184e-05,-0.19993269575061,-6.51865029628606e-05,-0.000120662362046442,0.000761297806045613,-0.000238663606239031,0.199932713858514,0.000806904004233161 +0.960000000000001,0.559756477285073,-0.476369853640362,0.00430991436658547,-5.49372458080732e-05,-0.199931162724487,-6.56662874319591e-05,-0.000122507032296326,0.000771853754522044,-0.000241143996264348,0.199931181056201,0.00081787322872568 +0.962000000000001,0.55975636716559,-0.47676971442223,0.00430978255174598,-5.51841263241037e-05,-0.199929608335592,-6.6151078947918e-05,-0.000124396090650514,0.00078266408631844,-0.000243671152699853,0.199929626895256,0.000829103787254578 +0.964000000000001,0.559756256548568,-0.477169572073704,0.00430964976226968,-5.54348301706753e-05,-0.199928032068142,-6.66409720427585e-05,-0.000126331091421239,0.00079373592767884,-0.00024624607474627,0.199928050860002,0.000840602995995027 +0.966000000000001,0.559756145426269,-0.477569426550503,0.00430951598785781,-5.56894506897887e-05,-0.199926433391881,-6.71360632469031e-05,-0.000128313512592903,0.000805076675464144,-0.000248869808441854,0.199926452420294,0.000852378432648441 +0.968000000000001,0.559756033790765,-0.477969277807271,0.00430938121801669,-5.59480842210469e-05,-0.19992481176144,-6.76364512765259e-05,-0.000130344915416636,0.00081669399368267,-0.000251543418039457,0.199924831030877,0.00086443794887273 +0.970000000000001,0.559755921633932,-0.478369125797548,0.0043092454420527,-5.62108303514552e-05,-0.199923166615906,-6.81422369190609e-05,-0.000132426972165867,0.000828595782265128,-0.000254267970394017,0.199923186130957,0.00087678964074222 +0.972000000000001,0.559755808947444,-0.478768970473735,0.00430910864906901,-5.64777921097103e-05,-0.199921497378311,-6.8653523158102e-05,-0.000134561452258541,0.000840790295025816,-0.000257044615627196,0.199921517143686,0.000889441981843931 +0.974000000000001,0.559755695722764,-0.479168811787062,0.00430897082796007,-5.67490761604894e-05,-0.199919803454726,-6.91704153815697e-05,-0.000136750125112606,0.000853286049457002,-0.000259874522942623,0.199919823475263,0.000902403704885406 +0.976000000000001,0.559755581951139,-0.479568649687554,0.00430883196740749,-5.70247926101608e-05,-0.199918084234113,-6.96930212498725e-05,-0.000138994801779367,0.000866091868362283,-0.000262758841594601,0.199918104514779,0.00091568383637645 +0.978000000000001,0.559755467623593,-0.479968484123998,0.00430869205587507,-5.73050553676068e-05,-0.199916339087253,-7.02214507479481e-05,-0.000141297445965803,0.000879217011695576,-0.000265698811910408,0.19991635963315,0.000929291870480938 +0.980000000000001,0.559755352730918,-0.480368315043903,0.00430855108160449,-5.7589982394024e-05,-0.199914567366066,-7.07558164975141e-05,-0.00014366004219557,0.000892670985741527,-0.000268695758351422,0.199914588182436,0.00094323756661004 +0.982000000000001,0.559755237263664,-0.480768142393462,0.00430840903260908,-5.78796955363891e-05,-0.19991276840331,-7.12962337813538e-05,-0.000146084679075731,0.000906463640260035,-0.000271750942928972,0.199912789495538,0.00095753101232123 +0.984000000000001,0.559755121212136,-0.481167966117516,0.00430826589666937,-5.81743211103269e-05,-0.199910941511505,-7.184282026923e-05,-0.000148573542357866,0.000920605289916886,-0.000274865666685664,0.199910962885128,0.000972182766785275 +0.986000000000001,0.559755004566379,-0.481567786159508,0.004308121661328,-5.84739897058206e-05,-0.19990908598215,-7.23956964480964e-05,-0.000151128817793555,0.000935106603261459,-0.000278041372044069,0.199909107642856,0.000987203770049025 +0.988000000000001,0.559754887316177,-0.481967602461445,0.00430797631388358,-5.87788363815011e-05,-0.199907201085092,-7.29549857574063e-05,-0.000153752885423408,0.000949978668646211,-0.000281279506630927,0.199907223038731,0.00100260539670869 +0.990000000000001,0.559754769451034,-0.482367414963849,0.00430782984138497,-5.90890012475142e-05,-0.199905286067475,-7.35208144746202e-05,-0.000156448187738078,0.000965233028921152,-0.000284581482511148,0.199905308320063,0.0010183994578529 +0.992000000000001,0.559754650960172,-0.482767223605715,0.00430768223062568,-5.94046291324534e-05,-0.199903340152976,-7.40933116874509e-05,-0.000159217243556053,0.000980881629392138,-0.000287948817567773,0.199903362710699,0.001034598194017 +0.994000000000001,0.559754531832517,-0.483167028324461,0.00430753346813822,-5.97258702217384e-05,-0.199901362540958,-7.46726097448912e-05,-0.000162062682718122,0.000996936925373725,-0.000291383155451297,0.199901385410181,0.00105121438802186 +0.996000000000001,0.559754412056691,-0.483566829055879,0.0043073835401867,-6.00528798633259e-05,-0.199899352405275,-7.5258844309256e-05,-0.000164987259965584,0.00101341188427084,-0.000294886048739754,0.199899375592547,0.00106826130926552 +0.998000000000001,0.559754291620998,-0.483966625734082,0.00430723243276099,-6.03858192616008e-05,-0.199897308893421,-7.58521539398503e-05,-9.75381789229743e-05,0.0260671649633747,-0.000294682573482293,0.199897332405483,0.0260690130373831 +1,0.559754170513414,-0.484366418291452,0.00430708013157094,-6.04430325790178e-05,-0.199795083745421,-7.64375746031852e-05,0.000181329479210103,0.126295022618437,-0.000281883816635224,0.199795107509926,0.12629546736545 +1.002,0.559754049848868,-0.484765806069063,0.00430692668246258,-5.96605013447604e-05,-0.199392128802947,-7.69796892063911e-05,0.00067081605548178,0.301754251157131,-0.000253131304978916,0.199392152588358,0.301755102957744 +1.004,0.559753931871409,-0.485163986806664,0.00430677221281411,-5.77597683570907e-05,-0.198588066740793,-7.74500998231008e-05,0.00122716373174292,0.502296634473375,-0.0002133393834855,0.198588090243485,0.502298178821899 +1.006,0.559753818809794,-0.485560158336027,0.00430661688206328,-5.47518464177887e-05,-0.197382942265053,-7.78330467403331e-05,0.00177630789938154,0.702817390535459,-0.00016583350925109,0.197382965204592,0.702819654825886 +1.008,0.559753712864023,-0.485953518575724,0.00430646088062715,-5.06545367595645e-05,-0.195776797178651,-7.81134338601052e-05,0.00231482606000144,0.903318120141355,-0.000111007428943433,0.195776819315059,0.903321092922139 +1.01,0.559753616191647,-0.486343265524741,0.00430630442832784,-4.54925421777829e-05,-0.193769669784488,-7.82770764561069e-05,0.00283923853872103,1.1038007291797,-4.93791275230082e-05,0.193769690935558,1.10380438187725 +1.012,0.559753530893854,-0.486728597254862,0.00430614777232133,-3.92975826046804e-05,-0.191361594261932,-7.83109503701972e-05,0.00334600738782775,1.30426744048007,1.84088959354067e-05,0.191361614320567,1.30427173257751 +1.014,0.559753459001317,-0.487108711901789,0.00430599118452636,-3.21085126264719e-05,-0.188552600022568,-7.82034408723652e-05,0.00383153592187212,1.50472080259756,9.15904383586018e-05,0.188552618974135,1.50472568358029 +1.016,0.559753402459804,-0.487482807654952,0.00430583495855784,-2.39714389171919e-05,-0.185342711051542,-7.79445886167628e-05,0.00429216920339037,1.70516369543336,0.00016927770209868,0.185342728991251,1.70516910586468 +1.018,0.559753363115561,-0.487850082745995,0.00430567940617189,-1.49398358129105e-05,-0.181731945240834,-7.75263300639705e-05,0.00472419539598866,1.90559933168841,0.000250465028925339,0.181731962391177,1.90560520404573 +1.02,0.55975334270046,-0.488209735435916,0.00430552485323758,-5.07465733323719e-06,-0.177720313724788,-7.69427285010615e-05,0.00512384813744492,2.10603125340783,0.000334034139758132,0.177720330453139,2.10603751289693 +1.022,0.559753342816932,-0.488560964000894,0.00430537163525789,5.55555673686924e-06,-0.173307820227203,-7.6190193504938e-05,0.00548731003691127,2.30646332358488,0.000418761001569345,0.173307837063747,2.30646988902401 +1.024,0.559753364922687,-0.488902966716825,0.00430522009246356,1.68745828144079e-05,-0.168494460430449,-7.52676844947841e-05,0.00581071746275085,2.50689971174541,0.000503324711770281,0.168494478086743,2.50690649656552 +1.026,0.559753410315263,-0.489234941842616,0.00430507056451991,2.87984265878727e-05,-0.163280221380221,-7.41768946578569e-05,0.00609016669039785,2.70734487282237,0.000586318185842593,0.163280240768855,2.70735178621045 +1.028,0.559753480116394,-0.489556087602345,0.00430492338488493,4.12352495759993e-05,-0.157665080939159,-7.29224117514137e-05,0.00632172156289633,2.90780351892145,0.000666260911025284,0.157665103195271,2.90781046712454 +1.03,0.559753575256261,-0.489865602166373,0.00430477887487291,5.4085312839458e-05,-0.151649007304536,-7.15118510137557e-05,0.00650142268493469,3.10828058345397,0.000741613910906768,0.151649033810364,3.10828747125568 +1.032,0.559753696457645,-0.490162683631564,0.00430463733748088,6.72409403157381e-05,-0.145231958605343,-6.99559561077866e-05,0.00662529832384794,3.3087811770957,0.000810796836787915,0.145231991019624,3.30878790947262 +1.034,0.559753844220023,-0.490446530000794,0.00430449905104847,8.05865061348498e-05,-0.138413882596153,-6.82686636666041e-05,0.00668937710779359,3.50931053501312,0.000872207243929331,0.138413922891221,3.50931701897473 +1.036,0.55975401880367,-0.490716339161948,0.00430436426282621,9.39984487469125e-05,-0.131194716465291,-6.64671271320693e-05,0.0066897025280399,3.70987395546113,0.000924242325901769,0.131194766976413,3.70988010207238 +1.038,0.559754220213817,-0.490971308866655,0.00430423318253995,0.000107345316247009,-0.123574386774308,-6.4571694362997e-05,0.00662234942577776,3.91047672951977,0.000965322802595757,0.12357445026852,3.91048245610125 +1.04,0.559754448184935,-0.491210636709045,0.00430410597604876,0.000120487846450024,-0.115552809547212,-6.26058359216863e-05,0.00648344198467265,4.1111240619146,0.000993918799693793,0.115552889323747,4.11112929440899 +1.042,0.559754702165203,-0.491433520104844,0.00430398275919626,0.0001332790841857,-0.10712989052665,-6.05960191642218e-05,0.00624049414921223,4.29136721119549,0.00100479510649343,0.107129990569606,4.29137186628055 +1.044,0.559754981301271,-0.491639156271152,0.0043038635919721,0.000145449823046872,-0.0983873407024299,-5.85866554957126e-05,0.00577625879072395,4.35743215300085,0.000981257125901022,0.0983874656576905,4.3574360920198 +1.046,0.559755283964495,-0.491827069467654,0.00430374841257428,0.000156384119348596,-0.0897001619146464,-5.66709906606177e-05,0.00506162380065067,4.24988742290382,0.000917792755857069,0.0897003161371672,4.24989053620621 +1.048,0.559755606837749,-0.491997956918811,0.00430363690800946,0.000165696318249475,-0.0813877910108145,-5.49154844722843e-05,0.00427815895209792,4.0559296413424,0.000837487427497729,0.0813879782068601,4.05593198408814 +1.05,0.559755946749768,-0.492152620631697,0.00430352875063639,0.000173496755156988,-0.0734764433492768,-5.33210409506268e-05,0.00355842529053961,3.85540775485817,0.000758583530192924,0.0734766675313799,3.85540947164631 +1.052,0.559756300824769,-0.492291862692208,0.00430342362384566,0.000179930019411634,-0.0659661599913818,-5.18811503515126e-05,0.00290938274860597,3.6548646445908,0.000683378000021494,0.0659664257821676,3.65486586645711 +1.054,0.559756666469846,-0.492416485271663,0.00430332122603498,0.000185134286151411,-0.0588569847709136,-5.05875289505408e-05,0.0023277691788004,3.45429918097855,0.000612655298021234,0.0588572976794343,3.45430001962264 +1.056,0.559757041361914,-0.492527290631291,0.00430322127372985,0.000189241096126835,-0.0521489632674676,-4.94305291594277e-05,0.00181021766326728,3.25371073004246,0.000547003629891572,0.0521493300574548,3.25371127958434 +1.058,0.559757423434231,-0.492625081124733,0.00430312350391834,0.000192375156804481,-0.0458421418507437,-4.83995144309745e-05,0.00135326748418318,3.05309911697443,0.000486832549967397,0.0458425710467646,3.05309945570216 +1.06,0.559757810862541,-0.492710659198694,0.00430302767567213,0.000194654166063568,-0.0399365667995699,-4.74831989595581e-05,0.000953375683954149,2.85246458754662,0.000432390544376215,0.0399370694046904,2.85246477964133 +1.062,0.559758202050895,-0.492784827391931,0.00430293357112251,0.000196188659540297,-0.0344322835005572,-4.66699522534697e-05,0.000606928229895942,2.6518077677097,0.000383782428579893,0.034432874047011,2.65180786493599 +1.064,0.559758595617179,-0.492848388332697,0.00430284099586312,0.000197081878983152,-0.0293293357287311,-4.59480692452385e-05,0.000310251019319807,2.45112962244872,0.000340986208019478,0.0293300338694095,2.45112966580165 +1.066,0.559758990378411,-0.492902144734846,0.00430274977884552,0.000197429663617576,-0.0246277650107623,-4.53060074213918e-05,5.9621058090542e-05,2.25043141449318,0.000303869450435368,0.0246285980220498,2.25043143579828 +1.068,0.559759385335834,-0.49294689939274,0.00430265977183343,0.000197320363215514,-0.0203276100707583,-4.4732591443497e-05,-0.000148723131032646,2.04971466385983,0.000272204965165445,0.0203286169603166,2.04971468732996 +1.07,0.559759779659864,-0.492983455175129,0.00430257084847975,0.000196834771093446,-0.016428906355323,-4.421718756073e-05,-0.000318571523327815,1.84898110879364,0.000245685500589732,0.0164301449512401,1.84898115256076 +1.072,0.559760172674918,-0.493012615018161,0.00430248290308319,0.000196046077122203,-0.0129316856355838,-4.37498494411381e-05,-0.000453733078886742,1.64823266827346,0.000223937565539678,0.0129332455899996,1.64823274593899 +1.074,0.559760563844172,-0.493035181917671,0.00430239584908199,0.000195019838777899,-0.00983597568222915,-4.33214372985713e-05,-0.000558026971853031,1.44747140696025,0.000206534150025417,0.00983800422371579,1.44747152925988 +1.076,0.559760952754273,-0.49305195892089,0.00430230961733399,0.00019381396923479,-0.00714180000774278,-4.29237128410364e-05,-0.000635274652327488,1.24669950317749,0.000193006311453858,0.00714455832437453,1.24669967997442 +1.078,0.559761339100049,-0.493063749117702,0.00430222415423062,0.000192478740168589,-0.0048491776695192,-4.25494120527558e-05,-0.000689292685429609,1.0459192197193,0.0001828534910292,0.0048531827277001,1.04591946283547 +1.08,0.559761722669234,-0.493071355631568,0.00430213941968578,0.000191056798493072,-0.00295812312886556,-4.21922988769196e-05,-0.000723886298126252,0.845132877864278,0.000175552617581729,0.00296458687439993,0.845133206114427 +1.082,0.559762103327243,-0.493075581610218,0.00430205538503511,0.000189583194976084,-0.00146864615806208,-4.18472015824289e-05,-0.000742844091794481,0.644342835410399,0.000170565927701259,0.00148142313796165,0.644343286187411 +1.084,0.559762481002014,-0.4930772302162,0.00430197203087945,0.000188085422125894,-0.000380751787223965,-4.15100351661146e-05,-0.00074993360826836,0.443551468075342,0.000167347397224404,0.00042669794059734,0.443552133618315 +1.086,0.559762855668932,-0.493077104617366,0.00430188934489445,0.00018658346054301,0.000305559714239289,-4.11778119935313e-05,-0.000718740893283561,0.242690604851669,0.000158709063567876,0.000360382767227813,0.242691721037837 +1.088,0.559763227335856,-0.493076007977343,0.00430180731963148,0.00018521045855276,0.00059001063218271,-4.08751989118431e-05,-0.000649676609300961,0.0994113711961051,0.00014323907698958,0.000619746917646266,0.0994135972602322 +1.08944271909999,0.559763493296515,-0.493075073376494,0.00430174862306644,0.000184346806471353,0.000647805140612399,-4.06846800873142e-05,-0.000598628022192733,0.0400594325188151,0.000132055383844362,0.000674752168172762,0.0400641227017747 diff --git a/protos/cmvr/api/arm_command.proto b/protos/cmvr/api/arm_command.proto new file mode 100644 index 00000000..3c0d7b99 --- /dev/null +++ b/protos/cmvr/api/arm_command.proto @@ -0,0 +1,185 @@ +syntax = "proto3"; + +package cmvr.api; + +import "cmvr/api/common.proto"; + +enum ArmFrameType { + ARM_FRAME_BASE = 0; + ARM_FRAME_TOOL = 1; + ARM_FRAME_WORLD = 2; + ARM_FRAME_USER = 3; +} + +message JointPositionCommand { + repeated double position = 1; +} + +message JointVelocityCommand { + repeated double velocity = 1; +} + +message MotionOptions { + double velocity = 1; + double acceleration = 2; + double blend_radius = 3; + double jerk = 4; + repeated double joint_velocity_limits = 5; + bool asynchronous = 6; +} + +message CartesianPose { + double x = 1; + double y = 2; + double z = 3; + double rx = 4; + double ry = 5; + double rz = 6; +} + +message CartesianVelocity { + double vx = 1; + double vy = 2; + double vz = 3; + double wx = 4; + double wy = 5; + double wz = 6; +} + +message TransformMatrix4x4 { + double m00 = 1; double m01 = 2; double m02 = 3; double m03 = 4; + double m10 = 5; double m11 = 6; double m12 = 7; double m13 = 8; + double m20 = 9; double m21 = 10; double m22 = 11; double m23 = 12; + double m30 = 13; double m31 = 14; double m32 = 15; double m33 = 16; +} + +message MoveJ { + message Request { + CommandHeader.Request header = 1; + JointPositionCommand target = 2; + MotionOptions options = 3; + } + + message Response { + CommandHeader.Feedback header = 1; + } +} + +message MoveL { + message Request { + CommandHeader.Request header = 1; + CartesianPose target = 2; + MotionOptions options = 3; + ArmFrameType frame = 4; + } + + message Response { + CommandHeader.Feedback header = 1; + } +} + +message SpeedJ { + message Request { + CommandHeader.Request header = 1; + JointVelocityCommand velocity = 2; + double acceleration = 3; + double duration = 4; + } + + message Response { + CommandHeader.Feedback header = 1; + } +} + +message SpeedL { + message Request { + CommandHeader.Request header = 1; + CartesianVelocity velocity = 2; + double acceleration = 3; + double duration = 4; + ArmFrameType frame = 5; + } + + message Response { + CommandHeader.Feedback header = 1; + } +} + +message ServoJ { + message Request { + CommandHeader.Request header = 1; + JointPositionCommand target = 2; + } + + message Response { + CommandHeader.Feedback header = 1; + } +} + +message JointState { + repeated string name = 1; + repeated double position = 2; + repeated double velocity = 3; + repeated double effort = 4; + double timestamp = 5; +} + +message JointRequest { + CommandHeader.Request header = 1; +} + +message JointResponse { + CommandHeader.Feedback header = 1; + JointState state = 2; +} + +message GetPose { + message Request { + CommandHeader.Request header = 1; + string base_link = 2; + string ee_link = 3; + } + + message Response { + CommandHeader.Feedback header = 1; + CartesianPose pose = 2; + } +} + +message CalibrateZeroQ { + message Request { + CommandHeader.Request header = 1; + string joint_name = 2; + } + + message Response { + CommandHeader.Feedback header = 1; + } +} + +message GetPoseMatrix { + message Request { + CommandHeader.Request header = 1; + string base_link = 2; + string ee_link = 3; + } + + message Response { + CommandHeader.Feedback header = 1; + TransformMatrix4x4 matrix = 2; + } +} + +message ComputeForwardKinematics { + message Request { + CommandHeader.Request header = 1; + string base_link = 2; + string ee_link = 3; + JointPositionCommand joints = 4; + } + + message Response { + CommandHeader.Feedback header = 1; + TransformMatrix4x4 matrix = 2; + } +} diff --git a/protos/cmvr/api/humanoid_robot_service.proto b/protos/cmvr/api/arm_service.proto similarity index 86% rename from protos/cmvr/api/humanoid_robot_service.proto rename to protos/cmvr/api/arm_service.proto index b046eb92..565b4939 100644 --- a/protos/cmvr/api/humanoid_robot_service.proto +++ b/protos/cmvr/api/arm_service.proto @@ -1,22 +1,22 @@ - - syntax = "proto3"; -package cmvr.api; -import "cmvr/api/common.proto"; -import "cmvr/api/humanoid_robot_command.proto"; -service HumanoidRobotService{ +package cmvr.api; + +import "cmvr/api/common.proto"; +import "cmvr/api/arm_command.proto"; + +service ArmService { rpc torqueOff(CommandHeader.Request) returns (CommandHeader.Feedback); rpc torqueOn(CommandHeader.Request) returns (CommandHeader.Feedback); rpc moveJ(MoveJ.Request) returns (MoveJ.Response); rpc moveL(MoveL.Request) returns (MoveL.Response); rpc speedJ(SpeedJ.Request) returns (SpeedJ.Response); rpc speedL(SpeedL.Request) returns (SpeedL.Response); + rpc servoJ(ServoJ.Request) returns (ServoJ.Response); + rpc stopMotion(CommandHeader.Request) returns (CommandHeader.Feedback); rpc getJointState(JointRequest) returns (JointResponse); rpc getPose(GetPose.Request) returns (GetPose.Response); rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response); - rpc servoJ(ServoJ.Request) returns (ServoJ.Response); - rpc getPoseMatrix(GetPoseMatrix.Request) returns (GetPoseMatrix.Response); rpc computeForwardKinematics(ComputeForwardKinematics.Request) returns (ComputeForwardKinematics.Response); } diff --git a/protos/cmvr/api/humanoid_robot_command.proto b/protos/cmvr/api/humanoid_robot_command.proto deleted file mode 100644 index 75804fb1..00000000 --- a/protos/cmvr/api/humanoid_robot_command.proto +++ /dev/null @@ -1,202 +0,0 @@ - - -syntax = "proto3"; -package cmvr.api; -import "cmvr/api/common.proto"; - -message JointCmd { - string joint_name = 1; // 关节名称 - double rad = 2; // 弧度 - double vel = 3; // 角速度，rad/s -} - -message Pose3D{ - double x = 1; - double y = 2; - double z = 3; - double rx = 4; - double ry = 5; - double rz = 6; -} - -enum RobotCartesian{ - X = 0; - Y = 1; - Z = 2; - RX = 3; - RY = 4; - RZ = 5; -} ; -enum RobotJointIndexDirection{ - FORWARD = 0; - BACKWARD = 1; - X_POSITIVE = 2; // X轴正向 - X_NEGATIVE = 3; // X轴负向 - Y_POSITIVE = 4; // Y轴正向 - Y_NEGATIVE = 5; // Y轴负向 - Z_POSITIVE = 6; // Z轴正向 - Z_NEGATIVE = 7; // Z轴负向 - ROTATE_X = 8; // 绕X轴旋转 - ROTATE_Y = 9; // 绕Y轴旋转 - ROTATE_Z = 10; // 绕Z轴旋转 -} - - -// 4x4 齐次变换矩阵 -message TransformMatrix4x4 { - double m00 = 1; double m01 = 2; double m02 = 3; double m03 = 4; - double m10 = 5; double m11 = 6; double m12 = 7; double m13 = 8; - double m20 = 9; double m21 = 10; double m22 = 11; double m23 = 12; - double m30 = 13; double m31 = 14; double m32 = 15; double m33 = 16; -} - - -message MoveJ{ - message Request{ - CommandHeader.Request header = 1; - repeated JointCmd cmds = 2; - double vel = 3; - double acc = 4; - } - - message Response{ - CommandHeader.Feedback header= 1; - } - -} - -message GetPose{ - message Request{ - CommandHeader.Request header = 1; - string base_link = 2; - string ee_link = 3; - } - - message Response{ - CommandHeader.Feedback header= 1; - Pose3D pose = 2; - } -} - -message MoveL{ - - message Request{ - CommandHeader.Request header = 1; - string ee_link = 2; //连杆名称 - Pose3D targetPose = 3; - double vel = 4; - double acc = 5; - } - - message Response{ - CommandHeader.Feedback header= 1; - } - -} - -message SpeedJ{ - - message Request{ - CommandHeader.Request header = 1; - string joint_name = 2; - double vel = 3; - double acc = 4; - RobotJointIndexDirection dir = 5; - } - - message Response{ - CommandHeader.Feedback header= 1; - } -} - -message SpeedL{ - message Request{ - CommandHeader.Request header = 1; - string ee_link = 2; - double vel = 3; - double acc = 4; - RobotJointIndexDirection dir = 5; - RobotCartesian cart = 6; - } - - message Response{ - CommandHeader.Feedback header= 1; - } -} - -// 关节状态消息 -message JointState { - repeated string name = 1; // 关节名称 - repeated double position = 2; // 关节位置 - repeated double velocity = 3; // 可选：关节速度 - repeated double effort = 4; // 可选：关节力矩 - double timestamp = 5; // 可选：时间戳 -} - -// 返回消息 -message JointResponse { - CommandHeader.Feedback header= 1; - repeated JointState state = 2; -} - -// 请求消息（可选） -message JointRequest { - CommandHeader.Request header = 1; -} - - -message CalibrateZeroQ { - message Request{ - CommandHeader.Request header = 1; - string joint_name = 2; - } - - message Response{ - CommandHeader.Feedback header= 1; - } -} - - -message ServoJ{ - message Request{ - CommandHeader.Request header = 1; - repeated JointCmd cmds = 2; - double vel = 3; - } - - message Response{ - CommandHeader.Feedback header= 1; - } - -} - - - -// 获取当前位姿的4x4矩阵 -message GetPoseMatrix { - message Request { - CommandHeader.Request header = 1; - string base_link = 2; // 基坐标系 - string ee_link = 3; // 末端连杆名称 - } - - message Response { - CommandHeader.Feedback header = 1; - TransformMatrix4x4 matrix = 2; // 4x4 位姿矩阵 - } -} - -// 根据关节角度计算正运动学位姿矩阵 -message ComputeForwardKinematics { - message Request { - CommandHeader.Request header = 1; - string base_link = 2; // 基坐标系 - string ee_link = 3; // 末端连杆名称 - repeated JointCmd joints = 4; // 关节角度 - } - - message Response { - CommandHeader.Feedback header = 1; - TransformMatrix4x4 matrix = 2; // 计算出的4x4位姿矩阵 - } -} \ No newline at end of file diff --git a/protos/cmvr/common/geometry.proto b/protos/cmvr/common/geometry.proto new file mode 100644 index 00000000..729596fe --- /dev/null +++ b/protos/cmvr/common/geometry.proto @@ -0,0 +1,79 @@ +syntax = "proto3"; + +package cmvr.common; + +message Vec2 { + optional double x = 1; + optional double y = 2; +} + +message Vec3 { + optional double x = 1; + optional double y = 2; + optional double z = 3; +} + +message Vec6 { + optional double x = 1; + optional double y = 2; + optional double z = 3; + optional double rx = 4; + optional double ry = 5; + optional double rz = 6; +} + +message Quat { + optional double w = 1; + optional double x = 2; + optional double y = 3; + optional double z = 4; +} + +message Euler { + optional double rx = 1; + optional double ry = 2; + optional double rz = 3; +} + +message Pose3d { + Vec3 position = 1; + Quat quaternion = 2; + Euler euler = 3; +} + +message Pose2d { + optional double x = 1; + optional double y = 2; + optional double theta = 3; +} + +message Mat3 { + optional double m00 = 1; + optional double m01 = 2; + optional double m02 = 3; + optional double m10 = 4; + optional double m11 = 5; + optional double m12 = 6; + optional double m20 = 7; + optional double m21 = 8; + optional double m22 = 9; +} + +message Mat4 { + optional double m00 = 1; + optional double m01 = 2; + optional double m02 = 3; + optional double m03 = 4; + optional double m10 = 5; + optional double m11 = 6; + optional double m12 = 7; + optional double m13 = 8; + optional double m20 = 9; + optional double m21 = 10; + optional double m22 = 11; + optional double m23 = 12; + optional double m30 = 13; + optional double m31 = 14; + optional double m32 = 15; + optional double m33 = 16; +} diff --git a/protos/cmvr/config/agv_config/agv_config.proto b/protos/cmvr/config/agv_config/agv_config.proto new file mode 100644 index 00000000..004e2155 --- /dev/null +++ b/protos/cmvr/config/agv_config/agv_config.proto @@ -0,0 +1,24 @@ +syntax = "proto3"; +package cmvr.config; + +message MyAgvConfig { + string id = 1; + string ip = 2; + int32 port = 3; +} + +message AGVDeviceConfig { + string id = 1; + + oneof backend { + MyAgvConfig my_agv = 10; + } +} + +message AGVConfig { + repeated AGVDeviceConfig agvs = 1; +} + +message AGVRootConfig { + AGVConfig agv = 1; +} diff --git a/protos/cmvr/config/arm_config/arm_config.proto b/protos/cmvr/config/arm_config/arm_config.proto new file mode 100644 index 00000000..f69fae94 --- /dev/null +++ b/protos/cmvr/config/arm_config/arm_config.proto @@ -0,0 +1,168 @@ +syntax = "proto3"; +package cmvr.config; + +import "cmvr/common/geometry.proto"; +import "cmvr/config/joint_limits_config.proto"; +import "cmvr/config/lawba_ik_config.proto"; +import "cmvr/config/pinocchio_dls_ik_config.proto"; +import "cmvr/config/pinocchio_qp_ik_config.proto"; +import "cmvr/config/srs_ik_config.proto"; + +enum ToppraPathType { + TOPPRA_PATH_TYPE_UNKNOWN = 0; + TOPPRA_PATH_TYPE_LINEAR = 1; + TOPPRA_PATH_TYPE_CUBIC_HERMITE = 2; + TOPPRA_PATH_TYPE_QUINTIC = 3; + TOPPRA_PATH_TYPE_NATURAL = 4; +} + +message MotorRobotArmBackendConfig { + string motor_system_id = 2; + int32 dof = 3; + repeated string joint_names = 4; + int32 upd_freq = 5; + int32 buffer_size = 6; + double default_vel = 7; + double default_acc = 8; + repeated string motor_group_ids = 9; +} + +enum VendorRobotArmBrand { + VENDOR_ROBOT_ARM_BRAND_UNKNOWN = 0; + VENDOR_ROBOT_ARM_BRAND_AUBO_ARM = 1; +} + +message VendorRobotArmBackendConfig { + VendorRobotArmBrand brand = 1; + string ip = 2; + int32 port = 3; + string model = 4; + int32 dof = 5; + repeated string joint_names = 6; + string base_frame = 7; + string tool_frame = 8; + string username = 9; + string password = 10; +} + +message QpJointLimitAvoidanceConfig { + bool enable = 1; + double margin_ratio = 2; + double gain = 3; + double max_push = 4; + double weight = 5; +} + +message CartesianVelocityQpConfig { + JointLimitsConfig joint_limits = 1; + .cmvr.common.Vec6 twist_tracking_weight = 2; + double qdot_regularization = 3; + double prev_qdot_regularization = 4; + double solver_eps = 5; + QpJointLimitAvoidanceConfig joint_limit_avoidance = 6; +} + +message SpeedLPlannerConfig { + double linear_velocity_max = 1; + double linear_acceleration_max = 2; + double linear_jerk_max = 3; + double angular_velocity_max = 4; + double angular_acceleration_max = 5; + double angular_jerk_max = 6; + repeated double joint_acceleration_max = 7; + double linear_target_replan_threshold = 8; + double angular_target_replan_threshold = 9; + double linear_reverse_cos_threshold = 10; + double linear_reverse_switch_speed_threshold = 11; + double normal_direction_deviation_deg = 12; + double mild_direction_deviation_deg = 13; + double severe_direction_deviation_deg = 14; + double linear_min_speed_ratio = 15; + CartesianVelocityQpConfig qp = 16; +} + +message CartesianVelocityControllerConfig { + double control_period_s = 1; + double stop_twist_norm = 2; + double stop_command_velocity_norm = 3; + double stop_measured_velocity_norm = 4; +} + +message ToppraJointMotionPlannerConfig { + ToppraPathType path_type = 1; + double sample_period_s = 2; + int32 grid_size = 3; + int32 high_grid_size = 4; +} + +message MoveJConfig { + oneof algorithm { + ToppraJointMotionPlannerConfig toppra_joint_motion_planner = 1; + } +} + +message MoveLPlannerConfig { + double sample_period_s = 1; + double position_gain = 2; + double rotation_gain = 3; + CartesianVelocityQpConfig qp = 4; +} + +message MoveLConfig { + oneof algorithm { + MoveLPlannerConfig pinocchio_dls_cartesian_motion_planner = 1; + MoveLPlannerConfig pinocchio_qp_cartesian_motion_planner = 2; + } +} + +message SpeedLControllerConfig { + oneof algorithm { + CartesianVelocityControllerConfig cartesian_velocity_controller = 1; + } +} + +message SpeedLConfig { + oneof algorithm { + SpeedLPlannerConfig pinocchio_dls_cartesian_motion_planner = 1; + SpeedLPlannerConfig pinocchio_qp_cartesian_motion_planner = 2; + } + + SpeedLControllerConfig speed_l_controller = 10; +} + +message ArmKinematicsConfig { + oneof algorithm { + PinocchioDlsIKConfig pinocchio_dls_ik_solver = 10; + PinocchioQpIKConfig pinocchio_qp_ik_solver = 11; + SrsIKConfig srs_ik_solver = 12; + LawbaIKConfig lawba_ik_solver = 13; + } +} + +message ArmMotionConfig { + MoveJConfig move_j = 1; + MoveLConfig move_l = 2; + reserved 3; + SpeedLConfig speed_l = 4; +} + +message RobotArmConfig { + string id = 1; + reserved 2; + + oneof backend { + MotorRobotArmBackendConfig motor = 10; + VendorRobotArmBackendConfig vendor = 11; + } + + ArmKinematicsConfig kinematics = 20; + ArmMotionConfig motion = 21; +} + +message ArmConfig { + repeated RobotArmConfig robot_arms = 1; +} + +message ArmRootConfig { + ArmConfig arm = 1; +} diff --git a/protos/cmvr/config/aubo_robot_config/aubo_robot_config.proto b/protos/cmvr/config/aubo_robot_config/aubo_robot_config.proto new file mode 100644 index 00000000..4cc6b0e1 --- /dev/null +++ b/protos/cmvr/config/aubo_robot_config/aubo_robot_config.proto @@ -0,0 +1,10 @@ +syntax = "proto3"; +package cmvr.config; + +message AuboRobotConfig { + string id = 1; + string ip = 2; + int32 port = 3; + string username = 4; + string password = 5; +} diff --git a/protos/cmvr/config/biohead_config/biohead_config.proto b/protos/cmvr/config/biohead_config/biohead_config.proto new file mode 100644 index 00000000..543a96e7 --- /dev/null +++ b/protos/cmvr/config/biohead_config/biohead_config.proto @@ -0,0 +1,23 @@ +syntax = "proto3"; +package cmvr.config; + +message BioHeadServoGroupConfig { + string name = 1; + int32 address = 2; + int32 channel_start = 3; + int32 channel_end = 4; + repeated double offset = 5; + repeated double min_angles = 6; + repeated double max_angles = 7; +} + +message BioHeadRobotConfig { + string id = 1; + string serial_port = 2; + int32 baud_rate = 3; + repeated BioHeadServoGroupConfig servo_groups = 4; +} +message BioHeadRobotRootConfig { + BioHeadRobotConfig bio_head = 1; +} + diff --git a/protos/cmvr/config/camera_config/camera_config.proto b/protos/cmvr/config/camera_config/camera_config.proto index 602337ff..153a7ecc 100644 --- a/protos/cmvr/config/camera_config/camera_config.proto +++ b/protos/cmvr/config/camera_config/camera_config.proto @@ -21,6 +21,8 @@ enum AlignMode { // RealSense摄像头 message RealSenseCameraConfig { + reserved 12; + string id = 1; string serialNumber = 2; int32 width = 3; @@ -32,13 +34,14 @@ message RealSenseCameraConfig { AlignMode align_mode = 9; int32 buffer_size = 10; bool sync = 11; - bool enable = 12; int32 encode_width = 13; int32 encode_height = 14; } // UVC摄像头 message UVCCameraConfig { + reserved 10; + string id = 1; string usb = 2; // 或者叫 device_path int32 width = 3; @@ -48,13 +51,35 @@ message UVCCameraConfig { CameraMode camera_mode = 7; StreamMode stream_mode = 8; int32 buffer_size = 9; - bool enable = 10; int32 encode_width = 11; int32 encode_height = 12; } -// 主配置消息 - 统一管理所有摄像头 -message CameraConfig { - repeated UVCCameraConfig uvc_cameras = 1; - repeated RealSenseCameraConfig realsense_cameras = 2; +// MechMind工业相机 +message MechMindCameraConfig { + reserved 5; + + string id = 1; + string ip = 2; + optional bool align = 3; + string image2d_type = 4; +} + +message CameraDeviceConfig { + string id = 1; + reserved 2; + + oneof backend { + UVCCameraConfig uvc = 10; + RealSenseCameraConfig realsense = 11; + MechMindCameraConfig mechmind = 12; + } +} + +message CameraConfig { + repeated CameraDeviceConfig cameras = 1; +} + +message CameraRootConfig { + CameraConfig camera = 1; } diff --git a/protos/cmvr/config/cmvr_es_config/cmvr_es_config.proto b/protos/cmvr/config/cmvr_es_config/cmvr_es_config.proto new file mode 100644 index 00000000..ea54076f --- /dev/null +++ b/protos/cmvr/config/cmvr_es_config/cmvr_es_config.proto @@ -0,0 +1,14 @@ +syntax = "proto3"; +package cmvr.config; + +message CMVRESConfig { + reserved 3, 5; + + string logger_config_file = 1; + string device_manager_config_file = 2; + string task_manager_config_file = 4; + string grpc_server_config_file = 6; +} +message CMVRESRootConfig { + CMVRESConfig cmvr_es = 1; +} diff --git a/protos/cmvr/config/device_manager_config/device_manager_config.proto b/protos/cmvr/config/device_manager_config/device_manager_config.proto new file mode 100644 index 00000000..21a36747 --- /dev/null +++ b/protos/cmvr/config/device_manager_config/device_manager_config.proto @@ -0,0 +1,38 @@ +syntax = "proto3"; +package cmvr.config; + +message DeviceConfigEntry { + enum DeviceType { + reserved 1, 2, 3, 4, 5, 6, 7, 10, 11; + reserved "DEVICE_TYPE_UVC_CAMERA", "DEVICE_TYPE_REALSENSE_CAMERA", + "DEVICE_TYPE_RH56DFTP_DEXHAND", "DEVICE_TYPE_PX6AX_GEN3", + "DEVICE_TYPE_FFMPEG_MICROPHONE", "DEVICE_TYPE_FFMPEG_SPEAKER", + "DEVICE_TYPE_HUMANOID_ROBOT", "DEVICE_TYPE_MOTOR_ARM", + "DEVICE_TYPE_CARTESIAN_ARM"; + + DEVICE_TYPE_UNKNOWN = 0; + DEVICE_TYPE_BIO_HEAD_ROBOT = 8; + DEVICE_TYPE_MOTOR_SYSTEM = 9; + DEVICE_TYPE_ROBOT_ARM = 12; + DEVICE_TYPE_CAMERA = 13; + DEVICE_TYPE_DEXHAND = 14; + DEVICE_TYPE_MICROPHONE = 15; + DEVICE_TYPE_SPEAKER = 16; + DEVICE_TYPE_AGV = 17; + } + + string id = 1; + DeviceType type = 2; + string config_file = 3; + bool enable = 4; +} + +message DeviceManagerConfig { + string name = 1; + string version = 2; + string description = 3; + repeated DeviceConfigEntry devices = 4; +} +message DeviceManagerRootConfig { + DeviceManagerConfig device_manager = 1; +} diff --git a/protos/cmvr/config/dexhand_config/dexhand_config.proto b/protos/cmvr/config/dexhand_config/dexhand_config.proto index 3c555da9..b822f3f5 100644 --- a/protos/cmvr/config/dexhand_config/dexhand_config.proto +++ b/protos/cmvr/config/dexhand_config/dexhand_config.proto @@ -2,10 +2,11 @@ syntax = "proto3"; package cmvr.config; message RH56DFTPDexHandConfig{ + reserved 4; + string id = 1; string ip = 2; int32 port = 3; - bool enable = 4; int32 poll_interval_ms = 5; } @@ -16,6 +17,8 @@ enum PX6AXGen3PollingReadMode { } message PX6AXGen3{ + reserved 10; + string id = 1; string serial_port = 2; string sensor_model = 3; @@ -25,7 +28,6 @@ message PX6AXGen3{ int32 resultant_length = 7; int32 poll_interval_ms = 8; bool auto_calibrate = 9; - bool enable = 10; int32 response_timeout_ms = 11; int32 tactile_rows = 12; int32 tactile_cols = 13; @@ -38,7 +40,20 @@ message PX6AXGen3{ -message DexHandConfig{ - repeated RH56DFTPDexHandConfig rh56dftp_dexhands = 1; - repeated PX6AXGen3 px_6ax_gen3 = 2; +message DexHandDeviceConfig { + string id = 1; + reserved 2; + + oneof backend { + RH56DFTPDexHandConfig rh56dftp = 10; + PX6AXGen3 px_6ax_gen3 = 11; + } +} + +message DexHandConfig{ + repeated DexHandDeviceConfig dexhands = 1; +} + +message DexHandRootConfig { + DexHandConfig dexhand = 1; } diff --git a/protos/cmvr/config/grpc_server_config/grpc_server_config.proto b/protos/cmvr/config/grpc_server_config/grpc_server_config.proto new file mode 100644 index 00000000..11ad8018 --- /dev/null +++ b/protos/cmvr/config/grpc_server_config/grpc_server_config.proto @@ -0,0 +1,12 @@ +syntax = "proto3"; +package cmvr.config; + +message GRPCServerConfig { + string host = 1; + string port = 2; + bool enable_reflection = 3; + string id = 4; +} +message GRPCServerRootConfig { + GRPCServerConfig grpc_server = 1; +} diff --git a/protos/cmvr/config/joint_limits_config.proto b/protos/cmvr/config/joint_limits_config.proto new file mode 100644 index 00000000..b18fc4e8 --- /dev/null +++ b/protos/cmvr/config/joint_limits_config.proto @@ -0,0 +1,20 @@ +syntax = "proto3"; +package cmvr.config; + +enum JointLimitSource { + JOINT_LIMIT_SOURCE_UNKNOWN = 0; + JOINT_LIMIT_SOURCE_URDF = 1; + JOINT_LIMIT_SOURCE_CUSTOM = 2; +} + +message JointLimitConfig { + string joint_name = 1; + double lower = 2; + double upper = 3; + double velocity = 4; +} + +message JointLimitsConfig { + JointLimitSource source = 1; + repeated JointLimitConfig joints = 2; +} diff --git a/protos/cmvr/config/lawba_ik_config.proto b/protos/cmvr/config/lawba_ik_config.proto new file mode 100644 index 00000000..8cee9509 --- /dev/null +++ b/protos/cmvr/config/lawba_ik_config.proto @@ -0,0 +1,15 @@ +syntax = "proto3"; + +package cmvr.config; + +import "cmvr/config/srs_ik_config.proto"; + +message LawbaIKConfig { + SrsIKConfig srs_config = 1; + double lambda_q = 2; + double cur_branch_cost_threshold = 3; + double opt_psi_update_alpha = 4; + double opt_psi_max_delta = 5; + double opt_psi_min = 6; + double opt_psi_eps = 7; +} diff --git a/protos/cmvr/config/logger_config/logger_config.proto b/protos/cmvr/config/logger_config/logger_config.proto new file mode 100644 index 00000000..1e6814f6 --- /dev/null +++ b/protos/cmvr/config/logger_config/logger_config.proto @@ -0,0 +1,37 @@ +syntax = "proto3"; +package cmvr.config; + +enum LogLevel { + LOG_LEVEL_UNSPECIFIED = 0; + LOG_LEVEL_DEBUG = 1; + LOG_LEVEL_INFO = 2; + LOG_LEVEL_WARNING = 3; + LOG_LEVEL_ERROR = 4; + LOG_LEVEL_FATAL = 5; +} + +message LogRoute { + LogLevel level = 1; + bool terminal = 2; + bool file = 3; +} + +message LogFormat { + optional bool show_time = 1; + optional bool show_level = 2; + optional bool show_thread_id = 3; + optional bool show_source_location = 4; +} + +message LoggerConfig { + LogLevel minimum_level = 1; + repeated LogRoute routes = 2; + string directory = 3; + int32 max_file_size_mb = 4; + int32 flush_interval_seconds = 5; + LogFormat format = 6; +} + +message LoggerRootConfig { + LoggerConfig logger = 1; +} diff --git a/protos/cmvr/config/microphone_config/microphone_config.proto b/protos/cmvr/config/microphone_config/microphone_config.proto index be4451f0..741d44d2 100644 --- a/protos/cmvr/config/microphone_config/microphone_config.proto +++ b/protos/cmvr/config/microphone_config/microphone_config.proto @@ -2,14 +2,28 @@ syntax = "proto3"; package cmvr.config; message FFMpegMicroPhoneConfig{ + reserved 5; + string id = 1; int32 channels = 2; int32 sampleRate = 3; int32 volume = 4; - bool enable = 5; string input_device = 6; } +message MicrophoneDeviceConfig { + string id = 1; + reserved 2; + + oneof backend { + FFMpegMicroPhoneConfig ffmpeg = 10; + } +} + message MicroPhoneConfig{ - repeated FFMpegMicroPhoneConfig ffmpeg_microphones = 1; -} \ No newline at end of file + repeated MicrophoneDeviceConfig microphones = 1; +} + +message MicroPhoneRootConfig { + MicroPhoneConfig microphone = 1; +} diff --git a/protos/cmvr/config/motor_config/motor_config.proto b/protos/cmvr/config/motor_config/motor_config.proto new file mode 100644 index 00000000..666cfaa8 --- /dev/null +++ b/protos/cmvr/config/motor_config/motor_config.proto @@ -0,0 +1,52 @@ +syntax = "proto3"; +package cmvr.config; + +import "cmvr/config/joint_limits_config.proto"; + +message Ti5MotorConfig { + int32 id = 1; + string joint_name = 2; + double limit_q_lb = 3; + double limit_q_ub = 4; + double limit_qd = 5; +} + +message SocketCanConfig { + string dev_id = 1; + int32 channel_id = 2; +} + +message EtherCATConfig { + string master_id = 1; + int32 cycle_us = 2; +} + +enum MotorBusType { + MOTOR_BUS_UNKNOWN = 0; + MOTOR_BUS_CAN = 1; + MOTOR_BUS_ETHERCAT = 2; + MOTOR_BUS_MUJOCO = 3; +} + +message MotorGroupConfig { + string id = 1; + MotorBusType bus_type = 2; + string tool_frame = 3; + + oneof bus_config { + SocketCanConfig can = 10; + EtherCATConfig ethercat = 11; + } + + repeated Ti5MotorConfig motors = 20; + JointLimitsConfig joint_limits = 30; + string joint_limits_urdf_path = 31; +} + +message MotorConfig { + string id = 1; + repeated MotorGroupConfig motor_groups = 2; +} +message MotorRootConfig { + MotorConfig motor = 1; +} diff --git a/protos/cmvr/config/pinocchio_dls_ik_config.proto b/protos/cmvr/config/pinocchio_dls_ik_config.proto index e69de29b..4eb2ed0f 100644 --- a/protos/cmvr/config/pinocchio_dls_ik_config.proto +++ b/protos/cmvr/config/pinocchio_dls_ik_config.proto @@ -0,0 +1,26 @@ +syntax = "proto3"; + +package cmvr.config; + +import "cmvr/config/joint_limits_config.proto"; + +message PinocchioDlsIKConfig { + string urdf_path = 1; + string base_frame_name = 2; + string flange_frame_name = 3; + string tcp_frame_name = 4; + + int32 max_iters = 5; + double pos_eps = 6; + double rot_eps = 7; + double damping = 8; + JointLimitAvoidanceConfig joint_limit_avoidance = 9; + JointLimitsConfig joint_limits = 10; +} + +message JointLimitAvoidanceConfig { + bool enable = 1; + double gain = 2; + double margin_ratio = 3; + double max_push = 4; +} diff --git a/protos/cmvr/config/pinocchio_qp_ik_solver_config.proto b/protos/cmvr/config/pinocchio_qp_ik_config.proto similarity index 100% rename from protos/cmvr/config/pinocchio_qp_ik_solver_config.proto rename to protos/cmvr/config/pinocchio_qp_ik_config.proto diff --git a/protos/cmvr/config/speaker_config/speaker_config.proto b/protos/cmvr/config/speaker_config/speaker_config.proto new file mode 100644 index 00000000..d1505b77 --- /dev/null +++ b/protos/cmvr/config/speaker_config/speaker_config.proto @@ -0,0 +1,25 @@ +syntax = "proto3"; +package cmvr.config; + +message FFMpegSpeakerConfig{ + reserved 2; + + string id = 1; +} + +message SpeakerDeviceConfig { + string id = 1; + reserved 2; + + oneof backend { + FFMpegSpeakerConfig ffmpeg = 10; + } +} + +message SpeakerConfig{ + repeated SpeakerDeviceConfig speakers = 1; +} + +message SpeakerRootConfig { + SpeakerConfig speaker = 1; +} diff --git a/protos/cmvr/config/speaker_config/speaker_conifg.proto b/protos/cmvr/config/speaker_config/speaker_conifg.proto deleted file mode 100644 index 7402b460..00000000 --- a/protos/cmvr/config/speaker_config/speaker_conifg.proto +++ /dev/null @@ -1,11 +0,0 @@ -syntax = "proto3"; -package cmvr.config; - -message FFMpegSpeakerConfig{ - string id = 1; - bool enable = 2; -} - -message SpeakerConfig{ - repeated FFMpegSpeakerConfig ffmpeg_speakers = 1; -} \ No newline at end of file diff --git a/protos/cmvr/config/srs_ik_config.proto b/protos/cmvr/config/srs_ik_config.proto new file mode 100644 index 00000000..f21f64ea --- /dev/null +++ b/protos/cmvr/config/srs_ik_config.proto @@ -0,0 +1,13 @@ +syntax = "proto3"; + +package cmvr.config; + +message SrsIKConfig { + double initial_psi = 1; + int32 shoulder_config = 2; + int32 elbow_config = 3; + int32 wrist_config = 4; + repeated double link_lengths = 10; + repeated double joint_lower_limits = 11; + repeated double joint_upper_limits = 12; +} diff --git a/protos/cmvr/config/task_manager_config/task_manager_config.proto b/protos/cmvr/config/task_manager_config/task_manager_config.proto new file mode 100644 index 00000000..6c13c845 --- /dev/null +++ b/protos/cmvr/config/task_manager_config/task_manager_config.proto @@ -0,0 +1,32 @@ +syntax = "proto3"; +package cmvr.config; + +message TaskConfigEntry { + enum TaskType { + TASK_TYPE_UNKNOWN = 0; + TASK_TYPE_TOUCH_SCREEN = 1; + TASK_TYPE_GRPC_SERVER = 3; + reserved 2; + reserved "TASK_TYPE_ARM_CONTROL"; + } + + enum TaskRunMode { + TASK_RUN_MODE_UNKNOWN = 0; + TASK_RUN_MODE_PERIODIC_STEP = 1; + TASK_RUN_MODE_BLOCKING_SERVICE = 2; + } + + string id = 1; + TaskType type = 2; + string config_file = 3; + bool enable = 4; + TaskRunMode run_mode = 5; + double control_period_s = 6; +} + +message TaskManagerConfig { + repeated TaskConfigEntry tasks = 1; +} +message TaskManagerRootConfig { + TaskManagerConfig task_manager = 1; +} diff --git a/protos/cmvr/config/touch_screen_algorithm_config.proto b/protos/cmvr/config/touch_screen_algorithm_config.proto new file mode 100644 index 00000000..9a15bc0b --- /dev/null +++ b/protos/cmvr/config/touch_screen_algorithm_config.proto @@ -0,0 +1,32 @@ +syntax = "proto3"; + +package cmvr.config; + +import "cmvr/common/geometry.proto"; +import "cmvr/config/touch_screen_common_config.proto"; + +message TouchScreenApriltagConfig { + optional double tag_size_m = 1; + optional TouchScreenDepthPolicy depth_policy = 2; + optional TouchScreenTargetPointMethod target_point_method = 3; +} + +message TouchScreenIbvsConfig { + optional string camera_link = 1; + optional double lambda = 2; + optional double mu = 3; + optional double qdot_max = 4; + .cmvr.common.Vec6 vmax6 = 5; + .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; + + repeated string control_joint_names = 14; +} diff --git a/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto b/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto deleted file mode 100644 index b0c22c83..00000000 --- a/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto +++ /dev/null @@ -1,155 +0,0 @@ -syntax = "proto3"; - -package cmvr.config; - -message TouchScreenVec3 { - optional double x = 1; - optional double y = 2; - optional double z = 3; -} - -message TouchScreenTwist6 { - optional double vx = 1; - optional double vy = 2; - optional double vz = 3; - optional double wx = 4; - optional double wy = 5; - optional double wz = 6; -} - -message TouchScreenMatrix3d { - optional double m00 = 1; - optional double m01 = 2; - optional double m02 = 3; - optional double m10 = 4; - optional double m11 = 5; - optional double m12 = 6; - optional double m20 = 7; - optional double m21 = 8; - optional double m22 = 9; -} - -message TouchScreenErrorThreshold6 { - optional double x = 1; - optional double y = 2; - optional double z = 3; - optional double rx = 4; - optional double ry = 5; - optional double rz = 6; -} - -message TouchScreenInitJointPoint { - optional string joint_name = 1; - optional double rad = 2; -} - -enum TouchScreenDepthPolicy { - TOUCH_SCREEN_DEPTH_POLICY_NONE = 0; - TOUCH_SCREEN_DEPTH_POLICY_PREFER = 1; - TOUCH_SCREEN_DEPTH_POLICY_REQUIRE = 2; -} - -enum TouchScreenTargetPointMethod { - TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE = 0; - TOUCH_SCREEN_TARGET_POINT_METHOD_DEPTH_IMAGE = 1; -} - -enum TouchScreenFingerType { - TOUCH_SCREEN_FINGER_TYPE_PINKY = 0; - TOUCH_SCREEN_FINGER_TYPE_RING = 1; - TOUCH_SCREEN_FINGER_TYPE_MIDDLE = 2; - TOUCH_SCREEN_FINGER_TYPE_INDEX = 3; - TOUCH_SCREEN_FINGER_TYPE_THUMB = 4; -} - -enum TouchScreenTactileRegion { - TOUCH_SCREEN_TACTILE_REGION_TIP = 0; - TOUCH_SCREEN_TACTILE_REGION_FINGER = 1; - TOUCH_SCREEN_TACTILE_REGION_PAD = 2; - TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE = 4; -} - -enum TouchScreenTactileCriterion { - TOUCH_SCREEN_TACTILE_CRITERION_FZ = 0; - TOUCH_SCREEN_TACTILE_CRITERION_MAGNITUDE = 1; -} - -enum TouchScreenAlignMode { - TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION = 0; - TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION = 1; - TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY = 2; -} - -message TouchScreenAppConfig { - reserved 39; - reserved "tactile_pressure_peak_threshold"; - - optional string robot_id = 40; - optional string dexhand_id = 41; - optional string camera_id = 42; - optional bool move_to_init_position = 43; - repeated TouchScreenInitJointPoint init_joint_positions = 44; - optional double init_movej_vel = 45; - optional double init_movej_acc = 46; - optional bool move_to_init_position_before_start = 47; - - optional string urdf_path = 1; - optional string base_link = 2; - optional string flange_link = 3; - optional string camera_link = 4; - - optional double tag_size_m = 5; - optional TouchScreenDepthPolicy depth_policy = 6; - optional TouchScreenTargetPointMethod target_point_method = 7; - - TouchScreenVec3 hover_target_in_camera = 8; - optional double target_rx = 9; - optional double target_ry = 10; - optional double target_rz = 11; - optional TouchScreenAlignMode align_mode = 54; - - optional double ibvs_lambda = 12; - optional double ibvs_mu = 13; - optional double ibvs_qdot_max = 14; - TouchScreenTwist6 ibvs_vmax6 = 15; - TouchScreenTwist6 ibvs_amax6 = 57; - optional double ibvs_twist_filter_alpha = 58; - optional bool enable_joint_limit_avoidance = 16; - optional double joint_limit_avoidance_gain = 17; - optional double joint_limit_avoidance_margin_ratio = 18; - optional double joint_limit_avoidance_max_push = 19; - - TouchScreenMatrix3d r_camera_to_visp = 20; - TouchScreenMatrix3d r_camera_to_urdf = 21; - - repeated string control_joint_names = 22; - - optional double align_xy_threshold_m = 23; - optional double align_z_threshold_m = 24; - optional double align_rot_threshold_rad = 25; - TouchScreenErrorThreshold6 align_error_threshold6 = 55; - optional int32 align_stable_frames = 26; - optional double align_timeout_s = 27; - optional bool pause_after_align_reached = 28; - - TouchScreenTwist6 touch_twist_base = 29; - optional double touch_speedl_acceleration = 30; - optional double touch_speedl_forward_l = 31; - optional bool touch_use_speedl = 48; - optional double touch_forward_l = 49; - optional double touch_movel_speed = 50; - optional double touch_movel_acceleration = 51; - optional double touch_movel_jerk = 52; - repeated double touch_movel_qd_max = 53; - - optional double dwell_time_s = 32; - TouchScreenTwist6 retract_twist_base = 33; - optional double retract_acceleration = 34; - optional double retract_duration_s = 35; - - optional TouchScreenFingerType tactile_finger = 36; - optional TouchScreenTactileRegion tactile_region = 37; - optional TouchScreenTactileCriterion tactile_criterion = 59; - optional double tactile_pressure_sum_threshold = 38; - optional int32 tactile_nonzero_count_threshold = 56; -} diff --git a/protos/cmvr/config/touch_screen_common_config.proto b/protos/cmvr/config/touch_screen_common_config.proto new file mode 100644 index 00000000..b6dc8b29 --- /dev/null +++ b/protos/cmvr/config/touch_screen_common_config.proto @@ -0,0 +1,40 @@ +syntax = "proto3"; + +package cmvr.config; + +enum TouchScreenDepthPolicy { + TOUCH_SCREEN_DEPTH_POLICY_NONE = 0; + TOUCH_SCREEN_DEPTH_POLICY_PREFER = 1; + TOUCH_SCREEN_DEPTH_POLICY_REQUIRE = 2; +} + +enum TouchScreenTargetPointMethod { + TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE = 0; + TOUCH_SCREEN_TARGET_POINT_METHOD_DEPTH_IMAGE = 1; +} + +enum TouchScreenFingerType { + TOUCH_SCREEN_FINGER_TYPE_PINKY = 0; + TOUCH_SCREEN_FINGER_TYPE_RING = 1; + TOUCH_SCREEN_FINGER_TYPE_MIDDLE = 2; + TOUCH_SCREEN_FINGER_TYPE_INDEX = 3; + TOUCH_SCREEN_FINGER_TYPE_THUMB = 4; +} + +enum TouchScreenTactileRegion { + TOUCH_SCREEN_TACTILE_REGION_TIP = 0; + TOUCH_SCREEN_TACTILE_REGION_FINGER = 1; + TOUCH_SCREEN_TACTILE_REGION_PAD = 2; + TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE = 4; +} + +enum TouchScreenTactileCriterion { + TOUCH_SCREEN_TACTILE_CRITERION_FZ = 0; + TOUCH_SCREEN_TACTILE_CRITERION_MAGNITUDE = 1; +} + +enum TouchScreenAlignMode { + TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION = 0; + TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION = 1; + TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY = 2; +} diff --git a/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto new file mode 100644 index 00000000..c48681a7 --- /dev/null +++ b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto @@ -0,0 +1,102 @@ +syntax = "proto3"; + +package cmvr.config; + +import "cmvr/common/geometry.proto"; +import "cmvr/config/touch_screen_common_config.proto"; +import "cmvr/config/touch_screen_algorithm_config.proto"; + +message TouchScreenInitJointPoint { + optional string joint_name = 1; + optional double rad = 2; +} + +message TouchScreenTaskDevicesConfig { + reserved 1; + reserved "robot_id"; + + optional string arm_id = 4; + optional string dexhand_id = 2; + optional string camera_id = 3; +} + +message TouchScreenTaskInitializationConfig { + optional bool before_start = 1; + optional bool after_finish = 2; + repeated TouchScreenInitJointPoint joint_positions = 3; + optional double velocity = 4; + optional double acceleration = 5; +} + +message TouchScreenTaskPerceptionConfig { + TouchScreenApriltagConfig apriltag = 1; +} + +message TouchScreenAlignmentTargetConfig { + .cmvr.common.Vec3 position_in_camera = 1; + .cmvr.common.Vec3 rotation_vector = 2; + optional TouchScreenAlignMode mode = 3; +} + +message TouchScreenTaskAlignmentConfig { + reserved 1; + reserved "kinematics"; + TouchScreenIbvsConfig ibvs = 2; + TouchScreenAlignmentTargetConfig target = 3; + .cmvr.common.Vec6 error_threshold = 4; + optional int32 stable_frames = 5; + optional double timeout_s = 6; + optional bool pause_when_reached = 7; +} + +message TouchScreenTouchSpeedLConfig { + .cmvr.common.Vec6 twist_tool = 1; + optional double acceleration = 2; + optional double max_distance_m = 3; +} + +message TouchScreenTouchMoveLConfig { + .cmvr.common.Vec6 direction_tool = 1; + optional double distance_m = 2; + optional double velocity = 3; + optional double acceleration = 4; + optional double jerk = 5; + repeated double joint_velocity_limits = 6; +} + +message TouchScreenTactileTriggerConfig { + optional TouchScreenFingerType finger = 1; + optional TouchScreenTactileRegion region = 2; + optional TouchScreenTactileCriterion criterion = 3; + optional double force_threshold = 4; +} + +message TouchScreenTaskTouchConfig { + oneof motion { + TouchScreenTouchSpeedLConfig speed_l = 1; + TouchScreenTouchMoveLConfig move_l = 2; + } + + TouchScreenTactileTriggerConfig tactile = 3; + optional double dwell_time_s = 4; +} + +message TouchScreenTaskRetractConfig { + .cmvr.common.Vec6 twist_tool = 1; + optional double acceleration = 2; + optional double duration_s = 3; +} + +message TouchScreenTaskConfig { + TouchScreenTaskDevicesConfig devices = 1; + TouchScreenTaskInitializationConfig initialization = 2; + TouchScreenTaskPerceptionConfig perception = 3; + TouchScreenTaskAlignmentConfig alignment = 4; + TouchScreenTaskTouchConfig touch = 5; + TouchScreenTaskRetractConfig retract = 6; + optional string id = 7; +} + +message TouchScreenTaskRootConfig { + TouchScreenTaskConfig touch_screen_task = 1; +} diff --git a/protos/cmvr/msgs/geometry.proto b/protos/cmvr/msgs/geometry.proto deleted file mode 100644 index dca76637..00000000 --- a/protos/cmvr/msgs/geometry.proto +++ /dev/null @@ -1,51 +0,0 @@ -syntax = "proto3"; - -package cmvr.msgs; - - -message Vec2 { - double x = 1; - double y = 2; -} - -message Vec3 { - double x = 1; - double y = 2; - double z = 3; -} - -// ===== 四元数 ===== -message Quat { - double w = 1; - double x = 2; - double y = 3; - double z = 4; -} - -// ===== 位置（3D）===== -message Position { - double x = 1; // unit: m - double y = 2; - double z = 3; -} - -// ===== 欧拉角 ===== -message Euler { - double rx = 1; // unit: rad (roll) - double ry = 2; // unit: rad (pitch) - double rz = 3; // unit: rad (yaw) -} - -// ===== 位姿（3D）===== -message Pose3d { - Position position = 1; // (m) - Quat quaternion = 2; // 四元数 - Euler euler = 3; // 欧拉角 (rad) -} - -// ===== 位姿（2D）===== -message Pose2d { - double x = 1; // (m) - double y = 2; // (m) - double theta = 3; // (rad) -} diff --git a/python/hand_eye_calibration/collect_data.py b/python/hand_eye_calibration/collect_data.py index 9b2a0d2d..0489dcde 100644 --- a/python/hand_eye_calibration/collect_data.py +++ b/python/hand_eye_calibration/collect_data.py @@ -10,6 +10,7 @@ import re import numpy as np import cv2 import pyrealsense2 as rs +from pathlib import Path from libs.log_setting import CommonLog from libs.auxiliary import create_folder_with_date, get_ip, popup_message @@ -22,7 +23,8 @@ from robot_wrapper import Robot # -------------------- 全局配置 -------------------- TARGET_SERIAL = "243122075614" # 相机序列号 EXEC_PATH = "/home/lgv/cmvr/cmvr-es/cmake-build-debug/example/solve_fk" -ROBOT_CONFIG = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml" +REPO_ROOT = Path(__file__).resolve().parents[2] +ROBOT_CONFIG = str(REPO_ROOT / "cmvr-es/config/cmvr_es.pb.txt") logger = logging.getLogger(__name__) logger = CommonLog(logger) diff --git a/python/vision_servo/follow_target.py b/python/vision_servo/follow_target.py index 60788d5c..7278be61 100644 --- a/python/vision_servo/follow_target.py +++ b/python/vision_servo/follow_target.py @@ -6,6 +6,7 @@ import cv2 import subprocess import numpy as np import pyrealsense2 as rs +from pathlib import Path from scipy.spatial.transform import Rotation as R from get_chessboard_position import init_realsense, get_closest_red_point @@ -16,6 +17,9 @@ sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example") # 导入模块 from robot_wrapper import Robot +REPO_ROOT = Path(__file__).resolve().parents[2] +ROBOT_CONFIG = str(REPO_ROOT / "cmvr-es/config/cmvr_es.pb.txt") + @@ -217,7 +221,7 @@ def red_point_position(color_frame, depth_frame, depth_intr, vis_path=None, min_ # # 初始化机器人（传入配置文件路径和机器人名称） # # pipeline, align, depth_intr = start_pipeline("243122075614") -# robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") +# robot = Robot(ROBOT_CONFIG, "hc01") # js = robot.getJointQ('right') # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S") # T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") @@ -330,7 +334,7 @@ def compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target): # pipeline, align = init_realsense() # try: # # 初始化机器人 -# robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") +# robot = Robot(ROBOT_CONFIG, "hc01") # # # 移动到初始关节位姿 # @@ -460,7 +464,7 @@ if __name__ == "__main__": # 初始化机器人 - robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") + robot = Robot(ROBOT_CONFIG, "hc01") # js = robot.getJointQ('right') js = np.zeros(7) js = np.array([-0.054383226063586795, 1.4797969033848364, -1.4685445467885516, -0.03790717127687202, -0.2797606954361809, 0.1916896945079446, -0.17680932146902423]) @@ -469,4 +473,3 @@ if __name__ == "__main__": T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S") # T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") print(T_base_ee) - diff --git a/python/vision_servo/get_chessboard_position.py b/python/vision_servo/get_chessboard_position.py index 66631c5a..2c16c274 100644 --- a/python/vision_servo/get_chessboard_position.py +++ b/python/vision_servo/get_chessboard_position.py @@ -10,6 +10,7 @@ import numpy as np import pyrealsense2 as rs import sys import time +from pathlib import Path # 把.so所在目录加入 Python 路径 sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example") @@ -17,6 +18,9 @@ sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example") # 导入机器人控制模块 from robot_wrapper import Robot +REPO_ROOT = Path(__file__).resolve().parents[2] +ROBOT_CONFIG = str(REPO_ROOT / "cmvr-es/config/cmvr_es.pb.txt") + def init_realsense(): """初始化 RealSense 管道并返回 pipeline 和 align 对象""" @@ -87,7 +91,7 @@ def get_closest_red_point(pipeline, align, vis_path=None, warmup=30, def main(serial="243122075614", avg_window=3, loop=True): pipeline, align = init_realsense() - # robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") + # robot = Robot(ROBOT_CONFIG, "hc01") try: while True: diff --git a/python/vision_servo/robot_warpper_test.py b/python/vision_servo/robot_warpper_test.py index f4d26654..3464b06e 100644 --- a/python/vision_servo/robot_warpper_test.py +++ b/python/vision_servo/robot_warpper_test.py @@ -1,5 +1,6 @@ import sys import os +from pathlib import Path os.environ["GLOG_minloglevel"] = "1" # 屏蔽 INFO # from python.vision_servo.follow_target import moveJ import time @@ -10,9 +11,12 @@ sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example") # 导入模块 from robot_wrapper import Robot +REPO_ROOT = Path(__file__).resolve().parents[2] +ROBOT_CONFIG = str(REPO_ROOT / "cmvr-es/config/cmvr_es.pb.txt") + # R_WRIST_R_S # 初始化机器人（传入配置文件路径和机器人名称） -robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") +robot = Robot(ROBOT_CONFIG, "hc01") # robot.torqueOn() # @@ -59,4 +63,4 @@ print("--------------------------") # // {"R_WRIST_Y", -0.000210733}, # // {"R_WRIST_R", -0.0942364} # 控制右臂关节 -# robot.moveJ("right", [0.1, 0.4, -0.3, 0.2, 0.0, -0.1, 0.2]) \ No newline at end of file +# robot.moveJ("right", [0.1, 0.4, -0.3, 0.2, 0.0, -0.1, 0.2])