企业官网建设流程全解析

兰州做网站公司,域名不转出可以做网站吗,网站型建设模板,宁波网站建设熊掌号def compute_V0_screw_motion(x, R, xd, Rd, max_lin_vel0.05, max_ang_vel0.1, beta1.0):基于螺旋运动理论计算引导速度#xff0c;更符合物理运动参数#xff1a;beta: 收敛速度系数# 计算位置误差pos_error xd - x# 计算姿态误差0.05, max_ang_vel0.1, beta1.0): 基于螺旋运动理论计算引导速度更符合物理运动 参数 beta: 收敛速度系数 # 计算位置误差 pos_error xd - x # 计算姿态误差轴角表示 R_err Rd.T R angle np.arccos(np.clip((np.trace(R_err) - 1) / 2, -1, 1)) if angle 1e-6: axis np.array([0, 0, 1]) else: axis np.array([ R_err[2, 1] - R_err[1, 2], R_err[0, 2] - R_err[2, 0], R_err[1, 0] - R_err[0, 1] ]) / (2 * np.sin(angle) 1e-10) # 计算螺旋轴上的运动 if angle 1e-6: # 纯平移运动 lin_vel beta * pos_error ang_vel np.zeros(3) else: # 计算瞬时螺旋轴 screw_axis axis # 计算螺旋运动上的点选择目标点 p xd # 计算线速度和角速度 ang_vel beta * angle * screw_axis # 计算伴随线速度 v_parallel np.cross(ang_vel, p - x) v_perpendicular beta * (pos_error - np.dot(pos_error, screw_axis) * screw_axis) lin_vel v_parallel v_perpendicular # 限幅 lin_vel_norm np.linalg.norm(lin_vel) if lin_vel_norm max_lin_vel: lin_vel lin_vel * (max_lin_vel / lin_vel_norm) ang_vel_norm np.linalg.norm(ang_vel) if ang_vel_norm max_ang_vel: ang_vel ang_vel * (max_ang_vel / ang_vel_norm) # 转换到体坐标系 V0_linear R.T lin_vel V0_angular R.T ang_vel return np.vstack([V0_linear.reshape(-1, 1), V0_angular.reshape(-1, 1)]) def compute_V0_time_optimal(x, R, xd, Rd, max_lin_vel0.05, max_ang_vel0.1, k_lin0.8, k_ang1.2): 基于距离加权的时间最优引导速度 参数 k_lin: 线速度的Sigmoid形状参数 k_ang: 角速度的Sigmoid形状参数 # 计算位置距离 pos_dist np.linalg.norm(xd - x) # 计算姿态距离旋转角度 R_err Rd.T R cos_theta (np.trace(R_err) - 1) / 2 cos_theta np.clip(cos_theta, -1, 1) rot_dist np.arccos(cos_theta) # 使用Sigmoid函数调整速度接近目标时减速 def sigmoid_weight(dist, k): 距离越近权重越小减速 return 1 / (1 np.exp(-k * (dist - 0.02))) # 0.02m或rad为切换点 # 计算线速度方向 if pos_dist 1e-6: lin_dir (xd - x) / pos_dist else: lin_dir np.zeros(3) # 计算角速度方向旋转轴 if rot_dist 1e-6: R_err_skew (R_err - R_err.T) / 2 ang_dir np.array([R_err_skew[2, 1], R_err_skew[0, 2], R_err_skew[1, 0]]) ang_dir ang_dir / (np.linalg.norm(ang_dir) 1e-10) else: ang_dir np.zeros(3) # 计算速度大小 lin_vel_mag max_lin_vel * sigmoid_weight(pos_dist, k_lin) ang_vel_mag max_ang_vel * sigmoid_weight(rot_dist, k_ang) # 组合速度 lin_vel lin_vel_mag * lin_dir ang_vel ang_vel_mag * ang_dir # 转换到体坐标系 V0_linear R.T lin_vel V0_angular R.T ang_vel return np.vstack([V0_linear.reshape(-1, 1), V0_angular.reshape(-1, 1)]) def compute_V0_interpolated(x, R, xd, Rd, max_lin_vel0.05, max_ang_vel0.1, interp_typelinear): 基于插值的引导速度特别适用于PiH任务 # 计算位置和姿态误差 pos_error xd - x pos_error_norm np.linalg.norm(pos_error) # 计算姿态误差 R_err Rd.T R cos_theta (np.trace(R_err) - 1) / 2 cos_theta np.clip(cos_theta, -1, 1) rot_error np.arccos(cos_theta) # 使用不同的插值策略 if interp_type linear: # 线性插值 pos_weight min(1.0, pos_error_norm / 0.1) # 0.1m为参考距离 rot_weight min(1.0, rot_error / 0.5) # 0.5rad为参考角度 elif interp_type smooth: # 平滑插值三次多项式 pos_weight 3 * (min(1.0, pos_error_norm / 0.1)) ** 2 \ - 2 * (min(1.0, pos_error_norm / 0.1)) ** 3 rot_weight 3 * (min(1.0, rot_error / 0.5)) ** 2 \ - 2 * (min(1.0, rot_error / 0.5)) ** 3 elif interp_type adaptive: # 自适应插值根据误差比例调整 total_error pos_error_norm rot_error * 0.1 # 角度误差权重较低 if total_error 0.01: pos_weight pos_error_norm / total_error rot_weight (rot_error * 0.1) / total_error else: pos_weight rot_weight 0.5 # 计算线速度 if pos_error_norm 1e-6: lin_dir pos_error / pos_error_norm lin_speed max_lin_vel * pos_weight lin_vel lin_speed * lin_dir else: lin_vel np.zeros(3) # 计算角速度使用SLERP思想 if rot_error 1e-6: # 计算旋转轴 R_err_skew (R_err - R_err.T) / 2 ang_axis np.array([R_err_skew[2, 1], R_err_skew[0, 2], R_err_skew[1, 0]]) ang_axis ang_axis / (np.linalg.norm(ang_axis) 1e-10) ang_speed max_ang_vel * rot_weight ang_vel ang_speed * ang_axis else: ang_vel np.zeros(3) # 转换到体坐标系 V0_linear R.T lin_vel V0_angular R.T ang_vel return np.vstack([V0_linear.reshape(-1, 1), V0_angular.reshape(-1, 1)])机器人操作空间速度计算python几种实现函数