diff --git a/IoT/iot_device_sdk_python/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/client_conf.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/client_conf.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/connect_auth_info.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/connect_auth_info.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/device_client.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/device_client.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/iot_result.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/iot_result.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/listener/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/listener/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/listener/command_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/listener/command_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/listener/default_publish_action_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/listener/default_publish_action_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/listener/device_message_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/listener/device_message_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/listener/device_shadow_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/listener/device_shadow_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/listener/property_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/listener/property_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/listener/raw_device_message_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/listener/raw_device_message_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/mqtt_connect_conf.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/mqtt_connect_conf.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/request/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/request/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/request/command.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/request/command.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/request/command_response.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/request/command_response.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/request/device_base_info.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/request/device_base_info.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/request/device_event.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/request/device_event.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/request/device_events.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/request/device_events.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/request/device_message.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/request/device_message.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/request/properties_data.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/request/properties_data.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/request/props_get.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/request/props_get.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/request/props_set.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/request/props_set.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/request/raw_device_message.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/request/raw_device_message.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/request/service_property.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/request/service_property.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/client/request/shadow_data.py:Zone.Identifier b/IoT/iot_device_sdk_python/client/request/shadow_data.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/devicelog/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/devicelog/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/devicelog/device_log_service.py:Zone.Identifier b/IoT/iot_device_sdk_python/devicelog/device_log_service.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/devicelog/listener/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/devicelog/listener/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/devicelog/listener/default_conn_action_log_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/devicelog/listener/default_conn_action_log_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/devicelog/listener/default_conn_log_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/devicelog/listener/default_conn_log_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/filemanager/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/filemanager/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/filemanager/file_manager_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/filemanager/file_manager_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/filemanager/file_manager_service.py:Zone.Identifier b/IoT/iot_device_sdk_python/filemanager/file_manager_service.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/filemanager/url_info.py:Zone.Identifier b/IoT/iot_device_sdk_python/filemanager/url_info.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/abstract_gateway.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/abstract_gateway.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/gtw_operate_sub_device_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/gtw_operate_sub_device_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/requests/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/requests/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/requests/add_sub_device_failed_reason.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/requests/add_sub_device_failed_reason.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/requests/added_sub_device_info.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/requests/added_sub_device_info.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/requests/added_sub_device_info_rsp.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/requests/added_sub_device_info_rsp.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/requests/del_sub_device_failed_reason.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/requests/del_sub_device_failed_reason.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/requests/device_info.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/requests/device_info.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/requests/device_property.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/requests/device_property.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/requests/device_status.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/requests/device_status.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/requests/gtw_add_sub_device_rsp.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/requests/gtw_add_sub_device_rsp.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/requests/gtw_del_sub_device_rsp.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/requests/gtw_del_sub_device_rsp.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/requests/scan_sub_device_notify.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/requests/scan_sub_device_notify.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/requests/sub_devices_info.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/requests/sub_devices_info.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/sub_dev_discovery_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/sub_dev_discovery_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/gateway/sub_devices_persistence.py:Zone.Identifier b/IoT/iot_device_sdk_python/gateway/sub_devices_persistence.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/iot_device.py:Zone.Identifier b/IoT/iot_device_sdk_python/iot_device.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/ota/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/ota/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/ota/ota_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/ota/ota_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/ota/ota_package_info.py:Zone.Identifier b/IoT/iot_device_sdk_python/ota/ota_package_info.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/ota/ota_package_info_v2.py:Zone.Identifier b/IoT/iot_device_sdk_python/ota/ota_package_info_v2.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/ota/ota_query_version.py:Zone.Identifier b/IoT/iot_device_sdk_python/ota/ota_query_version.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/ota/ota_service.py:Zone.Identifier b/IoT/iot_device_sdk_python/ota/ota_service.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/rule/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/rule/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/rule/model/action_handler.py:Zone.Identifier b/IoT/iot_device_sdk_python/rule/model/action_handler.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/rule/model/actions.py:Zone.Identifier b/IoT/iot_device_sdk_python/rule/model/actions.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/rule/model/condition_execute.py:Zone.Identifier b/IoT/iot_device_sdk_python/rule/model/condition_execute.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/rule/model/connditions.py:Zone.Identifier b/IoT/iot_device_sdk_python/rule/model/connditions.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/rule/model/device_info.py:Zone.Identifier b/IoT/iot_device_sdk_python/rule/model/device_info.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/rule/model/rule_info.py:Zone.Identifier b/IoT/iot_device_sdk_python/rule/model/rule_info.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/rule/model/time_range.py:Zone.Identifier b/IoT/iot_device_sdk_python/rule/model/time_range.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/rule/rule_manage_service.py:Zone.Identifier b/IoT/iot_device_sdk_python/rule/rule_manage_service.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/rule/timer_rule/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/rule/timer_rule/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/rule/timer_rule/timer_rule.py:Zone.Identifier b/IoT/iot_device_sdk_python/rule/timer_rule/timer_rule.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/rule/util/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/rule/util/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/rule/util/time_util.py:Zone.Identifier b/IoT/iot_device_sdk_python/rule/util/time_util.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/service/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/service/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/service/abstract_device.py:Zone.Identifier b/IoT/iot_device_sdk_python/service/abstract_device.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/service/abstract_service.py:Zone.Identifier b/IoT/iot_device_sdk_python/service/abstract_service.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/service/i_service.py:Zone.Identifier b/IoT/iot_device_sdk_python/service/i_service.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/service/property.py:Zone.Identifier b/IoT/iot_device_sdk_python/service/property.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/timesync/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/timesync/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/timesync/time_sync_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/timesync/time_sync_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/timesync/time_sync_service.py:Zone.Identifier b/IoT/iot_device_sdk_python/timesync/time_sync_service.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/transport/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/transport/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/transport/action_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/transport/action_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/transport/buffer_message.py:Zone.Identifier b/IoT/iot_device_sdk_python/transport/buffer_message.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/transport/connect_action_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/transport/connect_action_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/transport/connect_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/transport/connect_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/transport/connection.py:Zone.Identifier b/IoT/iot_device_sdk_python/transport/connection.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/transport/mqtt/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/transport/mqtt/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/transport/mqtt/mqtt_connection.py:Zone.Identifier b/IoT/iot_device_sdk_python/transport/mqtt/mqtt_connection.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/transport/raw_message.py:Zone.Identifier b/IoT/iot_device_sdk_python/transport/raw_message.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/transport/raw_message_listener.py:Zone.Identifier b/IoT/iot_device_sdk_python/transport/raw_message_listener.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/utils/__init__.py:Zone.Identifier b/IoT/iot_device_sdk_python/utils/__init__.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/IoT/iot_device_sdk_python/utils/iot_util.py:Zone.Identifier b/IoT/iot_device_sdk_python/utils/iot_util.py:Zone.Identifier deleted file mode 100644 index e69de29..0000000 diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py index 4ebf2fb..f656d81 100644 --- a/Massage/MassageControl/MassageRobot_nova5.py +++ b/Massage/MassageControl/MassageRobot_nova5.py @@ -1,49 +1,140 @@ -from hardware.dobot_nova5 import dobot_nova5 -from hardware.force_sensor import XjcSensor - -from algorithms.arm_state import ArmState -from algorithms.controller_manager import ControllerManager -from algorithms.admittance_controller import AdmittanceController -from algorithms.position_controller import PositionController - -from tools.log import CustomLogger -from tools.yaml_operator import read_yaml -from tools.Rate import Rate +from functools import wraps +import sys import numpy as np import threading import time +from typing import List, Literal, Union + +from scipy.interpolate import interp1d + +import requests import os from scipy.spatial.transform import Rotation as R import ctypes libc = ctypes.CDLL("libc.so.6") import atexit -from tools.Trajectory import TrajectoryInterpolator +import copy + from scipy.spatial.transform import Slerp + + +try: + # 导入算法 + from .algorithms.arm_state import ArmState + from .algorithms.controller_manager import ControllerManager + from .algorithms.admittance_controller import AdmittanceController + from .algorithms.hybrid_controller import HybridController + from .algorithms.admithybrid_controller import AdmitHybridController + from .algorithms.position_controller import PositionController + from .algorithms.hybridPid_controller import HybridPidController + from .algorithms.hybridAdmit_controller import HybridAdmitController + from .algorithms.positionerSensor_controller import PositionerSensorController + from .algorithms.admittanceZ_controller import AdmittanceZController + from .algorithms.interpolation import linear_interpolate,spline_interpolate,circle_trajectory,cloud_point_interpolate,oscillation_wrench_interpolate,linear_wrench_interpolate + from .algorithms.force_planner import ForcePlanner + + # 导入硬件 + from .hardware.dobot_nova5 import dobot_nova5 + from .hardware.force_sensor import XjcSensor + + # 导入工具 + from .tools.log import CustomLogger + from .tools.Rate import Rate + from .tools.yaml_operator import read_yaml + from .tools.decorator_tools import custom_decorator,apply_decorators + from .tools.serial_tools import find_serial_by_location + from .tools.Trajectory import TrajectoryInterpolator +except: + #导入算法 + from algorithms.arm_state import ArmState + from algorithms.controller_manager import ControllerManager + from algorithms.admittance_controller import AdmittanceController + from algorithms.admittanceZ_controller import AdmittanceZController + from algorithms.hybrid_controller import HybridController + from algorithms.admithybrid_controller import AdmitHybridController + from algorithms.position_controller import PositionController + from algorithms.hybridPid_controller import HybridPidController + from algorithms.hybridAdmit_controller import HybridAdmitController + from algorithms.positionerSensor_controller import PositionerSensorController + from algorithms.interpolation import linear_interpolate,spline_interpolate,circle_trajectory,cloud_point_interpolate,oscillation_wrench_interpolate,linear_wrench_interpolate + from algorithms.force_planner import ForcePlanner + + # 导入硬件 + from hardware.dobot_nova5 import dobot_nova5 + from hardware.force_sensor import XjcSensor + + # 导入工具 + from tools.log import CustomLogger + from tools.Rate import Rate + from tools.yaml_operator import read_yaml + from tools.decorator_tools import custom_decorator,apply_decorators + from tools.serial_tools import find_serial_by_location + from tools.Trajectory import TrajectoryInterpolator + +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation + +current_file_path = os.path.abspath(__file__) +MassageRobot_Dobot_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path))) +print(MassageRobot_Dobot_Path) +# 添加目标文件夹到系统路径 +sys.path.append(MassageRobot_Dobot_Path) +from VortXDB.client import VTXClient + +""" +在 Python 中，atexit.register 用于注册程序退出时要执行的函数。atexit 模块提供了一个简单的接口，用于在程序正常终止时执行清理操作。 +当你注册多个函数时，它们的执行顺序遵循先进后出的顺序（LIFO：Last In, First Out）。也就是说，最后注册的函数会最先执行。 +""" + +def track_function(function_name,log = False): + def before(self, *args, **kwargs): + self.current_function = function_name + if log: + self.logger.log_info(f"Entering function: {function_name}") + def after(self, result, *args, **kwargs): + self.current_function = None + if log: + self.logger.log_info(f"Exiting function: {function_name},code: {result}") + return custom_decorator(before=before, after=after) + +@apply_decorators class MassageRobot: def __init__(self,arm_config_path,is_log=False): - self.logger = CustomLogger( - log_name="测试日志", - log_file="logs/MassageRobot_nova5_test.log", - precise_time=True) + self.logger = CustomLogger() if is_log: - self.logger_data = CustomLogger(log_name="运动数据日志", - log_file="logs/MassageRobot_kinematics_data.log", - precise_time=True) + self.logger_data = CustomLogger() # 日志线程 threading.Thread(target=self.log_thread,daemon=True).start() + + self.vtxdb = VTXClient() self.arm_state = ArmState() self.arm_config = read_yaml(arm_config_path) # arm 实例化时机械臂类内部进行通讯连接 - self.arm = dobot_nova5(self.arm_config['arm_ip']) + self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip']) self.arm.start() + + self.thermotherapy = None + self.shockwave = None + self.stone = None + self.heat = None + self.ion = None + self.force_plan = ForcePlanner() # 传感器 + # self.force_sensor = None self.force_sensor = XjcSensor() - self.force_sensor.connect() - # 控制器初始化(初始化为导纳控制) + # self.force_sensor.connect() + # 控制器,初始为导纳控制 self.controller_manager = ControllerManager(self.arm_state) self.controller_manager.add_controller(AdmittanceController,'admittance',self.arm_config['controller'][0]) - self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][1]) + self.controller_manager.add_controller(HybridController,'hybrid',self.arm_config['controller'][1]) + self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][2]) + self.controller_manager.add_controller(AdmitHybridController,'admithybrid',self.arm_config['controller'][3]) + self.controller_manager.add_controller(HybridPidController,'hybridPid',self.arm_config['controller'][4]) + self.controller_manager.add_controller(HybridAdmitController,'hybridAdmit',self.arm_config['controller'][5]) + self.controller_manager.add_controller(PositionerSensorController,'positionerSensor',self.arm_config['controller'][6]) + self.controller_manager.add_controller(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7]) + self.controller_manager.switch_controller('admittance') # 按摩头参数暂时使用本地数据 massage_head_dir = self.arm_config['massage_head_dir'] @@ -84,8 +175,8 @@ class MassageRobot: self.skip_envent = threading.Event() self.skip_envent.clear() # 跳过初始化为False # 按摩调整 - self.adjust_wrench_event = threading.Event() - self.adjust_wrench_event.clear() # 调整初始化为False + self.adjust_wrench_envent = threading.Event() + self.adjust_wrench_envent.clear() # 调整初始化为False self.pos_increment = np.zeros(3,dtype=np.float64) self.adjust_wrench = np.zeros(6,dtype=np.float64) @@ -123,6 +214,114 @@ class MassageRobot: # --- self.last_time = -1 self.cur_time = -1 + + + def sensor_set_zero(self): + self.arm_position_set0,self.arm_orientation_set0 = self.arm.get_arm_position() + arm_orientation_set0 = self.arm_orientation_set0 + print("arm_orientation_set0",R.from_quat(arm_orientation_set0).as_euler('xyz',degrees=True)) + self.b_rotation_s_set0 = R.from_quat(arm_orientation_set0).as_matrix() + ######### + max_retries = 5 + #no_head_wrong=np.array([-80.01506042, 116.34187317, -87.65788269, -0.32910481, -0.65792173, -0.61110526])#旧版传感器） + head_set0=np.array([0, 0, 0, 0, 0, 0])#新版传感器） + min_tolerance=0.001 + retry_count = 0 + self.force_sensor.disable_active_transmission() + time.sleep(0.5) + while retry_count < max_retries: + # 调用读取数据的函数，假设返回值是一个 numpy 数组 + data = self.force_sensor.read_data_f32() + data = self.force_sensor.read_data_f32() + data = self.force_sensor.read_data_f32() + + # 检查返回值是否为 np.array 类型 + if isinstance(data, np.ndarray): + # 判断数据是否接近目标值 + + if np.allclose(data, head_set0, atol=min_tolerance): + print(f"检测到力传感器已置零：{data}") + break + else: + print(f"检测到力传感器未置零：{data}") + + # 禁用传感器传输 + self.force_sensor.disable_active_transmission() + self.force_sensor.disable_active_transmission() + time.sleep(0.5) + + # 尝试设置传感器清零 + code = self.force_sensor.set_zero() + code = self.force_sensor.set_zero() + max_try = 3 + + while code != 0 and max_try > 0: + self.logger.log_error("Force sensor set zero failed, retrying...") + self.force_sensor.disable_active_transmission() + self.force_sensor.disable_active_transmission() + time.sleep(0.5) + code = self.force_sensor.set_zero() + code = self.force_sensor.set_zero() + max_try -= 1 + time.sleep(0.5) + + # 如果多次尝试后失败 + if max_try == 0: + self.logger.log_error("Force sensor set zero failed, exiting...") + requests.post( + "http://127.0.0.1:5000/on_message", data={"message": "传感器初始化失败"} + ) + requests.post( + "http://127.0.0.1:5000/update_massage_status", data={"massage_service_started": False} + ) + # sys.exit(0) + return -1 # 置零失败 + + # 成功后跳出主循环 + break + + else: + self.logger.log_error("读取数据失败或格式错误，尝试重试...") + + # 增加重试次数 + retry_count += 1 + self.force_sensor.disable_active_transmission() + self.force_sensor.disconnect() + time.sleep(1) + self.force_sensor = XjcSensor() + time.sleep(1) # 每次重试之间添加一个短暂的延迟 + + if retry_count == max_retries: + self.logger.log_error(f"超过最大重试次数 ({max_retries})，操作失败，退出程序...") + requests.post( + "http://127.0.0.1:5000/on_message", data={"message": "传感器初始化失败"} + ) + requests.post( + "http://127.0.0.1:5000/update_massage_status", data={"massage_service_started": False} + ) + return -2 #读取失败 + + return 0 #读取成功并置零成功 + + def sensor_enable(self): + code = self.force_sensor.enable_active_transmission() + max_try = 3 + while code!= 0 and max_try > 0: + self.logger.log_error("Force sensor enable_active_transmission failed,retrying...") + code = self.force_sensor.enable_active_transmission() + max_try -= 1 + time.sleep(0.1) + if max_try == 0: + self.logger.log_error("Force sensor enable_active_transmission failed,exiting...") + requests.post( + "http://127.0.0.1:5000/on_message", data={"message": "传感器初始化失败"} + ) + requests.post( + "http://127.0.0.1:5000/update_massage_status", data={"massage_service_started": False} + ) + # sys.exit(0) + return -1 + return 0 # 预测步骤 def kalman_predict(self,x, P, Q): @@ -131,7 +330,7 @@ class MassageRobot: # 预测误差协方差 P_predict = P + Q return x_predict, P_predict - + # 更新步骤 def kalman_update(self,x_predict, P_predict, z, R): # 卡尔曼增益 @@ -441,6 +640,919 @@ class MassageRobot: force = rot_matrix.T @ temp_force torque = rot_matrix.T @ np.cross(temp_force,vector_p) + rot_matrix.T @ torque return np.concatenate([force, torque]) + + @track_function("set_position",True) + def set_position(self,pose,is_wait = False): + self.logger.log_info(f"set postion:{pose}") + x,y,z = pose[0],pose[1],pose[2] + roll,pitch,yaw = pose[3],pose[4],pose[5] + pose_command = np.concatenate([[x,y,z], [roll,pitch,yaw]]) + time.sleep(0.1) + code = self.arm.RunPoint_P_inPose_M_RAD(poses_list = pose_command) + time.sleep(0.1) + return code + + @track_function("move_to_point",True) + def move_to_point(self, t, pose, is_interrupt=True, wait=True, timeout=0.5, interpolation:Literal["linear","circle","cloud_point"] ='linear', algorithm:Literal["admittance","position","hybrid","admithybrid","hybridPid","hybridAdmit","admittanceZ"] = "admittance",is_switch_controller = False): + """ + 移动到指定的点 + param: + t: 时间或时间序列 + pose(nx6): 世界坐标系下的位姿或位姿序列,当输入为None时,表示保持当前位姿 + is_interrupt: 是否允许中断 + wait: 是否等待运动完成 + timeout: 超时时间 + interpolation: 插值方式 + algorithm: 控制算法 + return: + code: 0表示成功 + 1表示超时 + 2表示用户打断 + 3表示硬件错误 + 4表示参数错误 + """ + self.move_to_point_count += 1 + self.logger.log_blue(f"move_to_point_count: {self.move_to_point_count}") + self.logger.log_yellow(f"move_to_point: {pose[-1]}") + # 在函数内部直接规划轨迹 + traj = self.traj_generate(t, pose, None, interpolation=interpolation) + if traj is None: + self.logger.log_error("轨迹未生成或生成错误") + return 4 + + self.logger.log_yellow(f"traj_quat: {R.from_quat(traj['quat'][-1]).as_euler('xyz',degrees=False)}") + self.logger.log_yellow(f"traj_pos: {traj['pos'][-1]}") + + time_seq = traj['t'] + pos_traj = traj['pos'] + quat_traj = traj['quat'] + + if is_switch_controller == True: + try: + self.controller_manager.switch_controller(algorithm) + except ValueError as error: + self.logger.log_error(error) + self.logger.log_error("切换到%s控制器失败" % algorithm) + return 4 + self.logger.log_info("切换到%s控制器" % algorithm) + + # self.logger.log_info("当前为%s控制器" % algorithm) + + for i in range(len(time_seq)): + if self.interrupt_event.is_set(): + self.interrupt_event.clear() + self.logger.log_error("self.interrupt_event.clear()") + self.arm_state.desired_position = copy.deepcopy(self.arm_state.arm_position) + self.arm_state.desired_orientation = copy.deepcopy(self.arm_state.arm_orientation) + massage_wrench = np.array([0, 0, 0, 0, 0, 0]) + self.arm_state.desired_wrench = massage_wrench + return 2 + if self.pause_envent.is_set(): + self.pause_envent.clear() + self.logger.log_error("self.pause_envent.clear()") + self.arm_state.desired_position = copy.deepcopy(self.arm_state.arm_position) + self.arm_state.desired_orientation = copy.deepcopy(self.arm_state.arm_orientation) + massage_wrench = np.array([0, 0, 0, 0, 0, 0]) + self.arm_state.desired_wrench = massage_wrench + return 5 + if self.skip_envent.is_set(): + self.logger.log_error("self.skip_envent.clear()") + self.arm_state.desired_position = copy.deepcopy(self.arm_state.arm_position) + self.arm_state.desired_orientation = copy.deepcopy(self.arm_state.arm_orientation) + massage_wrench = np.array([0, 0, 0, 0, 0, 0]) + self.arm_state.desired_wrench = massage_wrench + return 6 + if self.arm.is_exit or self.exit_event.is_set(): + print("robot_hardware_error:",self.arm.is_exit,self.exit_event.is_set()) + return 3 + if self.adjust_wrench_envent.is_set() and self.is_execute == True: + if self.adjust_wrench is not None: + self.arm_state.desired_wrench = self.adjust_wrench + self.adjust_wrench_envent.clear() + + self.arm_state.desired_position = pos_traj[i] + self.arm_state.desired_orientation = quat_traj[i] + + if self.ion is not None: + + act_position = R.from_quat(self.arm_state.arm_orientation).as_matrix().T @ self.arm_state.arm_position + desired_position = R.from_quat(self.arm_state.arm_orientation).as_matrix().T @ self.arm_state.desired_position + if self.arm_state.positionerSensorData < 0.001: + desired_position[2] = desired_position[2] - self.arm_state.desired_high + else: + desired_position[2] = min(act_position[2] + self.arm_state.positionerSensorData - self.arm_state.desired_high , desired_position[2] - self.arm_state.desired_high) + self.arm_state.desired_position = R.from_quat(self.arm_state.arm_orientation).as_matrix() @ desired_position + self.command_rate.precise_sleep() + + start = time.time() + + while wait: + if self.interrupt_event.is_set(): + self.interrupt_event.clear() + return 2 + if self.pause_envent.is_set(): + self.pause_envent.clear() + return 5 + if self.skip_envent.is_set(): + # self.skip_envent.clear() + return 6 + if self.arm.is_exit or self.exit_event.is_set(): + print("robot_hardware_error:",self.arm.is_exit,self.exit_event.is_set()) + return 3 + if time.time() - start > timeout: + return 1 + self.command_rate.precise_sleep() + + return 0 + + @track_function("apply_force",True) + def apply_force(self, t, wrench, interpolation:Literal["linear","oscillation"] = "linear", algorithm:Literal["admittance","position","hybrid","admithybrid","hybridPid","hybridAdmit","admittanceZ"] = "admittance"): + """ + 施加力 + param: + t: 时间或时间序列 + wrench(nx6): 力矩或力矩序列,当输入为None时,表示力为[0,0,0,0,0,0] + is_interrupt: 是否允许中断 + wait: 是否等待运动完成 + timeout: 超时时间 + interpolation: 插值方式 + algorithm: 控制算法 + return: + code: 0表示成功 + 1表示超时 + 2表示用户打断 + 3表示硬件错误 + 4表示参数错误 + """ + + if t==0: + self.arm_state.desired_wrench = wrench + return 0 + + # 在函数内部直接规划力矩轨迹 + traj = self.wrench_traj_generate(t, wrench, interpolation=interpolation) + # # 在函数内部直接规划力矩轨迹 + # traj = self.traj_generate(t, None, wrench, interpolation=interpolation) + if traj is None: + return 4 + + time_seq = traj['t'] + wrench_traj = traj['wrench'] + + # try: + # self.controller_manager.switch_controller(algorithm) + # except ValueError as error: + # self.logger.log_error(error) + # return 4 + + self.logger.log_info("切换到%s控制器" % algorithm) + + for i in range(len(time_seq)): + if self.interrupt_event.is_set(): + self.interrupt_event.clear() + return 2 + if self.pause_envent.is_set(): + self.pause_envent.clear() + return 5 + if self.skip_envent.is_set(): + self.skip_envent.clear() + return 6 + if self.arm.is_exit or self.exit_event.is_set(): + return 3 + if self.adjust_wrench_envent.is_set(): + # if wrench_traj is not None: + # wrench_traj[i:] = self.adjust_wrench + self.adjust_wrench_envent.clear() + # print(wrench_traj[i]) + self.arm_state.desired_wrench = wrench_traj[i] if wrench_traj is not None else np.zeros(6, dtype=np.float64) + self.command_rate.precise_sleep() + + return 0 + + @track_function("resample_curve_strict",True) + def resample_curve_strict(self, points, num_resampled_points): + """ + 修正的曲线重新采样函数，移除重复点并确保累积弧长严格递增。 + """ + # 计算累积弧长 + distances = np.sqrt(np.sum(np.diff(points, axis=0)**2, axis=1)) # 计算相邻点之间的距离 + cumulative_length = np.insert(np.cumsum(distances), 0, 0) # 计算累积弧长 + + # 确保累积弧长严格递增（去除重复值） + unique_indices = np.where(np.diff(cumulative_length, prepend=-np.inf) > 0)[0] + cumulative_length = cumulative_length[unique_indices] + points = points[unique_indices] + + # 生成均匀间隔的新弧长 + target_lengths = np.linspace(0, cumulative_length[-1], num_resampled_points) + + # 在累积弧长上进行插值计算新点位置 + x_interp = interp1d(cumulative_length, points[:, 0], kind='linear', fill_value="extrapolate") + y_interp = interp1d(cumulative_length, points[:, 1], kind='linear', fill_value="extrapolate") + + new_x = x_interp(target_lengths) + new_y = y_interp(target_lengths) + + return np.column_stack((np.round(new_x).astype(int), np.round(new_y).astype(int))) + + @track_function("generate_in_spiral_cloudPoint",True) + def generate_in_spiral_cloudPoint(self, start, end, num_cycle_factor=None, width=None, direction:Literal["CW","CCW"] ='CW'): + """ + 绘制由内向外的螺旋线。 + + 参数: + - start: 起点坐标 (x, y) + - end: 终点坐标 (x, y) + - num_cycle_factor: 螺旋的圈数 + - points: 螺旋线上的点的数量 + """ + # 计算起点和终点之间的距离 + dx = end[0] - start[0] + dy = end[1] - start[1] + distance = np.sqrt(dx**2 + dy**2) + if width is None: + width = self.width_default + if num_cycle_factor is None: + num_cycle_factor = 5 + + num_cycle_factor = np.round(num_cycle_factor).astype(int) + if distance < 2: + print("距离过短，无法绘制螺旋线") + return 0 + if width <= 2: + width = 2 + if num_cycle_factor < 1: + print("圈数必须为大于1的整数") + num_cycle_factor = 1 + angle = np.arctan2(dy, dx) # 计算从起点到终点的角度 + + points = np.round(num_cycle_factor *2*np.pi/(np.pi/9)).astype(int) + + # 螺旋的角度范围 + theta = np.linspace(0, 2 * np.pi * num_cycle_factor, points) + + # 螺旋的半径随着角度增大而线性增长 + r = np.linspace(0, distance, points) + + tempK = np.abs(width/(distance*((2*num_cycle_factor-0.75)/num_cycle_factor))) + + # 计算螺旋的x, y坐标 + if direction == "CCW": + # 反向旋转时，使用负的角度来反向螺旋 + x = r * np.cos(-theta) # 逆时针旋转 + y = r * np.sin(-theta)*tempK + else: + # 正常旋转 + x = r * np.cos(theta) + y = r * np.sin(theta)*tempK + + # 旋转螺旋线以对齐起点和终点的角度 + x_rotated = x * np.cos(angle) - y * np.sin(angle) + y_rotated = x * np.sin(angle) + y * np.cos(angle) + + # 平移螺旋线，使其从起点开始 + x_final = x_rotated + start[0] - x_rotated[0] + y_final = y_rotated + start[1] - y_rotated[0] + + # return np.hstack((np.round(x_final).astype(int).reshape(-1, 1), np.round(y_final).astype(int).reshape(-1, 1))) + result_points = np.hstack((np.round(x_final).astype(int).reshape(-1, 1), + np.round(y_final).astype(int).reshape(-1, 1))) + + return self.resample_curve_strict(points=result_points, num_resampled_points=len(result_points)) + + @track_function("generate_out_spiral_cloudPoint",True) + def generate_out_spiral_cloudPoint(self,start, end, num_cycle_factor=None, width=None, direction:Literal["CW","CCW"] ='CW'): + """ + 绘制由外向内的螺旋线。 + + 参数: + - start: 起点坐标 (x, y) + - end: 终点坐标 (x, y) + - num_cycle_factor: 螺旋的圈数 + - points: 螺旋线上的点的数量 + """ + # 计算起点和终点之间的距离 + dx = end[0] - start[0] + dy = end[1] - start[1] + angle = np.arctan2(dy, dx) # 计算从起点到终点的角度 + distance = np.sqrt(dx**2 + dy**2) + if width is None: + width = self.width_default + if num_cycle_factor is None: + num_cycle_factor = 5 + + num_cycle_factor = np.round(num_cycle_factor).astype(int) + if distance < 2: + print("距离过短，无法绘制螺旋线") + return 0 + if width <= 2: + width = 2 + if num_cycle_factor < 1: + print("圈数必须为大于1的整数") + num_cycle_factor = 1 + + points = np.round(num_cycle_factor *2*np.pi/(np.pi/9)).astype(int) + + # 螺旋的角度范围 + theta = np.linspace(0, 2 * np.pi * num_cycle_factor, points) + + # 螺旋的半径随着角度增大而线性增长 + r = np.linspace(distance, 0, points) + + tempK = np.abs(width/(distance*((2*num_cycle_factor-0.75)/num_cycle_factor))) + + # 计算螺旋的x, y坐标 + if direction == "CCW": + # 反向旋转时，使用负的角度来反向螺旋 + x = r * np.cos(-theta) # 逆时针旋转 + y = r * np.sin(-theta)*tempK + else: + # 正常旋转 + x = r * np.cos(theta) + y = r * np.sin(theta)*tempK + + # 旋转螺旋线以对齐起点和终点的角度 + x = x * np.cos(np.pi) - y * np.sin(np.pi) + y = x * np.sin(np.pi) + y * np.cos(np.pi) + + # 旋转螺旋线以对齐起点和终点的角度 + x_rotated = x * np.cos(angle) - y * np.sin(angle) + y_rotated = x * np.sin(angle) + y * np.cos(angle) + + # 平移螺旋线，使其从起点开始 + x_final = x_rotated + start[0] - x_rotated[0] + y_final = y_rotated + start[1] - y_rotated[0] + + # return np.hstack((np.round(x_final).astype(int).reshape(-1, 1), np.round(y_final).astype(int).reshape(-1, 1))) + result_points = np.hstack((np.round(x_final).astype(int).reshape(-1, 1), + np.round(y_final).astype(int).reshape(-1, 1))) + + return self.resample_curve_strict(points=result_points, num_resampled_points=len(result_points)) + + @track_function("generate_prolateCycloid_cloudPoint",True) + def generate_prolateCycloid_cloudPoint(self, start, end, width=None, num_cycle_factor=None, direction: Literal["CW", "CCW"] = 'CW'): + # Parameters + step_degree = np.pi / 12 + a_prolate = 4.5 + radius = 30.0 + distance = np.linalg.norm(np.array(end) - np.array(start)) + angle = np.arctan2(end[1] - start[1], end[0] - start[0]) + + if width is None: + width = self.width_default + if num_cycle_factor is None: + num_cycle_factor = 5 + num_cycle_factor = np.round(num_cycle_factor).astype(int) + if distance < 2: + print("距离过短，无法绘制Cycloid线") + return 0 + if width <= 2: + width = 2 + if num_cycle_factor < 1: + print("圈数必须为大于1的整数") + num_cycle_factor = 1 + + r_cycloid = radius / (2 * a_prolate) + num_cycles = int(num_cycle_factor) + temp_k = distance / (r_cycloid * (np.pi + 2 * a_prolate + 2 * np.pi * num_cycles)) + + # Generate the prolate cycloid + theta_prolate = np.linspace(0.5 * np.pi, num_cycles * 2 * np.pi + 1.5 * np.pi, + round((num_cycles * 2 * np.pi + 1.5 * np.pi) / step_degree)) + if direction == 'CW': + theta_prolate = np.flip(theta_prolate) + + x_prolate = r_cycloid * (theta_prolate - a_prolate * np.sin(theta_prolate)) * temp_k + y_prolate = r_cycloid * (1 - a_prolate * np.cos(theta_prolate)) + + # Scale y-prolate based on width + y_prolate = (y_prolate / 30) * width + + # Rotation of cycloid + if direction == 'CW': + x_prolate, y_prolate = -x_prolate, -y_prolate + + x_rotated = x_prolate * np.cos(angle) - y_prolate * np.sin(angle) + y_rotated = x_prolate * np.sin(angle) + y_prolate * np.cos(angle) + + # Translate to starting point + x_final = x_rotated + start[0] - x_rotated[0] + y_final = y_rotated + start[1] - y_rotated[0] + + return np.column_stack((np.round(x_final).astype(int), np.round(y_final).astype(int))) + + @track_function("generate_ellipse_cloudPoint",True) + def generate_ellipse_cloudPoint(self,start, end, num_cycle_factor=None, width=None, direction:Literal["CW","CCW"] ='CW'): + # 计算椭圆的中心（两个端点的中点） + center = (start + end) / 2 + if num_cycle_factor is None: + num_cycle_factor = 1 + + num_cycle_factor = np.round(num_cycle_factor).astype(int) + + distance = np.sqrt((end[0] - start[0])**2 + (end[1] - start[1])**2) + + if width is None: + width = distance/2 + + + if distance < 2: + print("距离过短，无法绘制椭圆") + return 0 + if width <= 2: + print("椭圆半径过小，无法绘制椭圆") + width = self.width_default + if num_cycle_factor < 1: + print("圈数必须为大于1的整数") + num_cycle_factor = 1 + + # 计算两个端点的距离，即长半轴长度 + a = np.sqrt((end[0] - start[0])**2 + (end[1] - start[1])**2) / 2 + + # 假设短半轴长度为常量值radius + b = width / 2 + + # 计算旋转角度（由端点的方向确定） + angle = np.arctan2(end[1] - start[1], end[0] - start[0]) + points = np.round(((num_cycle_factor + 1) * np.pi * 2)/(np.pi/9)).astype(int) + + # 绘制椭圆的参数化公式 + theta = np.linspace(np.pi, (2+num_cycle_factor*2)* np.pi, points) + + # 计算螺旋的x, y坐标 + if direction == "CCW": + # 反向旋转时，使用负的角度来反向螺旋 + x_ellipse = a * np.cos(-theta) #顺时针 + y_ellipse = b * np.sin(-theta) + else: + # 正常旋转 + x_ellipse = a * np.cos(theta) + y_ellipse = b * np.sin(theta) + + # 绕中心旋转椭圆 + x_rot = center[0] + x_ellipse * np.cos(angle) - y_ellipse * np.sin(angle) + y_rot = center[1] + x_ellipse * np.sin(angle) + y_ellipse * np.cos(angle) + + # 返回椭圆点云，保留整数值 + return np.hstack((np.round(x_rot).astype(int).reshape(-1, 1), np.round(y_rot).astype(int).reshape(-1, 1))) + + @track_function("generate_Lemniscate_cloudPoint",True) + def generate_Lemniscate_cloudPoint(self, start, end, width = None,num_cycle_factor = None, direction:Literal["CW","CCW"] ='CW'): + """ + 生成从start到end的8字形插补路径 + + :param start: 起点坐标 (x0, y0) + :param end: 终点坐标 (x1, y1) + :param num_points: 插补点的数量 + :return: 插补路径的坐标列表 + """ + # 起点和终点的坐标 + x1, y1 = start + x0, y0 = end + step_degree=np.pi/12 + + # 计算轨迹的缩放因子和旋转角度 + distance = np.linalg.norm([x1 - x0, y1 - y0]) + + if width is None: + width = self.width_default + if num_cycle_factor is None: + num_cycle_factor = 1 + if distance < 2: + print("距离过短，无法绘制螺旋线") + return 0 + if width <= 2: + width = 2 + if num_cycle_factor < 1: + print("圈数必须为大于1的整数") + num_cycle_factor = 1 + + a = distance / 2 # 轨迹的宽度一半 + b = width/2 + angle = np.arctan2(y1 - y0, x1 - x0) + + num_points = np.abs((2*num_cycle_factor+1)*np.pi/step_degree).astype(int) + + # 生成标准的8字形轨迹 (Gerono卵形线) + t_values = np.linspace(0.5*np.pi, (2*num_cycle_factor + 1.5)* np.pi, num_points) + + x_curve = a * np.sin(t_values) + if direction == 'CCW': + y_curve = -b * np.sin(2 * t_values) + else: + y_curve = b * np.sin(2 * t_values) + + # 构建旋转矩阵 + rotation_matrix = np.array([[np.cos(angle), -np.sin(angle)], + [np.sin(angle), np.cos(angle)]]) + + # 旋转并平移轨迹 + curve_points = np.vstack((x_curve, y_curve)).T + rotated_points = curve_points @ rotation_matrix.T + shifted_points = rotated_points + [(x0 + x1) / 2, (y0 + y1) / 2] + + # 将插值结果四舍五入为整数 + return np.round(shifted_points).astype(int) + + @track_function("generate_circle_cloudPoint",True) + def generate_circle_cloudPoint(self, start_point, center, step_degree=np.pi/12, num_cycle_factor = 3): + """ + center: 圆心坐标，形如 (x_c, y_c) + start_point: 起始点坐标，形如 (x_0, y_0) + radius: 圆的半径 + delta_theta: 每次插补的角度增量 + num_turns: 绕圈的次数 + """ + # 确定总共需要生成的插补点数 + num_points = int((2 * np.pi * num_cycle_factor) / step_degree) + + # 圆心 + x_c, y_c = center + + radius = np.linalg.norm(np.array(start_point) - np.array(center)) # 半径 + + # 计算起始点的初始角度 + x_0, y_0 = start_point + theta_0 = np.arctan2(y_0 - y_c, x_0 - x_c) + + # 初始化存储插补点的列表 + circle_points = [] + + # 生成插补点 + for i in range(num_points): + # 当前角度 + theta_i = theta_0 + i * step_degree + + # 计算插补点的坐标 + x_i = x_c + radius * np.cos(theta_i) + y_i = y_c + radius * np.sin(theta_i) + + # 将点添加到列表中 + + circle_points.append((np.round(x_i).astype(int), np.round(y_i).astype(int))) + + circle_points.append((np.round(x_0).astype(int), np.round(y_0).astype(int))) + + return circle_points + + @track_function("generate_point_cloudPoint",True) + def generate_point_cloudPoint(self, point, num_cycle_factor=None, width=None, direction: Literal["CW", "CCW"] = 'CW'): + if num_cycle_factor is None: + num_cycle_factor = 12 + if num_cycle_factor < 1: + print("圈数必须为大于1的整数") + num_cycle_factor = 1 + num_cycle_factor = np.abs(np.round(num_cycle_factor).astype(int)) + if width is None: + width = 20 + if width < 2: + print("宽度必须为大于2的整数") + width = 2 + width = np.abs(np.round(width).astype(int)) + + radius = width/2 + + points = np.round((num_cycle_factor * np.pi * 2)/(np.pi/9)).astype(int) + thetas = np.linspace(0, num_cycle_factor * 2 * np.pi, points) + radiuss = np.ones_like(thetas) * radius + + for i in range(len(thetas)): + if thetas[i] <= np.pi/2: + radiuss[i] = np.sin(thetas[i]) * radius + if thetas[i] >= num_cycle_factor * np.pi * 2 - np.pi / 2: + radiuss[i] = np.sin(num_cycle_factor * np.pi * 2 - thetas[i]) * radius + + + + if direction == "CCW": + x_ellipse = radiuss * np.cos(-thetas) + y_ellipse = radiuss * np.sin(-thetas) + else: + x_ellipse = radiuss * np.cos(thetas) + y_ellipse = radiuss * np.sin(thetas) + x_rot = point[0] + x_ellipse + y_rot = point[1] + y_ellipse + result_points = np.hstack((np.round(x_rot).astype(int).reshape(-1, 1), np.round(y_rot).astype(int).reshape(-1, 1))) + return self.resample_curve_strict(points=result_points, num_resampled_points=len(result_points)) + + @track_function("generate_line_cloudPoint",True) + def generate_line_cloudPoint(self,start, end, step_distance = 5): + """ + 生成起点和终点之间的线性插值点 + + 参数: + start: 起点 (标量或数组，例如 [x1, y1, z1]) + end: 终点 (标量或数组，例如 [x2, y2, z2]) + num_points: 插值点的数量（包括起点和终点） + + 返回: + 包含线性插值点的数组 + """ + start = np.array(start) + end = np.array(end) + distance = np.linalg.norm(end - start) + num_points = int(distance / step_distance) + + if num_points <=2: + num_points = 2 + # 使用 numpy 的 linspace 在起点和终点之间生成线性插值点 + interpolated_points = np.linspace(start, end, num_points) + + # 将插值结果四舍五入为整数 + return np.round(interpolated_points).astype(int) + + @track_function("generate_repeat_line_cloudPoint",True) + def generate_repeat_line_cloudPoint(self, start_point, end_point, num_cycle_factor = 5): + """ + start_point: 起始点坐标，形如 (x_0, y_0) + end_point: 终点坐标，形如 (x_0, y_0) + num_cycle_factor: 重复的次数 + """ + + # 初始化存储插补点的列表 + result_points = [] + # 生成插补点 + for i in range(num_cycle_factor): + # 将点添加到列表中 + result_points.append(start_point) + result_points.append(end_point) + return np.round(np.array(result_points)).astype(int) + + @track_function("generate_finger_circle_cloudPoint",True) + def generate_finger_circle_cloudPoint(self, center_point, num_cycle_factor=5, radius=30,deviation=15): + """ + 生成一指禅绕球心的圆弧轨迹 + center_point: 圆心坐标 [x, y, z, roll, pitch, yaw] + num_cycle_factor: 轨迹的重复次数 + radius: 圆的半径 + """ + result_points = [] + result_points.append(copy.deepcopy(center_point)) + deviation = deviation * np.pi/180 + + totalCounts = int(360 / radius) + shift = 1 + + + for i in range(num_cycle_factor): + for countR in range(0, totalCounts): + # 当前旋转矩阵 + temp_pose = copy.deepcopy(center_point) + rotation = R.from_euler('xyz', temp_pose[3:], degrees=False) + current_rotation_matrix = rotation.as_matrix() + + + if i == 0: + shift = countR/(90/radius) if countR/(90/radius) < 1 else 1 + if i == num_cycle_factor-1: + shift = (totalCounts-countR)/(90/radius) if (totalCounts-countR)/(90/radius) < 1 else 1 + + + # 生成增量旋转矩阵 + increment_rotation= R.from_euler('xyz', [deviation*shift,0,countR * np.pi*2 /totalCounts], degrees=False).as_matrix() + + # 更新旋转矩阵 + updated_rotation_matrix = current_rotation_matrix @ increment_rotation + + # 将旋转矩阵转换回欧拉角 + temp_pose[3:] = R.from_matrix(updated_rotation_matrix).as_euler('xyz', degrees=False) + + # 添加当前点到结果列表 + result_points.append(copy.deepcopy(temp_pose)) + + result_points.append(copy.deepcopy(center_point)) + return result_points + + def traj_generate(self, t: Union[int, float, List[float]], pose = None, wrench = None, interpolation: Literal["linear", "cubic","circle","cloud_point"] = 'linear',**kwargs): + """ + 轨迹生成 + param: + t: 时间或时间序列 + pose(nx6): 世界坐标系下的位姿或位姿序列,当输入为None时,表示保持当前位姿 + wrench(nx6): 力矩或力矩序列,当输入为None时,表示保持当前的力 + interpolation: 插值方式 + return: + traj: 生成的轨迹 -> dict + """ + # 确保时间输入是列表 + if isinstance(t, (int, float)): + t = [t] + elif not isinstance(t, list) or any(not isinstance(i, (int, float)) for i in t): + self.logger.log_error("not isinstance(t, list) or any(not isinstance(i, (int, float)) for i in t)") + return None + # 检查时间点数量 + if len(t) < 1: + self.logger.log_error("len(t) < 1") + return None + # 时间序列 + time_points = [0.0] + t + time_points = np.array(time_points) + dt = self.command_rate.to_sec() + + for i in range(len(time_points)): + time_points[i] = np.round(time_points[i] / dt) * dt + if np.abs(time_points[-1]) < dt: + time_points[-1] = dt + # 由于浮点数精度问题，time_step要乘0.9 + time_seq = np.arange(time_points[0], time_points[-1] + dt * 0.9, dt) + pos_traj = None + quat_traj = None + wrench_traj = None + pos_vel = None + angular_vel = None + # 位置轨迹与速度轨迹生成 + if pose is not None: + if interpolation != 'cloud_point': + pose = np.array(pose).reshape(-1, 6) + if pose.shape[0] != len(t): + self.logger.log_error("pose.shape[0] != len(t)") + print(pose.shape[0],len(t)) + return None + pose_matrix = self.convert_to_7d(pose) + else: + pose = np.array(pose) + try: + # 插值 + if interpolation == 'linear': + pos_traj, quat_traj = linear_interpolate(positions=pose_matrix[:, :3], quaternions=pose_matrix[:, 3:], time_points=time_points, time_step=dt) + elif interpolation == 'cubic': + pos_traj, quat_traj = spline_interpolate(positions=pose_matrix[:, :3], quaternions=pose_matrix[:, 3:], time_points=time_points, time_step=dt) + elif interpolation == 'circle': + if self.ion is not None: + pos_traj = circle_trajectory(center=pose, radius=0.07,time_points=time_points,time_step=dt,**kwargs) + else: + pos_traj = circle_trajectory(center=pose,time_points=time_points,time_step=dt,**kwargs) + quat_traj = np.tile(R.from_euler('xyz', np.array(pose[0][3:])).as_quat(), (pos_traj.shape[0], 1)) + elif interpolation == 'cloud_point': + pos_traj, quat_traj = cloud_point_interpolate(positions=pose[:, :3], quaternions=pose[:, 3:], time_points=time_points, time_step=dt) + # self.logger.log_yellow(f'{pos_traj[0]} {pos_traj[-1]}') + except ValueError as error: + self.logger.log_error(error) + return None + else: + self.logger.log_error("未输入位置进行规划") + + # TODO 力轨迹生成 改为线性增加 + if wrench is not None: + wrench = np.array(wrench).reshape(-1, 6) + if wrench.shape[0] != len(t): + return None + indices = np.searchsorted(time_points, time_seq, side='left') - 1 + indices = np.clip(indices, 0, len(wrench) - 1) + wrench_traj = wrench[indices] + # 构建返回数据字典 + traj = { + 't': time_seq, + 'pos': pos_traj, + 'quat': quat_traj, + 'pos_vel': pos_vel, + 'angular_vel': angular_vel, + 'wrench': wrench_traj + } + return traj + + + def wrench_traj_generate(self, t: Union[int, float, List[float]], wrench = None, interpolation: Literal["linear", "oscillation"] = 'linear',**kwargs): + """ + 力/力矩轨迹生成 + param: + t: 时间或时间序列 + wrench(nx6): 力矩或力矩序列 + interpolation: 插值方式 + return: + traj: 生成的轨迹 -> dict + """ + # 确保时间输入是列表 + if isinstance(t, (int, float)): + t = [t] + elif not isinstance(t, list) or any(not isinstance(i, (int, float)) for i in t): + self.logger.log_error("not isinstance(t, list) or any(not isinstance(i, (int, float)) for i in t)") + return None + # 检查时间点数量 + if len(t) < 1: + self.logger.log_error("len(t) < 1") + return None + + if wrench is None: + self.logger.log_error("未输入力矩进行规划") + return None + # 时间序列 + time_points = [0.0] + t + time_points = np.array(time_points) + dt = self.command_rate.to_sec() + + for i in range(len(time_points)): + time_points[i] = np.round(time_points[i] / dt) * dt + if np.abs(time_points[-1]) < dt: + time_points[-1] = dt + # 由于浮点数精度问题，time_step要乘0.9 + time_seq = np.arange(time_points[0], time_points[-1] + dt * 0.9, dt) + + wrench_traj = None + + + + + # TODO 力轨迹生成 改为线性增加 + if wrench is not None: + wrench = np.array(wrench).reshape(-1, 6) + + + if wrench.shape[0] != len(t): + return None + if interpolation == 'oscillation': + wrench_traj = oscillation_wrench_interpolate(wrench=wrench, time_points=time_points, time_step=dt) + else: + wrench_traj = linear_wrench_interpolate(wrench=wrench, time_points=time_points, time_step=dt) + + # indices = np.searchsorted(time_points, time_seq, side='left') - 1 + + + # indices = np.clip(indices, 0, len(wrench) - 1) + + # wrench_traj = wrench[indices] + + + # 构建返回数据字典 + traj = { + 't': time_seq, + 'wrench': wrench_traj + } + return traj + + def force_traj_generate(self, t: Union[int, float, List[float]], wrench = None, interpolation: Literal["linear", "oscillation", "auto"] = "auto",**kwargs): + """ + 力/力矩轨迹生成 + param: + t: 时间或时间序列 + wrench(nx6): 力矩或力矩序列 + interpolation: 插值方式 + return: + traj: 生成的轨迹 -> dict + """ + # 确保时间输入是列表 + if isinstance(t, (int, float)): + t = [t] + elif not isinstance(t, list) or any(not isinstance(i, (int, float)) for i in t): + self.logger.log_error("not isinstance(t, list) or any(not isinstance(i, (int, float)) for i in t)") + return None + # 检查时间点数量 + if len(t) < 1: + self.logger.log_error("len(t) < 1") + return None + + if wrench is None: + self.logger.log_error("未输入力矩进行规划") + return None + # 时间序列 + time_points = [0.0] + t + time_points = np.array(time_points) + dt = self.command_rate.to_sec() + + for i in range(len(time_points)): + time_points[i] = np.round(time_points[i] / dt) * dt + if np.abs(time_points[-1]) < dt: + time_points[-1] = dt + # 由于浮点数精度问题，time_step要乘0.9 + time_seq = np.arange(time_points[0], time_points[-1] + dt * 0.9, dt) + + wrench_traj = None + + + + + # TODO 力轨迹生成 改为线性增加 + if wrench is not None: + wrench = np.array(wrench).reshape(-1, 6) + + + if wrench.shape[0] != len(t): + return None + if interpolation == 'oscillation': + wrench_traj = self.force_plan.oscillation_wrench_interpolate(wrench=wrench, time_points=time_points, time_step=dt) + elif interpolation == "auto": + # target_wrench = [] + # target_wrench.append(wrench) + wrench_traj = self.force_plan.S_shaped_wrench_interpolate(start=self.arm_state.desired_wrench, wrench=wrench, time_points=time_points, time_step=dt) + else: + wrench_traj = self.force_plan.linear_wrench_interpolate(wrench=wrench, time_points=time_points, time_step=dt) + + # indices = np.searchsorted(time_points, time_seq, side='left') - 1 + + + # indices = np.clip(indices, 0, len(wrench) - 1) + + # wrench_traj = wrench[indices] + + + # 构建返回数据字典 + traj = { + 't': time_seq, + 'wrench': wrench_traj + } + return traj + + if __name__ == "__main__": waypoints = [ diff --git a/Massage/MassageControl/algorithms/admithybrid_controller.py b/Massage/MassageControl/algorithms/admithybrid_controller.py new file mode 100755 index 0000000..7464db1 --- /dev/null +++ b/Massage/MassageControl/algorithms/admithybrid_controller.py @@ -0,0 +1,135 @@ +import sys +import numpy as np +from scipy.spatial.transform import Rotation as R +from .arm_state import ArmState +from .base_controller import BaseController +from pathlib import Path +sys.path.append(str(Path(__file__).resolve().parent.parent)) +from tools.yaml_operator import read_yaml +import time + +class AdmitHybridController(BaseController): + def __init__(self, name, state: ArmState,config_path) -> None: + super().__init__(name, state) + self.load_config(config_path) + self.laset_print_time = 0 + + + def load_config(self, config_path): + config_dict = read_yaml(config_path) + if self.name != config_dict['name']: + raise ValueError(f"Controller name {self.name} does not match config name {config_dict['name']}") + desired_xi = np.array(config_dict['desired_xi']) + damp_tran = np.array(config_dict['damp_tran']) + # TODO 修改控制率 + self.e_t = 0 + self.e_t1 = 0 + self.e_t2 = 0 + self.force_control_value = 0 + + # 姿态 3x3矩阵 + self.Kp = np.diag(np.array(config_dict['Kp'])) + self.Ki = np.diag(np.array(config_dict['Ki'])) + self.Kd = np.diag(np.array(config_dict['Kd'])) + + self.pose_integral_error = np.zeros(6) + + # 导纳xyz位置调节器 + mass_tran = np.array(config_dict['mass_tran']) + stiff_tran = np.array(config_dict['stiff_tran']) + damp_tran = np.array(config_dict['damp_tran']) + for i in range(3): + if damp_tran[i] < 0: + damp_tran[i] = 2 * desired_xi * np.sqrt(stiff_tran[i] * mass_tran[i]) + self.M_tran = np.diag(mass_tran) + self.M_tran_inv = np.linalg.inv(self.M_tran) + self.K_tran = np.diag(stiff_tran) + self.D_tran = np.diag(damp_tran) + + def step(self,dt): + # 方向统一 + if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + self.state.arm_orientation = -self.state.arm_orientation + # 缓存常用计算 + arm_ori_quat = R.from_quat(self.state.arm_orientation) + arm_ori_mat = arm_ori_quat.as_matrix() + # 位置误差 + temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error + # 姿态误差(四元数) + rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation) + self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) + wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + + self.pose_integral_error += self.state.pose_error * dt + # 姿态pid + self.state.arm_desired_acc[3:] = -self.Kd @ self.state.arm_desired_twist[3:] - self.Kp @ self.state.pose_error[3:] - self.Ki @ self.pose_integral_error[3:] + # 位置导纳控制 + self.state.arm_desired_acc[:3] = self.M_tran_inv @ (wrench_err[:3] - self.D_tran @ (self.state.arm_desired_twist[:3] - self.state.desired_twist[:3])-self.K_tran @ self.state.pose_error[:3]) + + self.clip_command(self.state.arm_desired_acc, "acc") + ## 更新速度和位姿 + self.state.arm_desired_twist += self.state.arm_desired_acc * dt + self.clip_command(self.state.arm_desired_twist, "vel") + # 计算位姿变化 + delta_pose = np.concatenate([ + arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt), + self.state.arm_desired_twist[3:] * dt + ]) + self.clip_command(delta_pose, "pose") + # 更新四元数 + delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat() + arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat) + self.state.arm_orientation_command = arm_ori_quat_new.as_quat() + # 归一化四元数 + self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) + # 更新位置 + self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + + def step_traj(self,dt): + # 方向统一 + if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + self.state.arm_orientation = -self.state.arm_orientation + # 缓存常用计算 + arm_ori_quat = R.from_quat(self.state.arm_orientation) + arm_ori_mat = arm_ori_quat.as_matrix() + # 位置误差 + + temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.desired_twist[:3] * dt + self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error + + # 姿态误差(四元数) + angular_velocity = np.array(self.state.desired_twist[3:]) # 形状 (3,) + + # 用旋转向量（小角度近似） + rotvec = angular_velocity * dt # 旋转向量 = 角速度 × 时间 + rot_quat = R.from_rotvec(rotvec).as_quat() # 转成四元数，形状 (4,) + rot_err_quat = R.from_quat(rot_quat).inv() * arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation) + self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) + # 期望加速度 + wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + + self.pose_integral_error += self.state.pose_error * dt + # 姿态pid + self.state.arm_desired_acc[3:] = -self.Kd @ self.state.arm_desired_twist[3:] - self.Kp @ self.state.pose_error[3:] - self.Ki @ self.pose_integral_error[3:] + # 位置导纳控制 + self.state.arm_desired_acc[:3] = self.M_tran_inv @ (wrench_err[:3] - self.D_tran @ (self.state.arm_desired_twist[:3] - self.state.desired_twist[:3] + self.state.desired_acc[:3]*dt)-self.K_tran @ self.state.pose_error[:3]) + self.state.desired_acc[:3] + + self.clip_command(self.state.arm_desired_acc, "acc") + ## 更新速度和位姿 + self.state.arm_desired_twist += self.state.arm_desired_acc * dt + self.clip_command(self.state.arm_desired_twist, "vel") + # 计算位姿变化 + delta_pose = np.concatenate([ + arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt), + self.state.arm_desired_twist[3:] * dt + ]) + self.clip_command(delta_pose, "pose") + # 更新四元数 + delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat() + arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat) + self.state.arm_orientation_command = arm_ori_quat_new.as_quat() + # 归一化四元数 + self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) + # 更新位置 + self.state.arm_position_command = self.state.arm_position + delta_pose[:3] diff --git a/Massage/MassageControl/algorithms/admittanceZ_controller.py b/Massage/MassageControl/algorithms/admittanceZ_controller.py new file mode 100755 index 0000000..522c4ac --- /dev/null +++ b/Massage/MassageControl/algorithms/admittanceZ_controller.py @@ -0,0 +1,153 @@ +import sys +import numpy as np +from scipy.spatial.transform import Rotation as R +from .base_controller import BaseController +from .arm_state import ArmState +from pathlib import Path +sys.path.append(str(Path(__file__).resolve().parent.parent)) +import time + +from tools.yaml_operator import read_yaml +class AdmittanceZController(BaseController): + def __init__(self, name, state:ArmState,config_path) -> None: + super().__init__(name, state) + self.load_config(config_path) + + + def load_config(self, config_path): + config_dict = read_yaml(config_path) + if self.name != config_dict['name']: + raise ValueError(f"Controller name {self.name} does not match config name {config_dict['name']}") + mass_tran = np.array(config_dict['mass_tran']) + mass_rot = np.array(config_dict['mass_rot']) + stiff_tran = np.array(config_dict['stiff_tran']) + stiff_rot = np.array(config_dict['stiff_rot']) + desired_xi = np.array(config_dict['desired_xi']) + damp_tran = np.array(config_dict['damp_tran']) + damp_rot = np.array(config_dict['damp_rot']) + self.pos_scale_factor = config_dict['pos_scale_factor'] + self.rot_scale_factor = config_dict['rot_scale_factor'] + for i in range(2): + if damp_tran[i] < 0: + damp_tran[i] = 2 * desired_xi * np.sqrt(stiff_tran[i] * mass_tran[i]) + if damp_rot[i] < 0: + damp_rot[i] = 2 * desired_xi * np.sqrt(stiff_rot[i] * mass_rot[i]) + + if damp_rot[2] < 0: + damp_rot[2] = 2 * desired_xi * np.sqrt(stiff_rot[i] * mass_rot[i]) + + self.M_trans = np.diag(mass_tran[:2]) + self.M_rot = np.diag(mass_rot) + self.M_z = mass_tran[2] + self.M = np.diag(np.concatenate([mass_tran, mass_rot])) + self.M_trans_inv = np.linalg.inv(self.M_trans) + self.M_rot_inv = np.linalg.inv(self.M_rot) + + self.K_trans = np.diag(stiff_tran[:2]) + self.K_rot = np.diag(stiff_rot) + self.K_z = stiff_tran[2] + self.K_trans_inv = np.linalg.inv(self.K_trans) + self.K_rot_inv = np.linalg.inv(self.K_rot) + self.D_trans = np.diag(damp_tran[:2]) + self.D_rot = np.diag(damp_rot) + self.D_z = damp_tran[2] + + self.laset_print_time = 0 + + self.pose_integral_error = np.zeros(6) + + def step(self,dt): + # 方向统一 + if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + self.state.arm_orientation = -self.state.arm_orientation + # 缓存常用计算 + arm_ori_quat = R.from_quat(self.state.arm_orientation) + arm_ori_mat = arm_ori_quat.as_matrix() + # 位置误差 + temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error + # 姿态误差(四元数) + rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation) + self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) + # 期望加速度 + wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + self.state.arm_desired_acc[:2] = self.M_trans_inv @ (wrench_err[:2] - self.D_trans @ (self.state.arm_desired_twist[:2] -self.state.desired_twist[:2]) - self.K_trans @ self.state.pose_error[:2]) + self.state.arm_desired_acc[2] = 1/self.M_z * (wrench_err[2] - self.D_z * (self.state.arm_desired_twist[2] -self.state.desired_twist[2]) - self.K_z * self.state.pose_error[2]) + self.state.arm_desired_acc[3:] = self.M_rot_inv @ (wrench_err[3:] - self.D_rot @ (self.state.arm_desired_twist[3:] -self.state.desired_twist[3:]) - self.K_rot @ self.state.pose_error[3:]) + self.clip_command(self.state.arm_desired_acc, "acc") + ## 更新速度和位姿 + self.state.arm_desired_twist += self.state.arm_desired_acc * dt + self.clip_command(self.state.arm_desired_twist, "vel") + # 计算位姿变化 + delta_pose = np.concatenate([ + arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt), + self.state.arm_desired_twist[3:] * dt + ]) + self.clip_command(delta_pose, "pose") + # 更新四元数 + delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat() + arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat) + self.state.arm_orientation_command = arm_ori_quat_new.as_quat() + # 归一化四元数 + self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) + # 更新位置 + self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + + def step_traj(self,dt): + # 方向统一 + if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + self.state.arm_orientation = -self.state.arm_orientation + # 缓存常用计算 + arm_ori_quat = R.from_quat(self.state.arm_orientation) + arm_ori_mat = arm_ori_quat.as_matrix() + # 位置误差 + + temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.desired_twist[:3] * dt + self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error + + # 姿态误差(四元数) + angular_velocity = np.array(self.state.desired_twist[3:]) # 形状 (3,) + + # 用旋转向量（小角度近似） + rotvec = angular_velocity * dt # 旋转向量 = 角速度 × 时间 + rot_quat = R.from_rotvec(rotvec).as_quat() # 转成四元数，形状 (4,) + rot_err_quat = R.from_quat(rot_quat).inv() * arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation) + self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) + + # 期望加速度 + wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + self.state.arm_desired_acc[:2] = self.M_trans_inv @ (wrench_err[:2] - self.D_trans @ (self.state.arm_desired_twist[:2] -self.state.desired_twist[:2] + self.state.desired_acc[:2]*dt) - self.K_trans @ self.state.pose_error[:2]) + self.state.desired_acc[:2] + self.state.arm_desired_acc[2] = 1/self.M_z * (wrench_err[2] - self.D_z * (self.state.arm_desired_twist[2] -self.state.desired_twist[2] + self.state.desired_acc[2]*dt) - self.K_z * self.state.pose_error[2]) + self.state.desired_acc[2] + self.state.arm_desired_acc[3:] = self.M_rot_inv @ (wrench_err[3:] - self.D_rot @ (self.state.arm_desired_twist[3:] -self.state.desired_twist[3:] + self.state.desired_acc[3:]*dt) - self.K_rot @ self.state.pose_error[3:]) + self.state.desired_acc[3:] + self.clip_command(self.state.arm_desired_acc, "acc") + ## 更新速度和位姿 + self.state.arm_desired_twist += self.state.arm_desired_acc * dt + self.clip_command(self.state.arm_desired_twist, "vel") + # 计算位姿变化 + delta_pose = np.concatenate([ + arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt), + self.state.arm_desired_twist[3:] * dt + ]) + self.clip_command(delta_pose, "pose") + # 更新四元数 + delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat() + arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat) + self.state.arm_orientation_command = arm_ori_quat_new.as_quat() + # 归一化四元数 + self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) + # 更新位置 + self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + + + + +if __name__ == "__main__": + state = ArmState() + controller = AdmittanceZController("admittance",state,"/home/zyc/admittance_control/MassageControl/config/admittance.yaml") + print(controller.name) + print(controller.state.arm_position) + state.arm_position = np.array([1,2,3]) + print(controller.state.arm_position) + print(controller.M) + print(controller.D) + print(controller.K) \ No newline at end of file diff --git a/Massage/MassageControl/algorithms/arm_state.py b/Massage/MassageControl/algorithms/arm_state.py index bdcf7e8..4fd4147 100644 --- a/Massage/MassageControl/algorithms/arm_state.py +++ b/Massage/MassageControl/algorithms/arm_state.py @@ -14,6 +14,10 @@ class ArmState: self.last_arm_orientation = np.array([0.0,0.0,0.0,1.0]) # [qx, qy, qz, qw] self.last_external_wrench_base = np.zeros(6,dtype=np.float64) self.last_external_wrench_tcp = np.zeros(6,dtype=np.float64) + + # 传感器数据 + self.positionerSensorData = 0.0 + self.desired_high = 0.06 # 目标状态 self.desired_position = np.zeros(3,dtype=np.float64) diff --git a/Massage/MassageControl/algorithms/force_planner.py b/Massage/MassageControl/algorithms/force_planner.py new file mode 100644 index 0000000..e4ab769 --- /dev/null +++ b/Massage/MassageControl/algorithms/force_planner.py @@ -0,0 +1,273 @@ +import numpy as np +import matplotlib.pyplot as plt +import copy + +class ForcePlanner: + def __init__(self): + self.force_vel_max = np.float64(20) # [0,0, 10, 0, 0, 0] # 10 N/s + self.force_acc_max = np.float64(10) #[0,0, 20, 0, 0, 0] # 20 N/s² + self.force_jerk_max = 90 #[0,0, 30, 0, 0, 0] # 30 N/s³ + self.Ta = self.force_vel_max / self.force_acc_max + + def linear_wrench_interpolate(self,wrench=None, time_points=None, time_step=0.1): + if wrench is None: + raise ValueError("至少需要输入力矩的序列") + if time_points is None or len(time_points) < 2: + raise ValueError("需要提供至少两个时间点") + + # 由于浮点数精度问题，time_step要乘0.9 + times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step) + + if wrench is not None: + all_wrenchs = np.zeros((len(times), wrench.shape[1])) + current_idx = 0 + start_wrench = np.array([0, 0, 0, 0, 0, 0]) + end_wrench = wrench + + for i in range(len(times)): + # 计算 phase + phase = i / len(times) # 使用 i 计算 + + # 计算 segment_wrenchs + segment_wrenchs = start_wrench + phase * (end_wrench - start_wrench) + + # 将 segment_wrenchs 填充到 all_wrenchs + all_wrenchs[current_idx:current_idx + len(segment_wrenchs)] = segment_wrenchs + current_idx += len(segment_wrenchs) + + # 确保最后一个位置被处理 + if current_idx < len(times): + all_wrenchs[current_idx:] = wrench + + all_wrenchs = np.clip(all_wrenchs, np.array([0, 0, -50, 0, 0, 0]), np.array([0, 0, 0, 0, 0, 0])) + + if wrench is not None: + return np.array(all_wrenchs) + + def oscillation_wrench_interpolate(self,wrench=None, time_points=None, time_step=0.1): + if wrench is None: + raise ValueError("至少需要输入力矩的序列") + if time_points is None or len(time_points) < 2: + raise ValueError("需要提供至少两个时间点") + + # 由于浮点数精度问题，time_step要乘0.9 + times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step) + + if wrench is not None: + all_wrenchs = np.zeros((len(times), wrench.shape[1])) + current_idx = 0 + + + up_wrench = np.array([0, 0, 0, 0, 0, 0]) + down_wrench = np.array([0, 0, 0, 0, 0, 0]) + up_wrench[2] = wrench[0][2] + 15 if wrench[0][2] + 15 < 0 else 0 + down_wrench[2] = wrench[0][2] - 15 if wrench[0][2] - 15 > -50 else -50 + + + for i in range(len(times)): + # 计算 phase + phase = np.sin(20 * np.pi * i / len(times)) # 使用 i 计算 + + phase = 1 if phase >= 0 else -1 + + # 计算 amplitude + # amplitude = np.array([0, 0, -50, 0, 0, 0]) if phase < 0 else np.array([0, 0, 0, 0, 0, 0]) + amplitude = down_wrench if phase < 0 else up_wrench + # 计算 segment_wrenchs + segment_wrenchs = wrench + phase * (amplitude - wrench) + + # 将 segment_wrenchs 填充到 all_wrenchs + all_wrenchs[current_idx:current_idx + len(segment_wrenchs)] = segment_wrenchs + current_idx += len(segment_wrenchs) + + # 确保最后一个位置被处理 + if current_idx < len(times): + all_wrenchs[current_idx:] = wrench + + all_wrenchs = np.clip(all_wrenchs, np.array([0, 0, -70, 0, 0, 0]), np.array([0, 0, 0, 0, 0, 0])) + + if wrench is not None: + return np.array(all_wrenchs) + + + def S_shaped_wrench_interpolate(self,start=None, wrench=None, time_points=None, time_step=0.1): + if wrench is None: + raise ValueError("At least a wrench sequence must be provided.") + if time_points is None or len(time_points) < 2: + raise ValueError("At least two time points are required.") + if start is None: + raise ValueError("At least a start wrench must be provided.") + Tave = time_points[-1] / len(wrench) + print("Tave:",Tave) + + start_wrench = copy.deepcopy(np.array(start)) + print("start_wrench:",start_wrench) + + # Time points array creation + times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step) + # wrench = [wrench] + wrench = np.array(wrench) + + force_vel = 0 + num = 0 + + all_wrenchs = np.zeros((len(times), 6)) # Use wrench.shape[0] to get the number of components + # 时间点的插值计算 + for count in range(len(times)): + # 计算当前阶段的进度 + phase = count / len(times) + plant = phase * times[-1] + # print("plant:",plant) + + if plant >= num * Tave: + # print("plant:",plant,num,Tave) + if wrench[num][2] != 0: + # Convert wrench to numpy array if it's a list + coeff = 1 + end_wrench = copy.deepcopy(wrench[num]) # No need to convert again since wrench is now a numpy array + print("end_wrench:",end_wrench,num) + num += 1 + + if np.abs(end_wrench[2]-start_wrench[2]) > self.force_vel_max*self.Ta: + Ta = np.floor(self.Ta /time_step) * time_step + self.force_vel = Ta * self.force_acc_max + Tj = (np.abs(end_wrench[2]-start_wrench[2])-self.force_vel*Ta)/self.force_vel + coeff = np.abs(end_wrench[2] - start_wrench[2]) / (np.floor((Ta + Tj)/time_step)*time_step * self.force_vel) + else: + Ta = np.floor(np.sqrt(np.abs(end_wrench[2]-start_wrench[2])/self.force_acc_max)/time_step)*time_step + Tj = 0 + coeff = np.abs(end_wrench[2] - start_wrench[2]) / ((self.force_acc_max*time_step) * (np.floor(Ta/time_step)*np.floor(Ta/time_step))*time_step) + + if Tave < 2*Ta + Tj: + Ta = np.floor(Tave/time_step/2)*time_step + Tj = 0 + coeff = np.abs(end_wrench[2] - start_wrench[2]) / ((self.force_acc_max*time_step) * (np.floor(Ta/time_step)*np.floor(Ta/time_step))*time_step) + + if np.abs(end_wrench[2]-start_wrench[2]) < 0.1: + coeff = 1 + + start_wrench = np.array(start_wrench).astype(float) # Ensure it's an array + last_wrench = copy.deepcopy(start_wrench[2]) + force_vel = 0 + plant = 0 + segment_wrenchs = copy.deepcopy(start_wrench) + dir = (end_wrench[2]-start_wrench[2])/np.abs(end_wrench[2]-start_wrench[2]) + t_start = copy.deepcopy(start_wrench) + else: + + coeff = 1 + end_wrench = copy.deepcopy(wrench[num]) + num += 1 + segment_wrenchs = copy.deepcopy(start_wrench) + if Tave > 2: + Ta = 1 + else: + Ta = np.floor(Tave*10/2)/10 + Tj = 0 + force_vel = 0 + plant = 0 + coeff = np.abs(end_wrench[2] - start_wrench[2]) / ((self.force_acc_max*time_step) * (np.floor(Ta/time_step)*np.floor(Ta/time_step))*time_step) + dir = (end_wrench[2]-start_wrench[2])/np.abs(end_wrench[2]-start_wrench[2]) + last_wrench = copy.deepcopy(start_wrench[2]) + force_vel = 0 + t_start = copy.deepcopy(start_wrench) + + # print("all_wrenchs:", all_wrenchs.shape) + + plant = phase * times[-1] - (num-1) * Tave + + if end_wrench[2] != 0: + if plant == 0: + segment_wrenchs = copy.deepcopy(start_wrench) + elif 0 < plant <= Ta: + # 加速阶段 + force_vel += self.force_acc_max * time_step + if force_vel > self.force_vel_max: + force_vel = copy.deepcopy(self.force_vel_max) + last_wrench += dir * force_vel * time_step + segment_wrenchs[2] = last_wrench + elif Ta < plant <= Ta + Tj: + # 恒速阶段 + last_wrench += dir * force_vel * time_step + segment_wrenchs[2] = last_wrench + elif Ta + Tj < plant <= 2 * Ta + Tj: + # 减速阶段 + force_vel -= self.force_acc_max * time_step + if force_vel < 0: + force_vel = 0 + last_wrench += dir * force_vel * time_step + segment_wrenchs[2] = last_wrench + else: + # 超过所有阶段后，使用结束力矩 + segment_wrenchs = segment_wrenchs + + all_wrenchs[count] = copy.deepcopy(segment_wrenchs) + all_wrenchs[count][2] = (all_wrenchs[count][2] - t_start[2]) * coeff + t_start[2] + start_wrench = copy.deepcopy(all_wrenchs[count]) + else: + if plant-(Tave-Tj-2*Ta) == 0: + segment_wrenchs = copy.deepcopy(start_wrench) + elif 0 < plant-(Tave-Tj-2*Ta) <= Ta: + # 加速阶段 + force_vel += self.force_acc_max * time_step + if force_vel > self.force_vel_max: + force_vel = copy.deepcopy(self.force_vel_max) + last_wrench += dir * force_vel * time_step + segment_wrenchs[2] = last_wrench + elif Ta < plant-(Tave-Tj-2*Ta) <= Ta + Tj: + # 恒速阶段 + last_wrench += dir * force_vel * time_step + segment_wrenchs[2] = last_wrench + elif Ta + Tj < plant-(Tave-Tj-2*Ta) <= 2 * Ta + Tj: + # 减速阶段 + force_vel -= self.force_acc_max * time_step + if force_vel < 0: + force_vel = 0 + last_wrench += dir * force_vel * time_step + segment_wrenchs[2] = last_wrench + else: + # 超过所有阶段后，使用结束力矩 + segment_wrenchs = segment_wrenchs + + all_wrenchs[count] = copy.deepcopy(segment_wrenchs) + all_wrenchs[count][2] = (all_wrenchs[count][2] - t_start[2]) * coeff + t_start[2] + start_wrench = copy.deepcopy(all_wrenchs[count]) + + + all_wrenchs = np.clip(all_wrenchs, np.array([0, 0, -70, 0, 0, 0]), np.array([0, 0, 0, 0, 0, 0])) + + return all_wrenchs + + +FP = ForcePlanner() + +# Example input for testing +start_wrench = [0, 0, 0, 0, 0, 0] # Starting wrench +#end_wrench = [0, 0, 47, 0, 0, 0] +# end_wrench = [[0, 0, -29, 0, 0, 0]] # Ending wrench +end_wrench = [[0, 0, -20, 0, 0, 0], [0, 0, -5, 0, 0, 0], [0, 0, -35, 0, 0, 0],[0, 0, -60, 0, 0, 0], [0, 0, 0, 0, 0, 0]] # Ending wrench + +time_points = [0, 1] # From time 0 to 1 +#time_points = None +time_step = 0.1 # 0.1 second steps + +# Call the function with example values +result = FP.S_shaped_wrench_interpolate(start=start_wrench, wrench=end_wrench, time_points=time_points, time_step=0.0083333) + +# z_values = result[:, 2] +# # for t, z in zip(times, z_values): +# # print(f"time = {t:.4f} s,\tZ = {z:.4f}") + +# plt.figure(figsize=(6, 4)) +# plt.plot(times, z_values, label='Z (third component)') +# plt.xlabel('Time (s)') +# plt.ylabel('Force Z') +# plt.title('S-shaped 插值 —— 第三分量 (Z)') +# plt.grid(True) +# plt.legend() +# plt.tight_layout() +# plt.show() + + + + diff --git a/Massage/MassageControl/algorithms/hybridAdmit_controller.py b/Massage/MassageControl/algorithms/hybridAdmit_controller.py new file mode 100755 index 0000000..b9c0ad3 --- /dev/null +++ b/Massage/MassageControl/algorithms/hybridAdmit_controller.py @@ -0,0 +1,150 @@ +import sys +import numpy as np +from scipy.spatial.transform import Rotation as R +from .arm_state import ArmState +from .base_controller import BaseController +from pathlib import Path +sys.path.append(str(Path(__file__).resolve().parent.parent)) +from tools.yaml_operator import read_yaml +import time + +class HybridAdmitController(BaseController): + def __init__(self, name, state: ArmState,config_path) -> None: + super().__init__(name, state) + self.load_config(config_path) + self.laset_print_time = 0 + + + def load_config(self, config_path): + config_dict = read_yaml(config_path) + if self.name != config_dict['name']: + raise ValueError(f"Controller name {self.name} does not match config name {config_dict['name']}") + # 姿态调节器 + # 位控 2x2矩阵 + self.Kp_R = np.diag(np.array(config_dict['Kp_R'])) + self.Ki_R = np.diag(np.array(config_dict['Ki_R'])) + self.Kd_R = np.diag(np.array(config_dict['Kd_R'])) + + mass_rot = np.array(config_dict['mass_z']) + stiff_rot = np.array(config_dict['stiff_z']) + desired_xi = np.array(config_dict['desired_xi']) + damp_rot = np.array(config_dict['damp_z']) + self.D_z_min = 10.0 + self.D_z_max = 100.0 + self.D_z_lambda = 5.0 + self.M_z = np.diag(mass_rot) + self.K_z = np.diag(stiff_rot) + self.D_z = np.diag(damp_rot) + # 位控 2x2矩阵 + self.Kp = np.diag(np.array(config_dict['Kp'])) + self.Ki = np.diag(np.array(config_dict['Ki'])) + self.Kd = np.diag(np.array(config_dict['Kd'])) + + self.pose_integral_error = np.zeros(6) + + def step(self,dt): + # 方向统一 + if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + self.state.arm_orientation = -self.state.arm_orientation + # 缓存常用计算 + arm_ori_quat = R.from_quat(self.state.arm_orientation) + arm_ori_mat = arm_ori_quat.as_matrix() + # 位置误差 + temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error + # 姿态误差(四元数) + rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation) + self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) + # 期望加速度 + wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + # z方向力导纳控制 + self.state.arm_desired_acc[2] = 1/self.M_z * (wrench_err[2] - self.D_z * (self.state.arm_desired_twist[2] - self.state.desired_twist[2]) - self.K_z * self.state.pose_error[2]) + self.pose_integral_error += self.state.pose_error * dt + # 位控制 + self.state.arm_desired_acc[:2] = -self.Kd @ self.state.arm_desired_twist[:2] - self.Kp @ self.state.pose_error[:2] - self.Ki @ self.pose_integral_error[:2] + # 姿态pid + self.state.arm_desired_acc[3:] = -self.Kd_R @ self.state.arm_desired_twist[3:] - self.Kp_R @ self.state.pose_error[3:] - self.Ki_R @ self.pose_integral_error[3:] + self.clip_command(self.state.arm_desired_acc, "acc") + ## 更新速度和位姿 + self.state.arm_desired_twist += self.state.arm_desired_acc * dt + self.clip_command(self.state.arm_desired_twist, "vel") + # 计算位姿变化 + delta_pose = np.concatenate([ + arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt), + self.state.arm_desired_twist[3:] * dt + ]) + self.clip_command(delta_pose, "pose") + # 更新四元数 + delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat() + arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat) + self.state.arm_orientation_command = arm_ori_quat_new.as_quat() + # 归一化四元数 + self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) + # 更新位置 + self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + + def step_traj(self,dt): + # 方向统一 + if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + self.state.arm_orientation = -self.state.arm_orientation + # 缓存常用计算 + arm_ori_quat = R.from_quat(self.state.arm_orientation) + arm_ori_mat = arm_ori_quat.as_matrix() + # 位置误差 + + temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.desired_twist[:3] * dt + self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error + + # 姿态误差(四元数) + angular_velocity = np.array(self.state.desired_twist[3:]) # 形状 (3,) + + # 用旋转向量（小角度近似） + rotvec = angular_velocity * dt # 旋转向量 = 角速度 × 时间 + rot_quat = R.from_rotvec(rotvec).as_quat() # 转成四元数，形状 (4,) + rot_err_quat = R.from_quat(rot_quat).inv() * arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation) + self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) + + # 期望加速度 + wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + + # z方向力导纳控制 + self.state.arm_desired_acc[2] = 1/self.M_z * (wrench_err[2] - self.D_z * (self.state.arm_desired_twist[2] - self.state.desired_twist[2] + self.state.desired_acc[2]*dt) - self.K_z * self.state.pose_error[2]) + self.state.desired_acc[2] + self.pose_integral_error += self.state.pose_error * dt + # 位控制 + self.state.arm_desired_acc[:2] = -self.Kd @ self.state.arm_desired_twist[:2] - self.Kp @ self.state.pose_error[:2] - self.Ki @ self.pose_integral_error[:2] + # 姿态pid + self.state.arm_desired_acc[3:] = -self.Kd_R @ self.state.arm_desired_twist[3:] - self.Kp_R @ self.state.pose_error[3:] - self.Ki_R @ self.pose_integral_error[3:] + + self.clip_command(self.state.arm_desired_acc, "acc") + ## 更新速度和位姿 + self.state.arm_desired_twist += self.state.arm_desired_acc * dt + self.clip_command(self.state.arm_desired_twist, "vel") + # 计算位姿变化 + delta_pose = np.concatenate([ + arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt), + self.state.arm_desired_twist[3:] * dt + ]) + self.clip_command(delta_pose, "pose") + # 更新四元数 + delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat() + arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat) + self.state.arm_orientation_command = arm_ori_quat_new.as_quat() + # 归一化四元数 + self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) + # 更新位置 + self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Massage/MassageControl/algorithms/hybridPid_controller.py b/Massage/MassageControl/algorithms/hybridPid_controller.py new file mode 100755 index 0000000..46d2c60 --- /dev/null +++ b/Massage/MassageControl/algorithms/hybridPid_controller.py @@ -0,0 +1,161 @@ +import sys +import numpy as np +from scipy.spatial.transform import Rotation as R +from .arm_state import ArmState +from .base_controller import BaseController +from pathlib import Path +sys.path.append(str(Path(__file__).resolve().parent.parent)) +from tools.yaml_operator import read_yaml +import time + +class HybridPidController(BaseController): + def __init__(self, name, state: ArmState,config_path) -> None: + super().__init__(name, state) + self.load_config(config_path) + self.laset_print_time = 0 + + + def load_config(self, config_path): + config_dict = read_yaml(config_path) + if self.name != config_dict['name']: + raise ValueError(f"Controller name {self.name} does not match config name {config_dict['name']}") + # 姿态调节器 + # 位控 2x2矩阵 + self.Kp_R = np.diag(np.array(config_dict['Kp_R'])) + self.Ki_R = np.diag(np.array(config_dict['Ki_R'])) + self.Kd_R = np.diag(np.array(config_dict['Kd_R'])) + + # 力控 + self.force_mass = np.array(config_dict['force_mass']) + self.force_damp = np.array(config_dict['force_damp']) + self.Kp_force = np.array(config_dict['Kp_force']) + self.Kd_force = np.array(config_dict['Kd_force']) + self.Ki_force = np.array(config_dict['Ki_force']) + # TODO 修改控制率 + self.e_t = 0 + self.e_t1 = 0 + self.e_t2 = 0 + self.force_control_value = 0 + + # 位控 2x2矩阵 + self.Kp = np.diag(np.array(config_dict['Kp'])) + self.Ki = np.diag(np.array(config_dict['Ki'])) + self.Kd = np.diag(np.array(config_dict['Kd'])) + self.pose_integral_error = np.zeros(6) + + def step(self,dt): + # 方向统一 + if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + self.state.arm_orientation = -self.state.arm_orientation + # 缓存常用计算 + arm_ori_quat = R.from_quat(self.state.arm_orientation) + arm_ori_mat = arm_ori_quat.as_matrix() + # 位置误差 + temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error + # 姿态误差(四元数) + rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation) + self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) + # 期望加速度 + wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + + # z方向力控制 + self.e_t2 = self.e_t1 + self.e_t1 = self.e_t + self.e_t = wrench_err[2] + self.force_control_value += self.Kp_force * (self.e_t - self.e_t1) + self.Ki_force * self.e_t * dt + self.Kd_force * (self.e_t - 2 * self.e_t1 + self.e_t2) /dt + self.state.arm_desired_acc[2] = (1.0 / self.force_mass) * (self.force_control_value - self.force_damp * self.state.arm_desired_twist[2]) + + self.pose_integral_error += self.state.pose_error * dt + # 位控制 + self.state.arm_desired_acc[:2] = -self.Kd @ self.state.arm_desired_twist[:2] - self.Kp @ self.state.pose_error[:2] - self.Ki @ self.pose_integral_error[:2] + # 姿态pid + self.state.arm_desired_acc[3:] = -self.Kd_R @ self.state.arm_desired_twist[3:] - self.Kp_R @ self.state.pose_error[3:] - self.Ki_R @ self.pose_integral_error[3:] + self.clip_command(self.state.arm_desired_acc, "acc") + ## 更新速度和位姿 + self.state.arm_desired_twist += self.state.arm_desired_acc * dt + self.clip_command(self.state.arm_desired_twist, "vel") + # 计算位姿变化 + delta_pose = np.concatenate([ + arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt), + self.state.arm_desired_twist[3:] * dt + ]) + self.clip_command(delta_pose, "pose") + # 更新四元数 + delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat() + arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat) + self.state.arm_orientation_command = arm_ori_quat_new.as_quat() + # 归一化四元数 + self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) + # 更新位置 + self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + + def step_traj(self,dt): + # 方向统一 + if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + self.state.arm_orientation = -self.state.arm_orientation + # 缓存常用计算 + arm_ori_quat = R.from_quat(self.state.arm_orientation) + arm_ori_mat = arm_ori_quat.as_matrix() + # 位置误差 + + temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.desired_twist[:3] * dt + self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error + + # 姿态误差(四元数) + angular_velocity = np.array(self.state.desired_twist[3:]) # 形状 (3,) + + # 用旋转向量（小角度近似） + rotvec = angular_velocity * dt # 旋转向量 = 角速度 × 时间 + rot_quat = R.from_rotvec(rotvec).as_quat() # 转成四元数，形状 (4,) + rot_err_quat = R.from_quat(rot_quat).inv() * arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation) + self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) + + # 期望加速度 + wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + + # z方向力控制 + self.e_t2 = self.e_t1 + self.e_t1 = self.e_t + self.e_t = wrench_err[2] + self.force_control_value += self.Kp_force * (self.e_t - self.e_t1) + self.Ki_force * self.e_t * dt + self.Kd_force * (self.e_t - 2 * self.e_t1 + self.e_t2) /dt + self.state.arm_desired_acc[2] = (1.0 / self.force_mass) * (self.force_control_value - self.force_damp * self.state.arm_desired_twist[2]) + self.pose_integral_error += self.state.pose_error * dt + # 位控制 + self.state.arm_desired_acc[:2] = -self.Kd @ self.state.arm_desired_twist[:2] - self.Kp @ self.state.pose_error[:2] - self.Ki @ self.pose_integral_error[:2] + # 姿态pid + self.state.arm_desired_acc[3:] = -self.Kd_R @ self.state.arm_desired_twist[3:] - self.Kp_R @ self.state.pose_error[3:] - self.Ki_R @ self.pose_integral_error[3:] + + self.clip_command(self.state.arm_desired_acc, "acc") + ## 更新速度和位姿 + self.state.arm_desired_twist += self.state.arm_desired_acc * dt + self.clip_command(self.state.arm_desired_twist, "vel") + # 计算位姿变化 + delta_pose = np.concatenate([ + arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt), + self.state.arm_desired_twist[3:] * dt + ]) + self.clip_command(delta_pose, "pose") + # 更新四元数 + delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat() + arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat) + self.state.arm_orientation_command = arm_ori_quat_new.as_quat() + # 归一化四元数 + self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) + # 更新位置 + self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Massage/MassageControl/algorithms/hybrid_controller.py b/Massage/MassageControl/algorithms/hybrid_controller.py new file mode 100755 index 0000000..07b6782 --- /dev/null +++ b/Massage/MassageControl/algorithms/hybrid_controller.py @@ -0,0 +1,150 @@ +import sys +import numpy as np +from scipy.spatial.transform import Rotation as R +from .arm_state import ArmState +from .base_controller import BaseController +from pathlib import Path +sys.path.append(str(Path(__file__).resolve().parent.parent)) +from tools.yaml_operator import read_yaml +import time + +class HybridController(BaseController): + def __init__(self, name, state: ArmState,config_path) -> None: + super().__init__(name, state) + self.load_config(config_path) + self.laset_print_time = 0 + + + def load_config(self, config_path): + config_dict = read_yaml(config_path) + if self.name != config_dict['name']: + raise ValueError(f"Controller name {self.name} does not match config name {config_dict['name']}") + # 姿态调节器 + mass_rot = np.array(config_dict['mass_rot']) + stiff_rot = np.array(config_dict['stiff_rot']) + desired_xi = np.array(config_dict['desired_xi']) + damp_rot = np.array(config_dict['damp_rot']) + + mass_z = np.array(config_dict['mass_z']) + stiff_z = np.array(config_dict['stiff_z']) + damp_z = np.array(config_dict['damp_z']) + for i in range(3): + if damp_rot[i] < 0: + damp_rot[i] = 2 * desired_xi * np.sqrt(stiff_rot[i] * mass_rot[i]) + self.M_rot = np.diag(mass_rot) + self.M_rot_inv = np.linalg.inv(self.M_rot) + self.K_rot = np.diag(stiff_rot) + self.D_rot = np.diag(damp_rot) + # 位控 2x2矩阵 + self.Kp = np.diag(np.array(config_dict['Kp'])) + self.Ki = np.diag(np.array(config_dict['Ki'])) + self.Kd = np.diag(np.array(config_dict['Kd'])) + + self.pose_integral_error = np.zeros(6) + + self.M_z = np.diag(mass_z) + self.K_z = np.diag(stiff_z) + self.D_z = np.diag(damp_z) + + + def step(self,dt): + # 方向统一 + if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + self.state.arm_orientation = -self.state.arm_orientation + # 缓存常用计算 + arm_ori_quat = R.from_quat(self.state.arm_orientation) + arm_ori_mat = arm_ori_quat.as_matrix() + # 位置误差 + temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error + # 姿态误差(四元数) + rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation) + self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) + # 期望加速度 + wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + # 姿态控制 期望目标点坐标系的姿态下的力矩为0 + self.state.arm_desired_acc[3:] = self.M_rot_inv @ (wrench_err[3:] - self.D_rot @ (self.state.arm_desired_twist[3:] - self.state.desired_twist[3:]) - self.K_rot @ self.state.pose_error[3:]) + # z方向力导纳控制 + self.state.arm_desired_acc[2] = 1/self.M_z * (wrench_err[2] - self.D_z * (self.state.arm_desired_twist[2] - self.state.desired_twist[2]) - self.K_z * self.state.pose_error[2]) + self.pose_integral_error += self.state.pose_error * dt + # 位控制 + self.state.arm_desired_acc[:2] = -self.Kd @ self.state.arm_desired_twist[:2] - self.Kp @ self.state.pose_error[:2] - self.Ki @ self.pose_integral_error[:2] + self.clip_command(self.state.arm_desired_acc, "acc") + ## 更新速度和位姿 + self.state.arm_desired_twist += self.state.arm_desired_acc * dt + self.clip_command(self.state.arm_desired_twist, "vel") + # 计算位姿变化 + delta_pose = np.concatenate([ + arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt), + self.state.arm_desired_twist[3:] * dt + ]) + self.clip_command(delta_pose, "pose") + # 更新四元数 + delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat() + arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat) + self.state.arm_orientation_command = arm_ori_quat_new.as_quat() + # 归一化四元数 + self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) + # 更新位置 + self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + + def step_traj(self,dt): + # 方向统一 + if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + self.state.arm_orientation = -self.state.arm_orientation + # 缓存常用计算 + arm_ori_quat = R.from_quat(self.state.arm_orientation) + arm_ori_mat = arm_ori_quat.as_matrix() + # 位置误差 + + temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.desired_twist[:3] * dt + self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error + + # 姿态误差(四元数) + angular_velocity = np.array(self.state.desired_twist[3:]) # 形状 (3,) + + # 用旋转向量（小角度近似） + rotvec = angular_velocity * dt # 旋转向量 = 角速度 × 时间 + rot_quat = R.from_rotvec(rotvec).as_quat() # 转成四元数，形状 (4,) + rot_err_quat = R.from_quat(rot_quat).inv() * arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation) + self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) + + # 期望加速度 + wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + # 姿态控制 期望目标点坐标系的姿态下的力矩为0 + self.state.arm_desired_acc[3:] = self.M_rot_inv @ (wrench_err[3:] - self.D_rot @ (self.state.arm_desired_twist[3:] - self.state.desired_twist[3:] + self.state.desired_acc[3:]*dt) - self.K_rot @ self.state.pose_error[3:]) + + self.state.desired_acc[3:] + # z方向力导纳控制 + self.state.arm_desired_acc[2] = 1/self.M_z * (wrench_err[2] - self.D_z * (self.state.arm_desired_twist[2] - self.state.desired_twist[2] + self.state.desired_acc[2]*dt) - self.K_z * self.state.pose_error[2]) + self.state.desired_acc[2] + self.pose_integral_error += self.state.pose_error * dt + # 位控制 + self.state.arm_desired_acc[:2] = -self.Kd @ self.state.arm_desired_twist[:2] - self.Kp @ self.state.pose_error[:2] - self.Ki @ self.pose_integral_error[:2] + + self.clip_command(self.state.arm_desired_acc, "acc") + ## 更新速度和位姿 + self.state.arm_desired_twist += self.state.arm_desired_acc * dt + self.clip_command(self.state.arm_desired_twist, "vel") + # 计算位姿变化 + delta_pose = np.concatenate([ + arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt), + self.state.arm_desired_twist[3:] * dt + ]) + self.clip_command(delta_pose, "pose") + # 更新四元数 + delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat() + arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat) + self.state.arm_orientation_command = arm_ori_quat_new.as_quat() + # 归一化四元数 + self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) + # 更新位置 + self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + + + + + + + + + + + \ No newline at end of file diff --git a/Massage/MassageControl/algorithms/interpolation.py b/Massage/MassageControl/algorithms/interpolation.py new file mode 100755 index 0000000..f1ba2fc --- /dev/null +++ b/Massage/MassageControl/algorithms/interpolation.py @@ -0,0 +1,817 @@ +import numpy as np +from scipy.interpolate import CubicSpline +from scipy.spatial.transform import Rotation as R, Slerp +from scipy.interpolate import BSpline +from scipy.interpolate import make_interp_spline +from scipy.signal import savgol_filter +from scipy.interpolate import interp1d + +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import math +import copy + +def linear_interpolate(positions=None, quaternions=None, time_points=None, time_step=0.1): + """ + 进行位置和/或姿态的线性插值，支持多个点 + :param positions: 位置序列，单位为米 (Nx2 或 Nx3 numpy array, 可选) + :param quaternions: 姿态序列，四元数 (Nx4 numpy array, 可选) + :param time_points: 时间点序列 (N个浮点数) + :param time_step: 离散时间步长，单位为秒 (float) + :return: 插值序列，包含位置序列、姿态序列或两者兼有 + """ + if positions is None and quaternions is None: + raise ValueError("至少需要输入位置或姿态的序列") + if time_points is None or len(time_points) < 2: + raise ValueError("需要提供至少两个时间点") + + # 由于浮点数精度问题，time_step要乘0.9 + times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step) + + if positions is not None: + all_positions = np.zeros((len(times), positions.shape[1])) + segment_durations = np.diff(time_points) + segment_counts = np.floor(segment_durations / time_step).astype(int) + current_idx = 0 + for i in range(len(segment_counts)): + segment_times = np.linspace(0, segment_durations[i], segment_counts[i] + 1) + if i > 0: + segment_times = segment_times[1:] + start_pos = positions[i] + end_pos = positions[i + 1] + segment_positions = start_pos + np.outer(segment_times, (end_pos - start_pos) / segment_durations[i]) + all_positions[current_idx:current_idx + len(segment_positions)] = segment_positions + current_idx += len(segment_positions) + + # 确保最后一个位置被处理 + if current_idx < len(times): + all_positions[current_idx:] = positions[-1] + + if quaternions is not None: + slerp = Slerp(time_points, R.from_quat(quaternions)) + all_quaternions = slerp(times).as_quat() + + all_quaternions = np.array([quat / np.linalg.norm(quat) for quat in all_quaternions]) + + if positions is not None and quaternions is not None: + return all_positions, all_quaternions + elif positions is not None: + return all_positions + elif quaternions is not None: + return all_quaternions + +def spline_interpolate(positions=None, quaternions=None, time_points=None, time_step=0.1): + """ + 进行位置和/或姿态的样条插值，支持多个点 + :param positions: 位置序列，单位为米 (Nx2 或 Nx3 numpy array, 可选) + :param quaternions: 姿态序列，四元数 (Nx4 numpy array, 可选) + :param time_points: 时间点序列 (N个浮点数) + :param time_step: 离散时间步长，单位为秒 (float) + :return: 插值序列，包含位置序列、姿态序列或两者兼有 + """ + if positions is None and quaternions is None: + raise ValueError("至少需要输入位置或姿态的序列") + if time_points is None or len(time_points) < 2: + raise ValueError("需要提供至少两个时间点") + + all_positions = [] + all_quaternions = [] + times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step) + + if positions is not None: + cs = CubicSpline(time_points, positions, axis=0) + all_positions = cs(times) + + if quaternions is not None: + slerp = Slerp(time_points, R.from_quat(quaternions)) + all_quaternions = slerp(times).as_quat() + + if positions is not None and quaternions is not None: + return np.array(all_positions), np.array(all_quaternions) + elif positions is not None: + return np.array(all_positions) + elif quaternions is not None: + return np.array(all_quaternions) + + + +def generate_circle_trajectory(center, omega=0.4, radius=0.02, reverse=False, time_points=None, time_step=0.01, start_transition_duration=None, end_transition_duration=None): + """ + Generate a 3D trajectory of a circular motion from the center to the specified radius. + + Parameters: + center (list): The center of the circle [x, y, z , r , p , y]. + omega (float): The angular velocity. + radius (float): The radius of the circle. + reverse (bool): If True, rotates counterclockwise. If False, rotates clockwise. + time_points (list or np.ndarray): List or array of time points. + time_step (float): Time step for generating the trajectory. + start_transition_duration (float): Duration for the transition at the start. + end_transition_duration (float): Duration for the transition at the end. + + Returns: + np.ndarray: Array of positions over time. + """ + + # print(time_points) + if time_points is None or len(time_points) < 2: + raise ValueError("需要提供至少两个时间点") + + + if start_transition_duration is None: + start_transition_duration = 2 + + if end_transition_duration is None: + end_transition_duration = 2 + t_points = time_points.copy() + t_points[-1] = time_points[-1] + end_transition_duration + start_transition_duration + t_points[-1] = round(t_points[-1] / time_step) * time_step + times = np.arange(t_points[0], t_points[-1] + time_step * 0.9, time_step) + + if reverse: + angles = -omega * times + else: + angles = omega * times + + radii = np.ones_like(times) * radius + + start_transition = times < start_transition_duration + end_transition = times > (times[-1] - end_transition_duration) + radii[start_transition] = radius * (1 - np.cos(np.pi * times[start_transition] / start_transition_duration)) / 2 + radii[end_transition] = radius * (1 + np.cos(np.pi * (times[end_transition] - (times[-1] - end_transition_duration)) / end_transition_duration)) / 2 + + x_positions = center[0][0] + radii * np.cos(angles) + y_positions = center[0][1] + radii * np.sin(angles) + z_positions = np.full_like(x_positions, center[0][2]) # Z position remains constant + + positions = np.column_stack((x_positions, y_positions, z_positions)) + return positions + +def bezier_curve(control_points, n_points=100): + # 确保输入为二维 float64 数组 + control_points = np.array(control_points, dtype=np.float64) + if control_points.ndim != 2: + raise ValueError("control_points must be a 2D array, got shape {}".format(control_points.shape)) + + n = len(control_points) - 1 + t = np.linspace(0.0, 1.0, n_points) + curve = np.zeros((n_points, control_points.shape[1]), dtype=np.float64) + + def bernstein(i, n, t): + from scipy.special import comb + return comb(n, i) * ((1 - t) ** (n - i)) * (t ** i) + + for i in range(n + 1): + # 明确每个控制点是 float64 向量 + point = np.asarray(control_points[i], dtype=np.float64) + if point.shape != (control_points.shape[1],): + raise ValueError(f"Invalid point shape: {point.shape}") + b = bernstein(i, n, t) + curve += np.outer(b, point) + + return curve + + + +def cloud_point_interpolate(positions=None, quaternions=None, time_points=None, time_step=0.1): + """ + 进行输入的点云位置和/或姿态的曲线插值，支持多个点，采用B样条插值法 + :param positions: 位置序列，单位为米 (Nx2 或 Nx3 numpy array, 可选) + :param quaternions: 姿态序列，四元数 (Nx4 numpy array, 可选) + :param time_points: 时间点序列 (N个浮点数) + :param time_step: 离散时间步长，单位为秒 (float) + :return: 插值序列，包含位置序列、姿态序列或两者兼有 + """ + if positions is None and quaternions is None: + raise ValueError("至少需要输入位置或姿态的序列") + if time_points is None or len(time_points) < 2: + raise ValueError("需要提供至少两个时间点") + + temp_positions = np.array(positions) + temp_quaternions = np.zeros((len(quaternions), 4)) + + # 将RPY角度转换为四元数 + for i in range(len(quaternions)): + temp_quaternions[i] = R.from_euler('xyz', quaternions[i]).as_quat() + + time_points = np.linspace(time_points[0], time_points[-1], len(temp_positions)) + times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step) + + temp_positions_smoothed = copy.deepcopy(temp_positions) + # temp_positions = np.array(positions, dtype=np.float64) + # temp_positions_smoothed = bezier_curve(temp_positions, n_points=len(temp_positions)) + temp_quaternions_smoothed = copy.deepcopy(temp_quaternions) + + all_positions, all_quaternions = [], [] + + # 进行B样条插值 + if temp_positions_smoothed is not None: + BS = make_interp_spline(time_points, temp_positions_smoothed, k=3, bc_type='natural') + all_positions = BS(times) + + # 进行四元数LERP插值 + if temp_quaternions_smoothed is not None: + temp_quaternions_smoothed = np.array(temp_quaternions_smoothed) + all_quaternions = [] + for t in times: + # 在时间点序列中找到最近的两个时间点，用于线性插值 + idx = np.searchsorted(time_points, t) - 1 + idx = max(0, min(idx, len(time_points) - 2)) + + t1, t2 = time_points[idx], time_points[idx + 1] + q1, q2 = temp_quaternions_smoothed[idx], temp_quaternions_smoothed[idx + 1] + + # 计算插值因子 alpha + alpha = (t - t1) / (t2 - t1) + + # 进行四元数LERP插值 + q_interp = (1 - alpha) * q1 + alpha * q2 + q_interp /= np.linalg.norm(q_interp) # 归一化四元数 + + all_quaternions.append(q_interp) + + euler_angles = R.from_quat(all_quaternions).as_euler('xyz') + + # 检查数据点数量 + if len(euler_angles) >= 5: + # 数据点数量大于等于3时使用 Savitzky-Golay 滤波器 + euler_angles_smoothed = savgol_filter(euler_angles, window_length=5, polyorder=3, axis=0) + else: + # 数据点小于5时直接使用原始数据 + euler_angles_smoothed = euler_angles + + # 将平滑后的欧拉角转换回四元数 + all_quaternions = R.from_euler('xyz', euler_angles_smoothed).as_quat() + + if temp_positions is not None and temp_quaternions is not None: + return np.array(all_positions), np.array(all_quaternions) + elif temp_positions is not None: + return np.array(all_positions) + elif temp_quaternions is not None: + return np.array(all_quaternions) + +def oscillation_wrench_interpolate(wrench=None, time_points=None, time_step=0.1): + if wrench is None: + raise ValueError("至少需要输入力矩的序列") + if time_points is None or len(time_points) < 2: + raise ValueError("需要提供至少两个时间点") + + # 由于浮点数精度问题，time_step要乘0.9 + times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step) + + if wrench is not None: + all_wrenchs = np.zeros((len(times), wrench.shape[1])) + current_idx = 0 + + + up_wrench = np.array([0, 0, 0, 0, 0, 0]) + down_wrench = np.array([0, 0, 0, 0, 0, 0]) + up_wrench[2] = wrench[0][2] + 15 if wrench[0][2] + 15 < 0 else 0 + down_wrench[2] = wrench[0][2] - 15 if wrench[0][2] - 15 > -50 else -50 + + + for i in range(len(times)): + # 计算 phase + phase = np.sin(20 * np.pi * i / len(times)) # 使用 i 计算 + + phase = 1 if phase >= 0 else -1 + + # 计算 amplitude + # amplitude = np.array([0, 0, -50, 0, 0, 0]) if phase < 0 else np.array([0, 0, 0, 0, 0, 0]) + amplitude = down_wrench if phase < 0 else up_wrench + # 计算 segment_wrenchs + segment_wrenchs = wrench + phase * (amplitude - wrench) + + # 将 segment_wrenchs 填充到 all_wrenchs + all_wrenchs[current_idx:current_idx + len(segment_wrenchs)] = segment_wrenchs + current_idx += len(segment_wrenchs) + + # 确保最后一个位置被处理 + if current_idx < len(times): + all_wrenchs[current_idx:] = wrench + + all_wrenchs = np.clip(all_wrenchs, np.array([0, 0, -70, 0, 0, 0]), np.array([0, 0, 0, 0, 0, 0])) + + if wrench is not None: + return np.array(all_wrenchs) + +def linear_wrench_interpolate(wrench=None, time_points=None, time_step=0.1): + if wrench is None: + raise ValueError("至少需要输入力矩的序列") + if time_points is None or len(time_points) < 2: + raise ValueError("需要提供至少两个时间点") + + # 由于浮点数精度问题，time_step要乘0.9 + times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step) + + if wrench is not None: + all_wrenchs = np.zeros((len(times), wrench.shape[1])) + current_idx = 0 + start_wrench = np.array([0, 0, 0, 0, 0, 0]) + end_wrench = wrench + + for i in range(len(times)): + # 计算 phase + phase = i / len(times) # 使用 i 计算 + + # 计算 segment_wrenchs + segment_wrenchs = start_wrench + phase * (end_wrench - start_wrench) + + # 将 segment_wrenchs 填充到 all_wrenchs + all_wrenchs[current_idx:current_idx + len(segment_wrenchs)] = segment_wrenchs + current_idx += len(segment_wrenchs) + + # 确保最后一个位置被处理 + if current_idx < len(times): + all_wrenchs[current_idx:] = wrench + + all_wrenchs = np.clip(all_wrenchs, np.array([0, 0, -70, 0, 0, 0]), np.array([0, 0, 0, 0, 0, 0])) + + if wrench is not None: + return np.array(all_wrenchs) + +def resample_curve_strict(points, num_resampled_points): + """ + 修正的曲线重新采样函数，移除重复点并确保累积弧长严格递增。支持三维曲线。 + :param points: Nx3 numpy array，三维轨迹点 + :param num_resampled_points: 重新采样的点数 + :return: Nx3 numpy array，重采样后的三维轨迹 + """ + points = np.array(points, dtype=np.float64) + + # 计算累积弧长 + distances = np.linalg.norm(np.diff(points, axis=0), axis=1) + cumulative_length = np.insert(np.cumsum(distances), 0, 0) + + # 移除重复点（累积弧长未变化的） + unique_indices = np.where(np.diff(cumulative_length, prepend=-np.inf) > 0)[0] + cumulative_length = cumulative_length[unique_indices] + points = points[unique_indices] + + # 生成等间隔弧长采样点 + target_lengths = np.linspace(0, cumulative_length[-1], num_resampled_points) + + # 对每个维度做线性插值 + interp_funcs = [interp1d(cumulative_length, points[:, i], kind='linear', fill_value="extrapolate") for i in range(3)] + new_coords = [interp(target_lengths) for interp in interp_funcs] + + return np.column_stack(new_coords) + + + +def circle_trajectory(center, omega=8.0, radius=0.04, reverse=False, time_points=None, time_step=0.01, start_transition_duration=None, end_transition_duration=None): + + + if time_points is None or len(time_points) < 2: + raise ValueError("需要提供至少两个时间点") + + + if start_transition_duration is None: + start_transition_duration = 2 + + if end_transition_duration is None: + end_transition_duration = 2 + t_points = time_points.copy() + # t_points[-1] = time_points[-1] + end_transition_duration + start_transition_duration + t_points[-1] = time_points[-1] + t_points[-1] = round(t_points[-1] / time_step) * time_step + times = np.arange(t_points[0], t_points[-1] + time_step * 0.9, time_step) + + if reverse: + angles = -omega * times + else: + angles = omega * times + + radii = np.ones_like(times) * radius + + start_transition = times < start_transition_duration + end_transition = times > (times[-1] - end_transition_duration) + radii[start_transition] = radius * (1 - np.cos(np.pi * times[start_transition] / start_transition_duration)) / 2 + radii[end_transition] = radius * (1 + np.cos(np.pi * (times[end_transition] - (times[-1] - end_transition_duration)) / end_transition_duration)) / 2 + + x_positions = radii * np.cos(angles) + y_positions = radii * np.sin(angles) + z_positions = np.full_like(x_positions, 0) # Z position remains constant + + + + positions = np.column_stack((x_positions, y_positions, z_positions)) + positions = resample_curve_strict(positions, len(positions)) + # print("circle_positions:") + tempToolRPY = R.from_euler('xyz', center[0][3:], degrees=False).as_matrix() + for i in range(len(positions)): + positions[i] = tempToolRPY @ positions[i] + center[0][:3] # 将RPY角度转换为四元数 + # print(positions[i]) + + return positions + +# def generate_circle_cloud_points(center, start_point, radius, delta_theta = 10*np.pi/180, num_turns = 3): +# """ +# center: 圆心坐标，形如 (x_c, y_c) +# start_point: 起始点坐标，形如 (x_0, y_0) +# radius: 圆的半径 +# delta_theta: 每次插补的角度增量 +# num_turns: 绕圈的次数 +# """ +# # 确定总共需要生成的插补点数 +# num_points = int((2 * np.pi * num_turns) / delta_theta) + +# # 圆心 +# x_c, y_c = center + +# # 计算起始点的初始角度 +# x_0, y_0 = start_point +# theta_0 = np.arctan2(y_0 - y_c, x_0 - x_c) + +# # 初始化存储插补点的列表 +# circle_points = [] + +# # 生成插补点 +# for i in range(num_points): +# # 当前角度 +# theta_i = theta_0 + i * delta_theta + +# # 计算插补点的坐标 +# x_i = x_c + radius * np.cos(theta_i) +# y_i = y_c + radius * np.sin(theta_i) + +# # 将点添加到列表中 + +# circle_points.append((np.round(x_i).astype(int), np.round(y_i).astype(int))) + +# circle_points.append((np.round(x_0).astype(int), np.round(y_0).astype(int))) + +# return circle_points + + + +def calculate_target_Euler(point): + + temp_euler = np.zeros((len(point), 3), dtype=np.float64) + + for i in range(len(point)): + if(point[i][5]<0): + temp_euler[i][0] = -math.asin(-point[i][4]) + temp_euler[i][1] = math.atan2(-point[i][3],-point[i][5]) + else: + temp_euler[i][0] = -math.asin(point[i][4]) + temp_euler[i][1] = math.atan2(point[i][3],point[i][5]) + + temp_euler[i][2] = 0.0 + + return temp_euler + + +if __name__ == "__main__": + # import pathlib + # import sys + # sys.path.append(str(pathlib.Path.cwd())) + # from MassageControl.tools.draw_tools import plot_trajectory + + import numpy as np + import pandas as pd + import matplotlib.pyplot as plt + + # 示例使用 + center = (80, 90) # 圆心 + start_point = (70, 0) # 起点 + radius = np.linalg.norm(np.array(start_point) - np.array(center)) # 半径 + delta_theta = np.pi / 9 # 每次插补的角度增量 + num_turns = 2 # 绕2圈 + + # 生成圆的插补点 + circle_points_with_start = generate_circle_cloud_points(center, start_point, radius, delta_theta, num_turns) + + # 将生成的插补点转换为可视化的DataFrame + circle_points_with_start_df = pd.DataFrame({ + "x": [point[0] for point in circle_points_with_start], + "y": [point[1] for point in circle_points_with_start] + }) + + # 打印生成的插补点 + print(circle_points_with_start_df) + + # 绘制插补点的图像 + x_vals = [point[0] for point in circle_points_with_start] + y_vals = [point[1] for point in circle_points_with_start] + + plt.figure(figsize=(6, 6)) + plt.scatter(x_vals, y_vals, marker='o', linestyle='-', color='b') + plt.scatter([x_vals[0]], [y_vals[0]], color='r', label='Start Point') # 标记起点 + plt.gca().set_aspect('equal', adjustable='box') + plt.title('Circle Interpolation Points with Start Point') + plt.xlabel('X') + plt.ylabel('Y') + plt.legend() + plt.grid(True) + + # 保存并显示图片 + plt.show() + + + +# start1pos = np.array([0.0392,-0.408,0.752]) +# end1pos = np.array([0.0392,0.2,0.752]) + +# start2pos = np.array([0.2281194148213992,-0.1320499159555817,0.7499999952316284]) +# end2pos = np.array([0.14268880718774088,-0.13746791895961052,0.7350000095367432]) + +# start3pos = np.array([0.23648124242722512,-0.2097320409627573,0.7430000257492065]) +# end3pos = np.array([0.1493414817211018,-0.21703966731273366,0.7340000224113464]) + +# start4pos = np.array([0.24389595888042098,-0.30559190060482105,0.7499999952316284]) +# end4pos = np.array([0.15822969083491112,-0.3106326911577041,0.7440000128746033]) + +# start5pos = np.array([0.2535787008200847,-0.402571052456421,0.7559999775886536]) +# end5pos = np.array([0.16737854928028986,-0.41016720685793384,0.7580000114440918]) + + + + + + + + +# # #-------------------------- +# # # 计算起始点和结束点之间的总距离 +# # total_distance = np.linalg.norm(start2pos - end1pos) + +# # # 根据欧式距离步长为0.005估算所需的点数 +# # num_points = int(total_distance / dis) + +# # # 初始化用于存储点的数组 +# # temppoints = np.zeros((num_points + 1, 3)) +# # temppoints[0] = end1pos + +# # # 生成欧式距离差约为0.01的点 +# # for i in range(1, num_points + 1): +# # direction = (start2pos - end1pos) / total_distance # 单位方向向量 +# # temppoints[i] = temppoints[i-1] + direction * dis + +# # # 确保最后一个点与endpos完全重合 +# # temppoints[-1] = start2pos + +# # points = np.vstack((points, temppoints)) + +# # #-------------------------- +# # # 计算起始点和结束点之间的总距离 +# # total_distance = np.linalg.norm(end2pos - start2pos) + +# # # 根据欧式距离步长为0.005估算所需的点数 +# # num_points = int(total_distance / dis) + +# # # 初始化用于存储点的数组 +# # temppoints = np.zeros((num_points + 1, 3)) +# # temppoints[0] = start2pos + +# # # 生成欧式距离差约为0.01的点 +# # for i in range(1, num_points + 1): +# # direction = (end2pos - start2pos) / total_distance # 单位方向向量 +# # temppoints[i] = temppoints[i-1] + direction * dis + +# # # 确保最后一个点与endpos完全重合 +# # temppoints[-1] = end2pos + +# # points = np.vstack((points, temppoints)) + +# # #-------------------------- +# # # 计算起始点和结束点之间的总距离 +# # total_distance = np.linalg.norm(start3pos - end2pos) + +# # # 根据欧式距离步长为0.005估算所需的点数 +# # num_points = int(total_distance / dis) + +# # # 初始化用于存储点的数组 +# # temppoints = np.zeros((num_points + 1, 3)) +# # temppoints[0] = end2pos + +# # # 生成欧式距离差约为0.01的点 +# # for i in range(1, num_points + 1): +# # direction = (start3pos - end2pos) / total_distance # 单位方向向量 +# # temppoints[i] = temppoints[i-1] + direction * dis + +# # # 确保最后一个点与endpos完全重合 +# # temppoints[-1] = start3pos + +# # points = np.vstack((points, temppoints)) + +# # #-------------------------- +# # # 计算起始点和结束点之间的总距离 +# # total_distance = np.linalg.norm(end3pos - start3pos) + +# # # 根据欧式距离步长为0.005估算所需的点数 +# # num_points = int(total_distance / dis) + +# # # 初始化用于存储点的数组 +# # temppoints = np.zeros((num_points + 1, 3)) +# # temppoints[0] = start3pos + +# # # 生成欧式距离差约为0.01的点 +# # for i in range(1, num_points + 1): +# # direction = (end3pos - start3pos) / total_distance # 单位方向向量 +# # temppoints[i] = temppoints[i-1] + direction * dis + +# # # 确保最后一个点与endpos完全重合 +# # temppoints[-1] = end3pos + +# # points = np.vstack((points, temppoints)) + +# # #-------------------------- +# # # 计算起始点和结束点之间的总距离 +# # total_distance = np.linalg.norm(start4pos - end3pos) + +# # # 根据欧式距离步长为0.005估算所需的点数 +# # num_points = int(total_distance / dis) + +# # # 初始化用于存储点的数组 +# # temppoints = np.zeros((num_points + 1, 3)) +# # temppoints[0] = end3pos + +# # # 生成欧式距离差约为0.01的点 +# # for i in range(1, num_points + 1): +# # direction = (start4pos - end3pos) / total_distance # 单位方向向量 +# # temppoints[i] = temppoints[i-1] + direction * dis + +# # # 确保最后一个点与endpos完全重合 +# # temppoints[-1] = start4pos + +# # points = np.vstack((points, temppoints)) + +# # #-------------------------- +# # # 计算起始点和结束点之间的总距离 +# # total_distance = np.linalg.norm(end4pos - start4pos) + +# # # 根据欧式距离步长为0.005估算所需的点数 +# # num_points = int(total_distance / dis) + +# # # 初始化用于存储点的数组 +# # temppoints = np.zeros((num_points + 1, 3)) +# # temppoints[0] = start4pos + +# # # 生成欧式距离差约为0.01的点 +# # for i in range(1, num_points + 1): +# # direction = (end4pos - start4pos) / total_distance # 单位方向向量 +# # temppoints[i] = temppoints[i-1] + direction * dis + +# # # 确保最后一个点与endpos完全重合 +# # temppoints[-1] = end4pos + +# # points = np.vstack((points, temppoints)) + +# # #-------------------------- +# # # 计算起始点和结束点之间的总距离 +# # total_distance = np.linalg.norm(start5pos - end4pos) + +# # # 根据欧式距离步长为0.005估算所需的点数 +# # num_points = int(total_distance / dis) + +# # # 初始化用于存储点的数组 +# # temppoints = np.zeros((num_points + 1, 3)) +# # temppoints[0] = end4pos + +# # # 生成欧式距离差约为0.01的点 +# # for i in range(1, num_points + 1): +# # direction = (start5pos - end4pos) / total_distance # 单位方向向量 +# # temppoints[i] = temppoints[i-1] + direction * dis + +# # # 确保最后一个点与endpos完全重合 +# # temppoints[-1] = start5pos + +# # points = np.vstack((points, temppoints)) + +# # #-------------------------- +# # # 计算起始点和结束点之间的总距离 +# # total_distance = np.linalg.norm(end5pos - start5pos) + +# # # 根据欧式距离步长为0.005估算所需的点数 +# # num_points = int(total_distance / dis) + +# # # 初始化用于存储点的数组 +# # temppoints = np.zeros((num_points + 1, 3)) +# # temppoints[0] = start5pos + +# # # 生成欧式距离差约为0.01的点 +# # for i in range(1, num_points + 1): +# # direction = (end5pos - start5pos) / total_distance # 单位方向向量 +# # temppoints[i] = temppoints[i-1] + direction * dis + +# # # 确保最后一个点与endpos完全重合 +# # temppoints[-1] = end5pos + +# # points = np.vstack((points, temppoints)) + + +# positions_2d = np.vstack([start1pos,end1pos]) + + +# #print(points) + +# time_points = np.array([0,10]) +# time_step = 0.01 +# #print(time_points) +# #使用点云B样条插值 +# temppose = np.array([1,1,1,1,0,0]) +# positions_2d_interp = circle_trajectory(center=temppose,radius=0.05,time_points=time_points,time_step=time_step) +# quaternions_interp = np.tile(R.from_euler('xyz', np.array(temppose[3:])).as_quat(), (positions_2d_interp.shape[0], 1)) +# # positions_2d_interp, quaternions_interp = cloud_point_interpolate(positions=positions_2d, time_points=time_points, time_step=time_step) +# # print("2D Position Trajectory (Spline):") +# # print(positions_2d) + +# # # 分解轨迹的x, y, z坐标 +# x_trajectory, y_trajectory, z_trajectory = zip(*positions_2d_interp) + +# # 分解散点的x, y, z坐标 +# x_scatter, y_scatter, z_scatter = zip(*positions_2d) + +# # 创建一个3D图形 +# fig = plt.figure() +# ax = fig.add_subplot(111, projection='3d') + +# # # 设置字体为 SimHei (黑体) +# # plt.rcParams['font.sans-serif'] = ['SimHei'] # 使用黑体 +# # plt.rcParams['axes.unicode_minus'] = False # 解决负号无法正常显示的问题 + +# # 绘制3D连续轨迹 +# ax.plot(x_trajectory, y_trajectory, z_trajectory, label='B-spline interpolation trajectory', color='b') + +# # 绘制3D散点图 +# ax.scatter(x_scatter, y_scatter, z_scatter, label='cloud points', color='r', marker='o') + +# # 设置轴标签 +# ax.set_xlabel('X Label') +# ax.set_ylabel('Y Label') +# ax.set_zlabel('Z Label') + +# # 设置标题 +# ax.set_title('3D Trajectory and Scatter Plot') + +# # 添加图例 +# ax.legend() + +# # 设置轴的比例为相等 +# plt.gca().set_aspect('equal', adjustable='box') +# # # 显示图形 +# # plt.show() + +# plot_trajectory(positions_2d_interp, quaternions_interp) + +# # for i in range(10): +# # tempX = np.linspace(startpos[0], endpos[0], 10) + +# # 示例使用 +# positions_2d = np.array([[0, 0], [1, 1], [1, 2],[2,2]]) #, [1, 1], [2, 1], [1, 1], [1, 1],[0, 0]]) +# positions_3d = np.array([[0, 0, 0], [1, 1, 1], [2, 0, 2], [3, 1, 3]]) +# quaternions = np.array([[0, 0, 0, 1], [0.707, 0, 0, 0.707], [1, 0, 0, 0], [1, 0, 0, 0]]) +# time_points = np.array([0, 1, 3, 4]) +# time_step = 0.5 + +# # # 只插值2D位置 +# # positions_2d_interp = linear_interpolate(positions=positions_2d, time_points=time_points, time_step=time_step) +# # print("2D Position Trajectory:") +# # print(positions_2d_interp) + +# # # 只插值3D位置 +# # positions_3d_interp = linear_interpolate(positions=positions_3d, time_points=time_points, time_step=time_step) +# # print("3D Position Trajectory:") +# # print(positions_3d_interp) + +# # # 只插值姿态 +# # quaternions_interp = linear_interpolate(quaternions=quaternions, time_points=time_points, time_step=time_step) +# # print("Quaternion Trajectory:") +# # print(quaternions_interp) + +# # # 同时插值3D位置和姿态 +# # positions_3d_interp, quaternions_interp = linear_interpolate(positions=positions_3d, quaternions=quaternions, time_points=time_points, time_step=time_step) +# # print("3D Position and Quaternion Trajectory:") +# # print(positions_3d_interp) +# # print(quaternions_interp) + +# # # 绘制插值轨迹 +# # plot_trajectory(positions_2d_interp) +# # plot_trajectory(positions_3d_interp) +# # plot_trajectory(quaternions_interp) +# # plot_trajectory(positions_3d_interp, quaternions_interp) + +# # # 使用样条插值 +# # positions_2d_interp = spline_interpolate(positions=positions_2d, time_points=time_points, time_step=time_step) +# # print("2D Position Trajectory (Spline):") +# # print(positions_2d_interp) +# # plot_trajectory(positions_2d_interp) + +# # positions_3d_interp, quaternions_interp = spline_interpolate(positions=positions_3d, quaternions=quaternions, time_points=time_points, time_step=time_step) +# # print("3D Position and Quaternion Trajectory (Spline):") +# # print(positions_3d_interp) +# # print(quaternions_interp) +# # plot_trajectory(positions_3d_interp, quaternions_interp) + +# # # 使用点云B样条插值 +# # positions_2d_interp = cloud_point_interpolate(positions=positions_2d, time_points=time_points, time_step=time_step) +# # print("2D Position Trajectory (Spline):") +# # print(positions_2d_interp) +# # plot_trajectory(positions_2d_interp) + +# # positions_3d_interp, quaternions_interp = cloud_point_interpolate(positions=positions_3d, quaternions=quaternions, time_points=time_points, time_step=time_step) +# # print("3D Position and Quaternion Trajectory (Spline):") +# # print(positions_3d_interp) +# # print(quaternions_interp) +# # plot_trajectory(positions_3d_interp, quaternions_interp) \ No newline at end of file diff --git a/Massage/MassageControl/algorithms/positionerSensor_controller.py b/Massage/MassageControl/algorithms/positionerSensor_controller.py new file mode 100755 index 0000000..215329d --- /dev/null +++ b/Massage/MassageControl/algorithms/positionerSensor_controller.py @@ -0,0 +1,161 @@ +import sys +import numpy as np +from scipy.spatial.transform import Rotation as R +from .base_controller import BaseController +from .arm_state import ArmState +from pathlib import Path +import time +sys.path.append(str(Path(__file__).resolve().parent.parent)) + +from tools.yaml_operator import read_yaml +# from pykalman import KalmanFilter +import copy + +class PositionerSensorController(BaseController): + def __init__(self, name, state:ArmState,config_path) -> None: + super().__init__(name, state) + self.load_config(config_path) + + + def load_config(self, config_path): + config_dict = read_yaml(config_path) + if self.name != config_dict['name']: + raise ValueError(f"Controller name {self.name} does not match config name {config_dict['name']}") + desired_xi = np.array(config_dict['desired_xi']) + damp_tran = np.array(config_dict['damp_tran']) + # TODO 修改控制率 + self.e_t = 0 + self.e_t1 = 0 + self.e_t2 = 0 + self.force_control_value = 0 + + # 姿态 3x3矩阵 + self.Kp_R = np.diag(np.array(config_dict['Kp_R'])) + self.Ki_R = np.diag(np.array(config_dict['Ki_R'])) + self.Kd_R = np.diag(np.array(config_dict['Kd_R'])) + + # 姿态 3x3矩阵 + self.Kp_M = np.diag(np.array(config_dict['Kp_M'])) + self.Ki_M = np.diag(np.array(config_dict['Ki_M'])) + self.Kd_M = np.diag(np.array(config_dict['Kd_M'])) + + self.pose_integral_error = np.zeros(6) + + # 导纳xyz位置调节器 + mass_tran = np.array(config_dict['mass_tran']) + stiff_tran = np.array(config_dict['stiff_tran']) + damp_tran = np.array(config_dict['damp_tran']) + for i in range(1): + if damp_tran[i] < 0: + damp_tran[i] = 2 * desired_xi * np.sqrt(stiff_tran[i] * mass_tran[i]) + self.M_tran = np.diag(mass_tran) + self.K_tran = np.diag(stiff_tran) + self.D_tran = np.diag(damp_tran) + + self.laset_print_time = 0 + + def step(self,dt): + # 方向统一 + if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + self.state.arm_orientation = -self.state.arm_orientation + # 缓存常用计算 + arm_ori_quat = R.from_quat(self.state.arm_orientation) + arm_ori_mat = arm_ori_quat.as_matrix() + # 位置误差 + temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error + # 姿态误差(四元数) + rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation) + self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) + # 期望加速度 + wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + + self.pose_integral_error += self.state.pose_error * dt + # 姿态pid + self.state.arm_desired_acc[3:] = -self.Kd_R @ self.state.arm_desired_twist[3:] - self.Kp_R @ self.state.pose_error[3:] - self.Ki_R @ self.pose_integral_error[3:] + # 位控制 + self.state.arm_desired_acc[:2] = -self.Kd_M @ self.state.arm_desired_twist[:2] - self.Kp_M @ self.state.pose_error[:2] - self.Ki_M @ self.pose_integral_error[:2] + self.state.arm_desired_acc[2] = (1/self.M_tran) * (wrench_err[2] - self.D_tran * (self.state.arm_desired_twist[2] - self.state.desired_twist[2])-self.K_tran * self.state.pose_error[2]) + + self.clip_command(self.state.arm_desired_acc, "acc") + ## 更新速度和位姿 + self.state.arm_desired_twist += self.state.arm_desired_acc * dt + self.clip_command(self.state.arm_desired_twist, "vel") + # 计算位姿变化 + delta_pose = np.concatenate([ + arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt), + self.state.arm_desired_twist[3:] * dt + ]) + self.clip_command(delta_pose, "pose") + # 更新四元数 + delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat() + arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat) + self.state.arm_orientation_command = arm_ori_quat_new.as_quat() + # 归一化四元数 + self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) + # 更新位置 + self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + + def step_traj(self,dt): + # 方向统一 + if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + self.state.arm_orientation = -self.state.arm_orientation + # 缓存常用计算 + arm_ori_quat = R.from_quat(self.state.arm_orientation) + arm_ori_mat = arm_ori_quat.as_matrix() + # 位置误差 + + temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.desired_twist[:3] * dt + self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error + + # 姿态误差(四元数) + angular_velocity = np.array(self.state.desired_twist[3:]) # 形状 (3,) + + # 用旋转向量（小角度近似） + rotvec = angular_velocity * dt # 旋转向量 = 角速度 × 时间 + rot_quat = R.from_rotvec(rotvec).as_quat() # 转成四元数，形状 (4,) + rot_err_quat = R.from_quat(rot_quat).inv() * arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation) + self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) + + # 期望加速度 + wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + + self.pose_integral_error += self.state.pose_error * dt + # 姿态pid + self.state.arm_desired_acc[3:] = -self.Kd_R @ self.state.arm_desired_twist[3:] - self.Kp_R @ self.state.pose_error[3:] - self.Ki_R @ self.pose_integral_error[3:] + # 位控制 + self.state.arm_desired_acc[:2] = -self.Kd_M @ self.state.arm_desired_twist[:2] - self.Kp_M @ self.state.pose_error[:2] - self.Ki_M @ self.pose_integral_error[:2] + self.state.arm_desired_acc[2] = (1/self.M_tran) * (wrench_err[2] - self.D_tran * (self.state.arm_desired_twist[2] - self.state.desired_twist[2] + self.state.desired_acc[2]*dt)-self.K_tran * self.state.pose_error[2]) + self.state.desired_acc[2] + + + self.clip_command(self.state.arm_desired_acc, "acc") + ## 更新速度和位姿 + self.state.arm_desired_twist += self.state.arm_desired_acc * dt + self.clip_command(self.state.arm_desired_twist, "vel") + # 计算位姿变化 + delta_pose = np.concatenate([ + arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt), + self.state.arm_desired_twist[3:] * dt + ]) + self.clip_command(delta_pose, "pose") + # 更新四元数 + delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat() + arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat) + self.state.arm_orientation_command = arm_ori_quat_new.as_quat() + # 归一化四元数 + self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) + # 更新位置 + self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + + +if __name__ == "__main__": + state = ArmState() + controller = PositionerSensorController("admittance",state,"/home/zyc/admittance_control/MassageControl/config/admittance.yaml") + print(controller.name) + print(controller.state.arm_position) + state.arm_position = np.array([1,2,3]) + print(controller.state.arm_position) + print(controller.M) + print(controller.D) + print(controller.K) + diff --git a/Massage/MassageControl/config/admithybrid.yaml b/Massage/MassageControl/config/admithybrid.yaml new file mode 100755 index 0000000..9efbfab --- /dev/null +++ b/Massage/MassageControl/config/admithybrid.yaml @@ -0,0 +1,13 @@ +name: 'admithybrid' + +desired_xi: 1 +pos_scale_factor: 1 +mass_tran: [3.0, 3.0, 3.0] +stiff_tran: [270, 270, 270] +damp_tran: [30, 30, 30] + + +Kp: [1,1,1] +Ki: [0, 0, 0] +Kd: [1, 1, 1] + diff --git a/Massage/MassageControl/config/admittanceZ.yaml b/Massage/MassageControl/config/admittanceZ.yaml new file mode 100755 index 0000000..5a74508 --- /dev/null +++ b/Massage/MassageControl/config/admittanceZ.yaml @@ -0,0 +1,17 @@ +name: admittanceZ + + +desired_xi: 1 +pos_scale_factor: 1 +rot_scale_factor: 1 + +mass_tran: [3.0, 3.0, 3.0] +mass_rot: [0.11, 0.11, 0.11] +stiff_tran: [270, 270, 0] +stiff_rot: [11, 11, 11] +damp_tran: [30, 30, 30] +damp_rot: [1.1, 1.1, 1.1] + + + + diff --git a/Massage/MassageControl/config/hybrid.yaml b/Massage/MassageControl/config/hybrid.yaml new file mode 100755 index 0000000..bf95136 --- /dev/null +++ b/Massage/MassageControl/config/hybrid.yaml @@ -0,0 +1,17 @@ +name: 'hybrid' + +mass_rot: [0.11, 0.11, 0.11] +stiff_rot: [11, 11, 11] +damp_rot: [1.1, 1.1, 1.1] +desired_xi: 1.0 + +mass_z: [3.0] +stiff_z: [0] +damp_z: [30] + +# 位控参数 +Kp: [20,20] +Ki: [0.2,0.2] +Kd: [10,10] + + diff --git a/Massage/MassageControl/config/hybridAdmit.yaml b/Massage/MassageControl/config/hybridAdmit.yaml new file mode 100755 index 0000000..e6c1d5d --- /dev/null +++ b/Massage/MassageControl/config/hybridAdmit.yaml @@ -0,0 +1,23 @@ +name: 'hybridAdmit' + +mass_z: [3.0] +stiff_z: [0] +damp_z: [30] + +# 位控参数 +Kp: [20,20] +Ki: [0.2,0.2] +Kd: [10,10] + +# 位控参数 +Kp_R: [1,1,1] +Ki_R: [0,0,0] +Kd_R: [1,1,1] + + + + +# # 位控参数 +# Kp: [90,90] +# Kd: [25,25] + diff --git a/Massage/MassageControl/config/hybridPid.yaml b/Massage/MassageControl/config/hybridPid.yaml new file mode 100755 index 0000000..1c47ad9 --- /dev/null +++ b/Massage/MassageControl/config/hybridPid.yaml @@ -0,0 +1,22 @@ +name: 'hybridPid' + + +# 力控参数 +force_mass: 2.5 +force_damp: 25 +Kp_force: 2.0 #0.6 +Kd_force: 0.0 +Ki_force: 0.0 #0 + + +# 位控参数 +Kp: [20,20] +Ki: [0.2,0.2] +Kd: [10,10] + +# 位控参数 +Kp_R: [1,1,1] +Ki_R: [0,0,0] +Kd_R: [1,1,1] + + diff --git a/Massage/MassageControl/config/positionerSensor.yaml b/Massage/MassageControl/config/positionerSensor.yaml new file mode 100755 index 0000000..5b61039 --- /dev/null +++ b/Massage/MassageControl/config/positionerSensor.yaml @@ -0,0 +1,18 @@ +name: positionerSensor + +desired_xi: 1 +pos_scale_factor: 1 +mass_tran: [3.0] +stiff_tran: [270] +damp_tran: [30] + + +Kp_R: [1,1,1] +Ki_R: [0, 0, 0] +Kd_R: [1, 1, 1] + +# 位控参数 +Kp_M: [20,20] +Ki_M: [0.2,0.2] +Kd_M: [10,10] + diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py index acca5c1..236ef1f 100644 --- a/Massage/MassageControl/hardware/dobot_nova5.py +++ b/Massage/MassageControl/hardware/dobot_nova5.py @@ -5,6 +5,7 @@ except: import threading from time import sleep,time import re +from tools.log import CustomLogger import copy import numpy as np from scipy.spatial.transform import Rotation as R @@ -35,8 +36,8 @@ import math ''' class dobot_nova5: - def __init__(self,ip): - self.ip = ip + def __init__(self,arm_ip = '192.168.5.1'): + self.ip = arm_ip ## 机械臂IP地址 self.dashboardPort = 29999 ## 一发一收-设置-控制及运动端口号 self.feedFourPort = 30004 ## 第四实时反馈(8ms)反馈数据端口号 self.dashboard = None ## 初始化控制及运动端口实例 @@ -60,6 +61,8 @@ class dobot_nova5: self.ToolValue = -1 ''' self.feedbackData = feedbackItem() # 自定义反馈数据结构体 + self.logger = CustomLogger('Dobot_nova5',True) + def parseResultId(self,valueRecv): # 解析返回值 @@ -170,12 +173,14 @@ class dobot_nova5: self.dashboard.Stop() self.dashboard.EmergencyStop(mode=1) print("当前工作停止，机械臂处于急停状态") + return -1 if self.feedbackData.robotMode == 10: # 10 当前工程暂停 print("机械臂为暂停状态") res = self.dashboard.Continue() code = self.parseResultId(res)[0] if code == 0: print("机械臂继续已暂停的运动指令") + return -1 if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID: print("该次运动完成") break diff --git a/Massage/MassageControl/tools/decorator_tools.py b/Massage/MassageControl/tools/decorator_tools.py new file mode 100755 index 0000000..da0ac03 --- /dev/null +++ b/Massage/MassageControl/tools/decorator_tools.py @@ -0,0 +1,29 @@ +from functools import wraps + +def custom_decorator(before=None, after=None): + """ + 创建一个自定义装饰器，可以在函数调用之前和之后执行自定义操作。 + + :param before: 在函数调用之前执行的操作，接受同样的参数。 + :param after: 在函数调用之后执行的操作，接受同样的参数和返回值。 + :return: 一个装饰器。 + """ + def decorator(func): + @wraps(func) + def wrapper(self, *args, **kwargs): + if before: + before(self, *args, **kwargs) + result = func(self, *args, **kwargs) + if after: + after(self, result, *args, **kwargs) + return result + return wrapper + return decorator + +def apply_decorators(cls): + for attr_name, attr_value in cls.__dict__.items(): + if callable(attr_value) and hasattr(attr_value, '_decorator'): + decorated_func = attr_value._decorator(attr_value) + setattr(cls, attr_name, decorated_func) + return cls + diff --git a/Massage/MassageControl/tools/serial_tools.py b/Massage/MassageControl/tools/serial_tools.py new file mode 100755 index 0000000..f25c484 --- /dev/null +++ b/Massage/MassageControl/tools/serial_tools.py @@ -0,0 +1,94 @@ +import serial.tools.list_ports as list_ports +import subprocess + +def find_serial_by_serial_number(serial_number): + ports = list_ports.comports() + for port in ports: + if port.serial_number == serial_number: + return port.device + return None + +def find_serial_by_location(location): + ports = list_ports.comports() + for port in ports: + if port.location == location: + return port.device + return None + +def list_usb_ports_with_details(): + ports = list_ports.comports() + usb_ports = {} + + for port in ports: + if "ttyUSB" in port.device: + usb_ports[port.device] = { + 'serial_number': port.serial_number, + 'vid': port.vid, + 'pid': port.pid, + 'location': port.location, + 'description': port.description, + 'manufacturer': port.manufacturer + } + + return usb_ports + +# 维护 socat 进程的字典，用于跟踪每个虚拟串口的进程 +socat_processes = {} + +def start_virtual_serial(remote_ip, remote_port, device_name): + """ + 启动一个 socat 进程来创建虚拟串口。 + :param remote_ip: 远程主机 IP 地址 + :param remote_port: 远程主机端口 + :param device_name: 虚拟串口的设备名称 + """ + # 构建 socat 命令 + socat_command = f'socat PTY,raw,echo=0,link={device_name} TCP:{remote_ip}:{remote_port}' + # 启动 socat 进程 + process = subprocess.Popen(socat_command, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + # 将进程存储到字典中 + socat_processes[device_name] = process + print(f"Started socat process for {device_name} on {remote_ip}:{remote_port}") + +def stop_virtual_serial(device_name): + """ + 停止一个 socat 进程。 + :param device_name: 虚拟串口的设备名称 + """ + # 检查进程是否在字典中 + if device_name in socat_processes: + # 获取进程对象 + process = socat_processes[device_name] + # 使用 pkill 终止相应的 socat 进程 + subprocess.call(['pkill', '-f', f'{device_name}']) + # 终止进程 + process.terminate() + process.wait() + # 移除已终止的进程 + del socat_processes[device_name] + print(f"Stopped socat process for {device_name}") + else: + print(f"No running socat process found for {device_name}") + +if __name__ == "__main__": + usb_ports_with_details = list_usb_ports_with_details() + + if usb_ports_with_details: + print("USB Ports and their Details:") + for device, details in usb_ports_with_details.items(): + print(f"Device: {device}, Serial Number: {details['serial_number']}, VID: {details['vid']}, PID: {details['pid']}, Location: {details['location']}, Description: {details['description']}, Manufacturer: {details['manufacturer']}") + else: + print("No USB ports found.") + + # serial_number = "1234567890" # Replace with your serial number + # port = find_serial_by_serial_number(serial_number) + # if port: + # print(f"Serial number {serial_number} found on port {port}") + # else: + # print(f"Serial number {serial_number} not found") + + port = find_serial_by_location("1-8") + if port: + print(f"Port found: {port}") + else: + print("Port not found") \ No newline at end of file diff --git a/Massage/aucpuncture2point/configs/using_img/depth_c.png b/Massage/aucpuncture2point/configs/using_img/depth_c.png old mode 100644 new mode 100755 index b3ecb33..4ea541c Binary files a/Massage/aucpuncture2point/configs/using_img/depth_c.png and b/Massage/aucpuncture2point/configs/using_img/depth_c.png differ