如何在Pydrake的DirectTranscription中用符号值调用PiecewisePolynomial.value？

能否以符号值作为参数t，调用pydrake.trajectories.PiecewisePolynomial类的trajectory.value(t)方法？我的应用场景是在DirectTranscription的代价函数中显式使用该轨迹，代价形式为：
$$J = \sum (x[k] - x_d[k])^T Q (x[k] - x_d[k])$$
约束条件：$x[k+1] = f(x[k], u[k])$

我曾使用简单的numpy函数定义$x_d[k]$，代码框架如下：

xd = lambda t: np.sin(t)
dirtran = DirectTranscription(sys, sys.CreateDefaultContext(), horizon + 1)
x_k = dirtran.state()
t_k= dirtran.time()

x_k_decision = x_k - xd(t_k[0])
dirtran.AddRunningCost(x_k_decision.transpose() @ Q @ x_k_decision)

该方案运行正常，求解器能生成跟踪$x_d=\sin(t)$的正确输入轨迹。现在我希望用同样方法跟踪轨迹优化（如直接配置法）得到的轨迹，具体是ReconstructStateTrajectory返回的PiecewisePolynomial类型轨迹，代码框架如下：

xd = dircol.ReconstructStateTrajectory(result)
dirtran = DirectTranscription(sys, sys.CreateDefaultContext(), horizon + 1)
x_k = dirtran.state()
t_k= dirtran.time()

x_k_decision = x_k - xd.value(t_k[0])
dirtran.AddRunningCost(x_k_decision.transpose() @ Q @ x_k_decision)

但该方案无法运行，因为t_k实际是(Variable 't(0)', Continuous)类型，不符合.value()的参数要求。我查看C++源码发现，t(0)的实际值会在DoAddRunningCost中通过SubstitutePlaceholderVariables替换，此时才能正确调用.value()，但程序在执行x_k_decision = x_k - xd.value(t_k[0])时已抛出错误："The following environment does not have an entry for the variable t(0)"。

一个显而易见的解决方案是手动编写循环并自行递增时间步（这样可以用浮点数调用.value()，而非Variable占位符），但这会破坏DirectTranscription的简洁性。请问有没有方法能在AddRunningCost中使用PiecewisePolynomial？

我知道可以通过变换坐标，求解$(x[k] - x_d[k])$而非$x[k]$，但编写MathematicalProgram时需要额外注意。像第一个代码框架那样的写法非常简洁直观（且能正常运行），我只是找不到在参考轨迹为DirectCollocation输出时的实现方法。

你可以利用PiecewisePolynomial的to_symbolic()方法，将分段多项式轨迹转换为符号表达式，这样就能直接接受DirectTranscription中的符号时间变量作为输入，完全适配你的简洁代码框架。

修改后的代码示例如下：

xd = dircol.ReconstructStateTrajectory(result)
# 将PiecewisePolynomial转换为符号表达式函数
xd_symbolic = xd.to_symbolic()

dirtran = DirectTranscription(sys, sys.CreateDefaultContext(), horizon + 1)
x_k = dirtran.state()
t_k= dirtran.time()

# 直接用符号时间变量调用xd_symbolic，无需.value()
x_k_decision = x_k - xd_symbolic(t_k[0])
dirtran.AddRunningCost(x_k_decision.transpose() @ Q @ x_k_decision)

原理说明 #

• to_symbolic()会把分段多项式的每一段转换为符号多项式，最终返回一个接受符号变量的函数；
• 当AddRunningCost处理符号表达式时，Drake会自动完成后续的变量替换与数值计算，完全兼容原有的DirectTranscription工作流程；
• 这种写法和你用lambda t: np.sin(t)的形式完全一致，既保持了代码简洁性，又能正确使用DirectCollocation输出的轨迹作为参考。

另外，如果你需要处理更复杂的分段轨迹（比如带不连续点的），to_symbolic()同样能正确生成对应的分段符号表达式，无需额外处理分段逻辑。

内容的提问来源于stack exchange，提问作者Tin Tran