实现接触开关时m.if2与m.abs2的问题排查及咨询

背景 #

这是此前问题的延续，模型已解决完全接触条件下的求解问题，可变接触约束框架搭建完成，但接触开关变量应用于几何约束时无解（动力学方程中使用正常），同时存在m.if2输出异常、柔化约束失效、积分式开关实现疑问三个核心问题。

问题1：m.if2输出c[0]异常 #

异常现象 #

根据swtch = adiff*bdiff和c = m.if2(swtch-thres,0,1)的逻辑，当a=-0.1、b=1时，初始时刻swtch[0] = 0.1*1.0 = 0.1 > thres=0.001，c[0]应为1，但实际输出为0。

原因与修正 #

adiff和bdiff被定义为m.Param，初始化时直接使用m.time-a.VALUE和b.VALUE-m.time会导致它们在模型编译阶段就固定为静态数组，无法随求解过程动态关联。

修正代码：
将adiff和bdiff改为m.Intermediate，让模型动态计算每个时间点的差值：

# 替换原adiff、bdiff定义
adiff = m.Intermediate(m.time - a)  # 动态计算每个时刻的m.time - a
bdiff = m.Intermediate(b - m.time)  # 动态计算每个时刻的b - m.time
swtch = m.Intermediate(adiff * bdiff)
thres = 0.001
c = m.if2(swtch - thres, 0, 1)

问题2：几何约束柔化后无法求解 #

问题现象 #

将硬几何约束改为c*m.abs2(constraint) <= tol后，即使不使用c、设置宽松容差也无解，但硬约束正常。

原因与修正 #

柔化约束使用m.abs2时，非接触阶段若没有合理松弛空间，求解器会陷入矛盾；直接乘以开关变量c可能导致切换点不连续，增加求解难度。

修正方案：

1. 为非接触阶段的几何约束设置宽松松弛范围
2. 使用m.if3替代m.if2，获得连续可微的平滑开关（更适合优化问题）
3. 拆分约束：接触时严格约束，非接触时松弛约束

# 替换原几何约束代码
c_smooth = m.if3(swtch - thres, 0, 1)  # 连续可微的开关函数

# 接触时约束严格，非接触时允许松弛
tol_contact = 1e-3
tol_non_contact = 0.5  # 非接触阶段的宽松容差
m.Equation(m.abs2(x + L1*m.sin(q1) + L2*m.sin(q1+q2)) <= tol_contact*c_smooth + tol_non_contact*(1-c_smooth))
m.Equation(m.abs2(y - L1*m.cos(q1) - L2*m.cos(q1+q2)) <= tol_contact*c_smooth + tol_non_contact*(1-c_smooth))

若仍无解，调整求解器选项：

m.options.SOLVER = 1  # 改用APOPT求解器，更适配混合整数/不连续问题
m.options.MAX_ITER = 1000  # 增加最大迭代次数
m.options.COLDSTART = 2  # 启用冷启动，帮助找到初始可行解

问题3：用m.integral实现t=a启用、t=b关闭的开关 #

实现原理 #

通过积分阶跃函数，实现开关在t=a时从0上升到1，t=b时从1下降到0。结合m.if3定义连续可微的阶跃信号，再积分得到平滑开关变量。

代码示例（归一化时间版） #

假设a和b是归一化时间（范围[0,1]）：

# 定义阶跃信号：t>a时为1，否则为0
step_a = m.if3(m.time - a, 0, 1)
# 定义阶跃信号：t<b时为1，否则为0
step_b = m.if3(b - m.time, 0, 1)
# 积分并归一化，得到a<=t<=b时为1，其余时间为0的开关
c_integral = m.integral(step_a * step_b) / (b - a)

# 可选：添加滤波进一步平滑开关
tau = 0.01  # 时间常数，控制平滑程度
c_smooth = m.Var(value=0)
m.Equation(tau * c_smooth.dt()/TF == c_integral - c_smooth)

若a和b是实际时间（秒），需先转换为归一化时间：

a_norm = m.Param(value=a / TF.VALUE)
b_norm = m.Param(value=b / TF.VALUE)
step_a = m.if3(m.time - a_norm, 0, 1)
step_b = m.if3(b_norm - m.time, 0, 1)
c_integral = m.integral(step_a * step_b) / (b_norm - a_norm)

核心修正后的代码片段 #

整合以上修正的核心部分：

###Intermediates
xdot_int = m.Intermediate(final*m.integral(xdot))  #for average velocity constraint
# 修正：将adiff、bdiff改为动态Intermediate
adiff = m.Intermediate(m.time - a)
bdiff = m.Intermediate(b - m.time)
swtch = m.Intermediate(adiff*bdiff) #positive if m.time > a AND m.time < b
thres = .001
# 修正：使用m.if3获得连续可微开关
c = m.if3(swtch-thres,0,1) #c=0 if swtch <0, c=1 if swtch >0

###Geometric constraints
# 修正：使用柔化约束，区分接触/非接触阶段容差
tol_contact = 1e-3
tol_non_contact = 0.5
m.Equation(m.abs2(x + L1*m.sin(q1) + L2*m.sin(q1+q2)) <= tol_contact*c + tol_non_contact*(1-c))
m.Equation(m.abs2(y - L1*m.cos(q1) - L2*m.cos(q1+q2)) <= tol_contact*c + tol_non_contact*(1-c))

###Solve
# 修正：调整求解器选项
m.options.IMODE = 6
m.options.SOLVER = 1  # 改用APOPT求解器
m.options.MAX_ITER = 1000
m.options.COLDSTART = 2
m.solve()

内容的提问来源于stack exchange，提问作者T Bounds