仿照加入其他控制器
This commit is contained in:
swayneleong
parent
b96c979023
commit
b172aef382
135
Massage/MassageControl/algorithms/admithybrid_controller.py
Executable file
135
Massage/MassageControl/algorithms/admithybrid_controller.py
Executable file
@ -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]
|
||||
153
Massage/MassageControl/algorithms/admittanceZ_controller.py
Executable file
153
Massage/MassageControl/algorithms/admittanceZ_controller.py
Executable file
@ -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)
|
||||
273
Massage/MassageControl/algorithms/force_planner.py
Normal file
273
Massage/MassageControl/algorithms/force_planner.py
Normal file
@ -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()
|
||||
|
||||
|
||||
|
||||
|
||||
150
Massage/MassageControl/algorithms/hybridAdmit_controller.py
Executable file
150
Massage/MassageControl/algorithms/hybridAdmit_controller.py
Executable file
@ -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]
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
161
Massage/MassageControl/algorithms/hybridPid_controller.py
Executable file
161
Massage/MassageControl/algorithms/hybridPid_controller.py
Executable file
@ -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]
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
150
Massage/MassageControl/algorithms/hybrid_controller.py
Executable file
150
Massage/MassageControl/algorithms/hybrid_controller.py
Executable file
@ -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]
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
817
Massage/MassageControl/algorithms/interpolation.py
Executable file
817
Massage/MassageControl/algorithms/interpolation.py
Executable file
@ -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)
|
||||
161
Massage/MassageControl/algorithms/positionerSensor_controller.py
Executable file
161
Massage/MassageControl/algorithms/positionerSensor_controller.py
Executable file
@ -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)
|
||||
|
||||
13
Massage/MassageControl/config/admithybrid.yaml
Executable file
13
Massage/MassageControl/config/admithybrid.yaml
Executable file
@ -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]
|
||||
|
||||
17
Massage/MassageControl/config/admittanceZ.yaml
Executable file
17
Massage/MassageControl/config/admittanceZ.yaml
Executable file
@ -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]
|
||||
|
||||
|
||||
|
||||
|
||||
17
Massage/MassageControl/config/hybrid.yaml
Executable file
17
Massage/MassageControl/config/hybrid.yaml
Executable file
@ -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]
|
||||
|
||||
|
||||
23
Massage/MassageControl/config/hybridAdmit.yaml
Executable file
23
Massage/MassageControl/config/hybridAdmit.yaml
Executable file
@ -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]
|
||||
|
||||
22
Massage/MassageControl/config/hybridPid.yaml
Executable file
22
Massage/MassageControl/config/hybridPid.yaml
Executable file
@ -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]
|
||||
|
||||
|
||||
18
Massage/MassageControl/config/positionerSensor.yaml
Executable file
18
Massage/MassageControl/config/positionerSensor.yaml
Executable file
@ -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]
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user