Crocoddyl 使用教程（二）

news2024/11/20 4:45:10

系列文章目录

前言

小车摆杆是另一个经典的控制实例。在这个系统中，一根欠驱动的杆子被固定在一辆一维驱动的小车顶部。游戏的目的是将杆子升到站立位置。

模型如下： https://en.wikipedia.org/wiki/Inverted_pendulum

我们用 $eq?m_1$ 表示小车质量、 $eq?m_2$ 表示摆杆质量 ( $eq?m%3Dm_1+m_2$ )、 $eq?l$ 表示摆杆长度、 $eq?%5Ctheta$ 表示摆杆相对于垂直轴的角度、 $eq?p$ 表示小车位置、 $eq?g%3D9.81$ 表示重力。

系统加速度可重写为

$eq?%5Cddot%7B%5Ctheta%7D%3D%5Cdfrac1%7B%5Cmu%28%5Ctheta%29%7D%5Cbig%28%5Cdfrac%7B%5Ccos%5Ctheta%7Dlf+%5Cdfrac%7Bmg%7Dl%5Csin%28%5Ctheta%29-m_2%5Ccos%28%5Ctheta%29%5Csin%28%5Ctheta%29%5Cdot%7B%5Ctheta%7D%5E2%5Cbig%29%2C%5C%5C%5Cddot%7Bp%7D%3D%5Cdfrac1%7B%5Cmu%28%5Ctheta%29%7D%5Cbig%28f+m_2%5Ccos%28%5Ctheta%29%5Csin%28%5Ctheta%29g-m_2l%5Csin%28%5Ctheta%29%5Cdot%7B%5Ctheta%7D%5Cbig%29%2C$

其中，

$eq?%5Cmu%28%5Ctheta%29%3Dm_1+m_2%5Csin%28%5Ctheta%29%5E2%2C$

其中， $eq?f$ 代表输入指令（即 $eq?f%3Du$ ）， $eq?m%3Dm_1+m_2$ 代表总质量。

一、微分动作模型

微分动作模型（DAM）描述的是连续时间内的动作（控制/动力学）。在本练习中，我们要求您编写小车摆杆的运动方程。

更多详情，请参阅 DifferentialActionModelCartpole 类中的说明。

import crocoddyl
import pinocchio
import numpy as np
from IPython.display import HTML
from cartpole_utils import animateCartpole


class DifferentialActionModelCartpole(crocoddyl.DifferentialActionModelAbstract):
    def __init__(self):
        crocoddyl.DifferentialActionModelAbstract.__init__(
            self, crocoddyl.StateVector(4), 1, 6
        )  # nu = 1; nr = 6
        self.unone = np.zeros(self.nu)

        self.m1 = 1.0
        self.m2 = 0.1
        self.l = 0.5
        self.g = 9.81
        self.costWeights = [
            1.0,
            1.0,
            0.1,
            0.001,
            0.001,
            1.0,
        ]  # sin, 1-cos, x, xdot, thdot, f

    def calc(self, data, x, u=None):
        if u is None:
            u = model.unone
        # Getting the state and control variables
        y, th, ydot, thdot = x[0].item(), x[1].item(), x[2].item(), x[3].item()
        f = u[0].item()

        # Shortname for system parameters
        m1, m2, l, g = self.m1, self.m2, self.l, self.g
        s, c = np.sin(th), np.cos(th)

        ###########################################################################
        ############ TODO: Write the dynamics equation of your system #############
        ###########################################################################
        # Hint:
        # You don't need to implement integration rules for your dynamic system.
        # Remember that DAM implemented action models in continuous-time.
        m = m1 + m2
        mu = m1 + m2 * s**2
        xddot, thddot = cartpole_dynamics(
            self, data, x, u
        )  # Write the cartpole dynamics here
        data.xout = np.matrix([xddot, thddot]).T

        # Computing the cost residual and value
        data.r = np.matrix(self.costWeights * np.array([s, 1 - c, y, ydot, thdot, f])).T
        data.cost = 0.5 * sum(np.asarray(data.r) ** 2).item()

    def calcDiff(model, data, x, u=None):
        # Advance user might implement the derivatives in cartpole_analytical_derivatives
        cartpole_analytical_derivatives(model, data, x, u)

取消下面一行的注释，就能得到小车摆杆动力学的解：

# %load solutions/cartpole_dyn.py

您可能需要检查一下您的计算结果。以下是创建模型和运行计算方法的方法。

cartpoleDAM = DifferentialActionModelCartpole()
cartpoleData = cartpoleDAM.createData()
x = cartpoleDAM.state.rand()
u = np.zeros(1)
cartpoleDAM.calc(cartpoleData, x, u)

二、使用 DAMNumDiff 写导数

在前面的练习中，我们没有定义 cartpole 系统的导数。在 crocoddyl 中，我们可以利用 DifferentialActionModelNumDiff 类来计算导数，而无需任何额外代码。该类通过数值微分计算导数。

在下面的单元格中，您需要创建一个使用 NumDiff 计算导数的 cartpole DAM。

# Creating the carpole DAM using NumDiff for computing the derivatives.
# We specify the withGaussApprox=True to have approximation of the
# Hessian based on the Jacobian of the cost residuals.
cartpoleND = crocoddyl.DifferentialActionModelNumDiff(cartpoleDAM, True)

使用 NumDiff 创建 cartpole DAM 后。我们希望您能回答以下问题：

Fx 的 2 列为空。是 Wich 列吗？为什么？
能否仔细检查一下 Fu 的值？

# %load solutions/cartpole_fxfu.py

三、积分模型

为 cartpole 系统创建 DAM 后。我们需要创建一个积分动模型（IAM）。请注意，IAM 将连续时间动作模型转换为离散时间动作模型。在本练习中，我们将使用简单欧拉积分器。

# %load solutions/cartpole_integration.py
###########################################################################
################## TODO: Create an IAM with from the DAM ##################
###########################################################################
# Hint:
# Use IntegratedActionModelEuler

四、编写问题，创建求解器

首先，您需要描述射击问题。为此，您必须说明步的数量及其时间步长。在本练习中，我们希望使用 50 步和 5e-2。

下面是我们创建问题的方法。

# Fill the number of knots (T) and the time step (dt)
x0 = np.array([0.0, 3.14, 0.0, 0.0])
T = 50
problem = crocoddyl.ShootingProblem(x0, [cartpoleIAM] * T, cartpoleIAM)

问题不能解决，只能积分：

us = [np.zeros(cartpoleIAM.differential.nu)] * T
xs = problem.rollout(us)

在 cartpole_utils 中，我们提供了 plotCartpole 和 animateCartpole 方法。让我们展示一下这个滚动条！

请注意，to_jshtml 会生成视频控制命令。

HTML(animateCartpole(xs).to_jshtml())

# %load solutions/cartpole_ddp.py
# #########################################################################
# ################# TODO: Create the DDP solver and run it ###############
# ##########################################################################

HTML(animateCartpole(ddp.xs.tolist()).to_jshtml())

五、调整问题，解决问题

指出解决问题的方法。

没有终端模型，我们可以看到一些波动，但无法稳定下来。怎么办？
最重要的是达到站立位置。我们还能使速度失效吗？
增加所有权重是行不通的。如何慢慢增加惩罚？

# %load solutions/cartpole_tuning.py
# ##########################################################################
# ################# TODO: Tune the weights for each cost ###################
# ##########################################################################
terminalCartpole = DifferentialActionModelCartpole()
terminalCartpoleDAM = crocoddyl.DifferentialActionModelNumDiff(terminalCartpole, True)
terminalCartpoleIAM = crocoddyl.IntegratedActionModelEuler(terminalCartpoleDAM)

terminalCartpole.costWeights[0] = 0  # fix me :)
terminalCartpole.costWeights[1] = 0  # fix me :)
terminalCartpole.costWeights[2] = 0  # fix me :)
terminalCartpole.costWeights[3] = 0  # fix me :)
terminalCartpole.costWeights[4] = 0  # fix me :)
terminalCartpole.costWeights[5] = 0  # fix me :)
problem = crocoddyl.ShootingProblem(x0, [cartpoleIAM] * T, terminalCartpoleIAM)

# Creating the DDP solver
ddp = crocoddyl.SolverDDP(problem)
ddp.setCallbacks([crocoddyl.CallbackVerbose()])

# Solving this problem
done = ddp.solve([], [], 300)
print(done)

HTML(animateCartpole(ddp.xs.tolist()).to_jshtml())

六、使用分析导数

取消下面一行的注释，就能得到分析导数的解：

# %load solutions/cartpole_analytical_derivatives.py
def cartpole_analytical_derivatives(model, data, x, u=None):
    pass

在定义了分析导数后，我们就不需要使用 DAMNumDiff 对导数进行数值逼近了。

timeStep = 5e-2
cartpoleIAM = crocoddyl.IntegratedActionModelEuler(cartpoleDAM, timeStep)

现在您可以再次运行 "IV. 编写问题，创建求解器 "中的所有模块，因为 cartpoleIAM 已被重新定义为直接使用解析导数。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/1641965.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！