来自斯坦福的廉价机器狗.中

原创云深之无迹 2021-06-24 21:20

TI MCU方案：能源基础设施实时控制 如何增强电动汽车的实时控制能力？

因为上篇文章没有对shell脚本做完全的解析，比较遗憾，这篇补上。

我们继续来研究这个

它的功能是为某一个文件或目录在另外一个位置建立一个同步的链接，

类似Windows下的超级链接。

创建的链接有两种，一种被称为硬链接（Hard Link），另一种被称为符号链接（Symbolic Link）。建立硬链接时，链接文件和被链接文件必须位于同一个文件系统中，并且不能建立指向目录的硬链接，就是要指向一个单一的实体。而对符号链接，则不存在这个问题。默认情况下，ln产生硬链接。

　　在硬链接的情况下，参数中的“目标”被链接至[链接名]。如果[链接名]是一个目录名，系统将在该目录之下建立一个或多个与“目标”同名的链接文件，链接文件和被链接文件的内容完全相同。如果[链接名]为一个文件，用户将被告知该文件已存在且不进行链接。如果指定了多个“目标”参数，那么最后一个参数必须为目录。

　　如果给ln命令加上- s选项，则建立符号链接。如果[链接名]已经存在但不是目录，将不做链接。[链接名]可以是任何一个文件名（可包含路径），也可以是一个目录，并且允许它与“目标”不在同一个文件系统中。如果[链接名]是一个已经存在的目录，系统将在该目录下建立一个或多个与“目标”同名的文件，此新建的文件实际上是指向原“目标”的符号链接文件。

https://blog.csdn.net/will5451/article/details/51323999

下面是我们文中使用的例子

sudo ln -s 源文件 目标文件

sudo ln -s $(realpath .)/robot.service /etc/systemd/system/

$(realpath .)

realpath 用于获取指定目录或文件的绝对路径。

-e, --canonicalize-existing  文件 FILE 的所有组成部件必须都存在-m, --canonicalize-missing  文件 FILE 的组成部件可以不存在-L, --logical  在软链接之前解析父目录 ..-P, --physical  解析软链接，默认动作-q, --quiet  静默模式输出，禁止显示大多数错误消息--relative-to=DIR  相对于目录 DIR 的路径--relative-base=DIR  如果文件在基目录 DIR下，打印结果会省去基目录，否则打印绝对路径-s, --strip, --no-symlinks  不扩展软链接-z, --zero  不分隔输出，即所有的输出均在一行而不是单独每行--help  显示帮助信息--version  显示版本信息

用法参数

https://man7.org/linux/man-pages/man1/realpath.1.html

参考资料

在我的WSL上面运行的结果

可以自己去试一下，这里是集成在shell脚本里面了，

sudo systemctl daemon-reloadsudo systemctl enable robotsudo systemctl start robot

接下来看systemctl这个工具

它是什么？

systemd是Linux系统最新的初始化系统(init),作用是提高系统的启动速度，尽可能启动较少的进程，尽可能更多进程并发启动。

根据 Linux 惯例，字母d是守护进程（daemon）的缩写。Systemd 这个名字的含义，就是它要守护整个系统。

使用了 Systemd，就不需要再用init了。Systemd 取代了initd，成为系统的第一个进程（PID 等于 1），其他进程都是它的子进程。

systemd 245 (245.4-4ubuntu3.4)+PAM +AUDIT +SELINUX +IMA +APPARMOR SMACK +SYSVINIT +UTMP +LIBCRYPTSETUP +GCRYPT +GNUTLS +ACL +XZ +LZ4 +SECCOMP +BLKID +ELFUTILS +KMOD +IDN2 -IDN +PCRE2 default-hierarchy=hybrid

这个是我机器上面的版本

暂且放一个构架图

# 重启系统$ sudo systemctl reboot
# 关闭系统，切断电源$ sudo systemctl poweroff
# CPU停止工作$ sudo systemctl halt
# 暂停系统$ sudo systemctl suspend
# 让系统进入冬眠状态$ sudo systemctl hibernate
# 让系统进入交互式休眠状态$ sudo systemctl hybrid-sleep
# 启动进入救援状态（单用户状态）$ sudo systemctl rescue

# 查看启动耗时$ systemd-analyze                                                                                       
# 查看每个服务的启动耗时$ systemd-analyze blame
# 显示瀑布状的启动过程流$ systemd-analyze critical-chain
# 显示指定服务的启动流$ systemd-analyze critical-chain atd.service

# 显示当前主机的信息$ hostnamectl
# 设置主机名。$ sudo hostnamectl set-hostname rhel7

上面的三组命令是我常用的

还有我们之处脚本出现的

systemctl 提供了一组子命令来管理单个的 unit，其命令格式为：
systemctl [command] [unit]

具体使用的命令参数

command 主要有：
start：立刻启动后面接的 unit。
stop：立刻关闭后面接的 unit。
restart：立刻关闭后启动后面接的 unit，亦即执行 stop 再 start 的意思。
reload：不关闭 unit 的情况下，重新载入配置文件，让设置生效。
enable：设置下次开机时，后面接的 unit 会被启动。
disable：设置下次开机时，后面接的 unit 不会被启动。
status：目前后面接的这个 unit 的状态，会列出有没有正在执行、开机时是否启动等信息。
is-active：目前有没有正在运行中。
is-enable：开机时有没有默认要启用这个 unit。
kill ：不要被 kill 这个名字吓着了，它其实是向运行 unit 的进程发送信号。
show：列出 unit 的配置。
mask：注销 unit，注销后你就无法启动这个 unit 了。
unmask：取消对 unit 的注销。

start：立刻启动后面接的 unit。enable：设置下次开机时，后面接的 unit 会被启动。

sudo systemctl daemon-reload

这个daemon-reload有点不一样

新添加 unit 配置文件时需要执行 daemon-reload 子命令，有 unit 的配置文件发生变化时也需要执行 daemon-reload 子命令。daemon-reload 命令会做很多的事情，其中之一是重新生成依赖树(也就是 unit 之间的依赖关系)，所以当你修改了 unit 配置文件中的依赖关系后如果不执行 daemon-reload 命令是不会生效的。

我们来总结一下

首先cd到文件夹下面

建立一个同步变化的超链接，具体同步的是一个service。相当于注册服务

然后设置为下次开机，自己启动。启动后，后面的单元跟着启动

下面就是上面看见的这个service

[Unit] 部分Description : 服务的简单描述Documentation ：服务文档Before、After:定义启动顺序。Before=xxx.service,代表本服务在xxx.service启动之前启动。After=xxx.service,代表本服务在xxx.service之后启动。Requires：这个单元启动了，它需要的单元也会被启动；它需要的单元被停止了，这个单元也停止了。Wants：推荐使用。这个单元启动了，它需要的单元也会被启动；它需要的单元被停止了，对本单元没有影响。[Service] 部分Type=simple（默认值）：systemd认为该服务将立即启动。服务进程不会fork。如果该服务要启动其他服务，不要使用此类型启动，除非该服务是socket激活型。Type=forking：systemd认为当该服务进程fork，且父进程退出后服务启动成功。对于常规的守护进程（daemon），除非你确定此启动方式无法满足需求，使用此类型启动即可。使用此启动类型应同时指定 PIDFile=，以便systemd能够跟踪服务的主进程。Type=oneshot：这一选项适用于只执行一项任务、随后立即退出的服务。可能需要同时设置 RemainAfterExit=yes 使得 systemd 在服务进程退出之后仍然认为服务处于激活状态。Type=notify：与 Type=simple 相同，但约定服务会在就绪后向 systemd 发送一个信号。这一通知的实现由 libsystemd-daemon.so 提供。Type=dbus：若以此方式启动，当指定的 BusName 出现在DBus系统总线上时，systemd认为服务就绪。Type=idle: systemd会等待所有任务(Jobs)处理完成后，才开始执行idle类型的单元。除此之外，其他行为和Type=simple 类似。PIDFile：pid文件路径ExecStart：指定启动单元的命令或者脚本，ExecStartPre和ExecStartPost节指定在ExecStart之前或者之后用户自定义执行的脚本。Type=oneshot允许指定多个希望顺序执行的用户自定义命令。ExecReload：指定单元停止时执行的命令或者脚本。ExecStop：指定单元停止时执行的命令或者脚本。PrivateTmp：True表示给服务分配独立的临时空间Restart：这个选项如果被允许，服务重启的时候进程会退出，会通过systemctl命令执行清除并重启的操作。RemainAfterExit：如果设置这个选择为真，服务会被认为是在激活状态，即使所以的进程已经退出，默认的值为假，这个选项只有在Type=oneshot时需要被配置。[Install] 部分Alias：为单元提供一个空间分离的附加名字。RequiredBy：单元被允许运行需要的一系列依赖单元，RequiredBy列表从Require获得依赖信息。WantBy：单元被允许运行需要的弱依赖性单元，Wantby从Want列表获得依赖信息。Also：指出和单元一起安装或者被协助的单元。DefaultInstance：实例单元的限制，这个选项指定如果单元被允许运行默认的实例。

对应我们的脚本是这样的

在上面的service里面会打开这个手柄的服务，那这里我把服务也写过来

上面的手柄服务是调用了这个py文件

最外面的层级是调用这个py文件

接下来就是我们正式读实现的阶段啦！

整个过程都是向图中所示

PS4操纵杆接口负责从 UDP 套接字读取操纵杆输入并将它们转换为通用机器人command类型。

实现里面有一个单独的程序，joystick.py发布这些 UDP 消息，并负责通过蓝牙从 PS4 控制器读取输入。

控制器完成大部分工作，在状态（小跑、行走、休息等）之间切换并生成伺服位置目标。

控制器的详细模型如下所示。

这里就是单独的joystick的文件

控制器模型

这是代码的第三个组件，硬件接口，将来自控制器的位置目标转换为 PWM 占空比，然后传递给 Python 绑定到pigpiod，然后在软件中生成 PWM 信号并将这些信号发送到连接到控制器的电机树莓派。

步态调度器负责计划在任何给定时间哪些脚应该放在地面上（站姿），哪些脚应该向前移动到下一步（摆动）。例如，在小跑中，对角线对的腿同步移动并在站立和摆动之间轮流移动。步态调度器可以被认为是每条腿的导体，随着时间的推移在站立和摆动之间切换。

姿态控制器控制着地面的脚，其实很简单。它查看所需的机器人速度，然后为这些站立脚生成与所需速度相反方向的与身体相关的目标速度。它还包含转动，在这种情况下，它会在与所需的身体旋转相反的方向上相对于身体旋转脚。

挥杆控制器拿起刚完成站立阶段的脚，并将它们带到下一个触地位置。选择触地位置，使脚在摆动时向前移动与在站立时向后移动相同的距离。例如，如果在站立阶段，脚以 -0.4m/s 的速度向后移动（以达到 +0.4m/s 的身体速度）并且站立阶段为 0.5 秒长，那么我们知道脚将向后移动 -0.20米。然后摆动控制器将脚向前移动 0.20m，将脚放回起始位置。可以想象，如果摇摆控制器只将腿向前移动0.15m，那么每走一步，脚就会越来越落后于身体-0.05m。

站姿控制器和摆动控制器都以相对于身体重心的笛卡尔坐标生成脚的目标位置。使用笛卡尔坐标进行站姿和挥杆规划很方便，但我们现在需要将它们转换为运动角度。这是通过使用逆运动学模型完成的，该模型在笛卡尔身体坐标和运动角度之间进行映射。然后将这些电机角度（也称为关节角度）填充到state变量中并由模型返回。

这一部分的代码是校准使用

实现具体的运动过程的代码

import numpy as npimport timefrom src.IMU import IMUfrom src.Controller import Controllerfrom src.JoystickInterface import JoystickInterfacefrom src.State import Statefrom pupper.HardwareInterface import HardwareInterfacefrom pupper.Config import Configurationfrom pupper.Kinematics import four_legs_inverse_kinematicsdef main(use_imu=False):    """    主程序    """    # 创建配置    config = Configuration()    hardware_interface = HardwareInterface()    # 硬件接口
    # 创建IMU处理    if use_imu:        imu = IMU(port="/dev/ttyACM0")        # 是一个串口呀        imu.flush_buffer()
    # 创建控制器和用户输入处理    controller = Controller(        config,        four_legs_inverse_kinematics,    )    state = State()    print("创建joystick监听...")    joystick_interface = JoystickInterface(config)    print("完成.")
    last_loop = time.time()
    print("步态参数汇总:")    print("重叠 time: ", config.overlap_time)    print("摇摆 time: ", config.swing_time)    print("z 间隙: ", config.z_clearance)    print("x 转换: ", config.x_shift)
    # 等待激活按钮被摁下    while True:        print("等待L1激活机器人.")        while True:            command = joystick_interface.get_command(state)            joystick_interface.set_color(config.ps4_deactivated_color)            if command.activate_event == 1:                break            time.sleep(0.1)        print("机器人激活.")        joystick_interface.set_color(config.ps4_color)
        while True:            now = time.time()            if now - last_loop < config.dt:                continue            last_loop = time.time()
            # 解析UDP操纵杆命令，然后更新机器人控制器参数            command = joystick_interface.get_command(state)            if command.activate_event == 1:                print("停止机器人")                break
            # 读取IMU数据，如果没有可以使用的数据，则方向为无            quat_orientation = (                imu.read_orientation() if use_imu else np.array([1, 0, 0, 0])            )            state.quat_orientation = quat_orientation
            # 通过dt使控制器前进            controller.run(state, command)
            # 更新加入伺服系统的pwm宽度            hardware_interface.set_actuator_postions(state.joint_angles)main()

这里先放一点代码，接下来慢慢解剖

上面出现的这个Teensy，是NXP的一个开发板，性能比较好

这是它的一些介绍

通用的py库

接下来是源码库和狗的库，我一直以为是小学生的意思

假如我们就以库引入的顺序作为其功能对最终的机器人的贡献，那对机器人最重要的就是姿态的控制了。

姿态测量单元

只有三个函数而已

首先是使用用串口的接口，做了一些相应的配置

最后的函数是最重要的，读取传感器的4元数信息

self.last_quat = np.array([1, 0, 0, 0])

一个空的数据结构

每次从串口处读取一行，编码为utf。下面是一些异常处理

这是Woofer，12自由度的机器人

https://stanfordstudentrobotics.org/woofer

源码，将两个库放在了一起

pupper，廉价的4足机器人

我们只要读pupper

我还是把完整的流程写完再分析源码，再全部安装过后。在开机后需要校准机器人。校准是运行机器人之前的必要步骤，因为还没有精确测量伺服臂如何相对于伺服输出轴固定。运行校准脚本将提示你将 12 个自由度中的每一个与已知角度（例如水平或垂直）对齐，从而帮助你确定此旋转偏移。

先SSH：

rwsudo systemctl stop robot

然后运行：

cd StanfordQudrupedsudo pigpiodpython3 calibrate_servos.py

校准时的三种摆放方式

sudo systemctl start robot

完成后，重启机器

这些是校准的函数，以后读

https://github.com/stanfordroboticsclub/StanfordQuadruped/blob/master/calibrate_servos.py

这个文件是控制机器人动起来时的参数

里面时4个大类

class Configuration:    def __init__(self):        ################# 控制器集成颜色 ##############        self.ps4_color = PS4_COLOR        self.ps4_deactivated_color = PS4_DEACTIVATED_COLOR
        #################### 命令 ####################        self.max_x_velocity = 0.4        self.max_y_velocity = 0.3        self.max_yaw_rate = 2.0        self.max_pitch = 30.0 * np.pi / 180.0
        #################### 运动参数 ####################        self.z_time_constant = 0.02        self.z_speed = 0.03  # maximum speed [m/s]        self.pitch_deadband = 0.02        self.pitch_time_constant = 0.25        self.max_pitch_rate = 0.15        self.roll_speed = 0.16  # maximum roll rate [rad/s]        self.yaw_time_constant = 0.3        self.max_stance_yaw = 1.2        self.max_stance_yaw_rate = 2.0
        #################### 姿态 ####################        self.delta_x = 0.1        self.delta_y = 0.09        self.x_shift = 0.0        self.default_z_ref = -0.16
        #################### 摇摆 ######################        self.z_coeffs = None        self.z_clearance = 0.07        self.alpha = (            0.5  # 触地距离与总水平站姿运动的比率        )        self.beta = (            0.5  # 触地距离与总水平站姿运动的比率        )
        #################### 步态 #######################        self.dt = 0.01        self.num_phases = 4        self.contact_phases = np.array(            [[1, 1, 1, 0], [1, 0, 1, 1], [1, 0, 1, 1], [1, 1, 1, 0]]        )        self.overlap_time = (            0.10  # 四足着地阶段的持续时间        )        self.swing_time = (            0.15  # 两脚落地时的阶段持续时间        )
        ######################## 几何学 ######################        self.LEG_FB = 0.10  # 两脚落地时的阶段持续时间        self.LEG_LR = 0.04  # 中心线到腿部平面的左右距离        self.LEG_L2 = 0.115        self.LEG_L1 = 0.1235        self.ABDUCTION_OFFSET = 0.03  # 外展轴到腿的距离        self.FOOT_RADIUS = 0.01
        self.HIP_L = 0.0394        self.HIP_W = 0.0744        self.HIP_T = 0.0214        self.HIP_OFFSET = 0.0132
        self.L = 0.276        self.W = 0.100        self.T = 0.050
        self.LEG_ORIGINS = np.array(            [                [self.LEG_FB, self.LEG_FB, -self.LEG_FB, -self.LEG_FB],                [-self.LEG_LR, self.LEG_LR, -self.LEG_LR, self.LEG_LR],                [0, 0, 0, 0],            ]        )
        self.ABDUCTION_OFFSETS = np.array(            [                -self.ABDUCTION_OFFSET,                self.ABDUCTION_OFFSET,                -self.ABDUCTION_OFFSET,                self.ABDUCTION_OFFSET,            ]        )
        ################### 惯性 ####################        self.FRAME_MASS = 0.560  # kg        self.MODULE_MASS = 0.080  # kg        self.LEG_MASS = 0.030  # kg        self.MASS = self.FRAME_MASS + (self.MODULE_MASS + self.LEG_MASS) * 4
        # Compensation factor of 3 because the inertia measurement was just        # of the carbon fiber and plastic parts of the frame and did not        # include the hip servos and electronics        self.FRAME_INERTIA = tuple(            map(lambda x: 3.0 * x, (1.844e-4, 1.254e-3, 1.337e-3))        )        self.MODULE_INERTIA = (3.698e-5, 7.127e-6, 4.075e-5)
        leg_z = 1e-6        leg_mass = 0.010        leg_x = 1 / 12 * self.LEG_L1 ** 2 * leg_mass        leg_y = leg_x        self.LEG_INERTIA = (leg_x, leg_y, leg_z)
    @property    def default_stance(self):        return np.array(            [                [                    self.delta_x + self.x_shift,                    self.delta_x + self.x_shift,                    -self.delta_x + self.x_shift,                    -self.delta_x + self.x_shift,                ],                [-self.delta_y, self.delta_y, -self.delta_y, self.delta_y],                [0, 0, 0, 0],            ]        )