关于studywolf_control动态运动原语

链接：

Dynamic movement primitive | studywolf

GitHub - studywolf/control: A repository for control benchmarking code

GitHub - studywolf/pydmps

以下内容讲解这个指令下的逻辑：python3 run.py arm3 dmp write

(一) run.py

1、导入模块

2、构建任务与控制器

3、执行与画图

（二）write.py

Task函数

1、检查控制器类型

2、根据arm类型设置不同的参数

3、生成要跟随的路径

4、实例化control_shell

（a）controller创建操作空间控制器实例

（b）control_pars参数

5、生成runner_pars参数

（三）dmp.py

Shell类（dmp.py）

1、初始化

2、control函数（trajectoy.py）

3、check_pen_up函数

4、gen_path函数（传入trajectory参数，DMP学习阶段）

5、set_next_seq函数

6、set_target()函数（DMP运用阶段）

（四）sim_and_plot.py

Runner类

1、定义参数

2、run函数

3、anim_animate更新函数

（五）osc.py

Control类（osc.py）

1、control函数

2、gen_target函数

(一) run.py

1、导入模块

通过importlib.import_module()函数将arms.three_link.arm...模块、controllers.dmps模块以及task.write模块导入其中（之所以是模块，必须有__init__.py文件，尽管为空）

arm_module=arms.three_link.arm...模块

controller_class=controllers.dmps模块

task_module=task.write模块

2、构建任务与控制器

主要依据arm，controller_class与要跟踪的序列sequence，调用task_module.Task()，本质是调用write.py中的Task函数，主要完成路径生成和控制器的创建与初始化，见下面（二）的介绍

control_shell, runner_pars = task(
    arm, controller_class,
    sequence=args['--sequence'], scale=args['--scale'],
    force=float(args['--force']) if args['--force'] is not None else None,
    write_to_file=bool(args['--write_to_file']))

3、执行与画图

调用Runner.run()来依据control_shell类（已根据任务-主要是trajector，完成参数设置和初始化）进行执行和画图

//实例化
runner = Runner(dt=dt, **runner_pars)
//执行
runner.run(arm=arm, control_shell=control_shell,
           end_time=(float(args['--end_time'])
                     if args['--end_time'] is not None else None))
//画图
runner.show()

（二）write.py

Task函数

参数：arm机械臂, controller_class控制器（只能是dmp或track）, sequence=None要跟踪的序列, scale=None缩放,force=None外力, write_to_file=False写入文件, **kwargs

返回：control_shell, runner_pars

头文件：

import controllers.osc as osc # 操作空间控制器（Operational Space Controller, OSC）
import controllers.forcefield as forcefield  #力场控制器（Forcefield Controller）
import tasks.write_data.read_path as rp # 导入用于生成路径的函数。

1、检查控制器类型

只能是'dmp'或'trace'，否则报错

2、根据arm类型设置不同的参数

PD控制器的位置误差增益kp、阈值、writebox书写范围（和机械臂的可达空间有关）

3、生成要跟随的路径

设定sequence与scale的default值；生成要跟随的路径：

trajectory = rp.get_sequence(sequence, writebox, spaces=True)
# 其中rp来源于“import tasks.write_data.read_path as rp”

# sequence 要跟随的序列
# writebox 是一个列表，定义了数字序列将要被绘制的矩形区域的最小和最大 x、y 坐标
# spaces 控制数字在给定 writebox（写框）中的布局方式，特别是数字之间的间距，如果为False是紧凑型，如果是True是宽松型

在get_sequence的内部

1、num = get_raw_data(nn, num_writebox)调用了路径中预先存好的字符路径（012345678hello）：studywolf/control/studywolf_control/tasks/write_data/'+input_name+'.txt'

之后想用自己的路径，可以从此处修改

2、num = get_raw_data(nn, num_writebox)中将字符路径数据计算调整到num_writebox区域内部，成为机械臂可达空间上的序列。

3、将num堆叠在nums上,同时在num和nums之间以及nums的末尾保留一行nans作为间隔。

4、返回nums，即是trajectory

4、实例化control_shell

control_shell = controller_class.Shell(controller=controller, **control_pars)

这个controller_class就是controllers.dmps模块，所以就是实例化dmp.py中的Shell类，这个类的继承关系为：dmp.py(Shell)<-trajectory.py(Shell)<-shell.py(Shell)

关于Shell中传入的两个参数：

（a）controller创建操作空间控制器实例

controller = osc.Control(kp=kp, kv=np.sqrt(kp), write_to_file=write_to_file)

继承关系osc.py(Control类)<-control.py(Control类)

shell.py父类Shell中control方法也用到了Control类中的control函数

（b）control_pars参数

（也就是Shell类需要的参数们）

additions——附加到输出控制信号的加法类列表
gain——轨迹跟踪的PD增益
pen_down——当末端在画画时为true
threshold——系统必须多接近初始目标
trajectory——np.array要遵循的点的时间序列。[DOF，时间]。none表示笔抬起
add_to_goals——用于在目标位置基础上进行空间偏移调整,便于轨迹放缩或变换
bfs——DMP中的基函数数量,影响轨迹精度和灵活性。
tau——时间尺度放缩因子
未提及：pattern:指定 DMP 模式为“离散”或“周期性”.....(还有一些参数)

这里只是实例化dmp.Shell类，完成了初始化，dmp.py(Shell)中做了什么呢，见下面（三）的介绍

真正执行里面的函数是在sim_and_plot.py(Runner)模块中执行的。

5、生成runner_pars参数

'infinite_trail': True,

'title': 'Task: Writing numbers',

'trajectory': trajectory

（三）dmp.py

上面提到Shell的继承关系：dmp.py(Shell)<-trajectory.py(Shell)<-shell.py(Shell)

作用：

⭐就是实例化Shell后（在write.py中实例化了），会执行gen_path（传入轨迹，分段学习）以及执行set_target（运用dmp进行step单步生成轨迹，这个轨迹保存到osc.Control类中target属性）

⭐在sim_and_plot中调用了control（arm）本质是调用的osc.Control类中的control()函数，用于控制机械臂运动

Shell类（dmp.py）

1、初始化

（a）------hell.Shell-------
#定义参数：
self.controller = controller  # 是osc.Control()类型的
self.pen_down = pen_down
self.kwargs = kwargs
#定义了control函数
（b）------trajectory.Shell-------
#定义参数:
self.done = False
self.gain = gain
self.not_at_start = True
self.num_seq = 0
self.tau = tau
self.threshold = threshold
self.x = None
#执行
self.gen_path(trajectory)
self.set_target()
#定义了control函数；声明了check_pen_up、gen_path、set_next_seq、set_target函数
（c）------dmp.Shell-------
#定义参数:
self.bfs = bfs
self.add_to_goals = add_to_goals
self.pattern = pattern
#如果提供了目标偏移量，则更新DMP目标位置.作用：允许动态调整目标位置以适应新的环境或任务需求
if add_to_goals is not None:
    for ii, dmp in enumerate(self.dmp_sets): 
        dmp.goal[0] += add_to_goals[ii*2]
        dmp.goal[1] += add_to_goals[ii*2+1] 
#定义了check_pen_up、gen_path、set_next_seq、set_target函数

add_to_goals就是用于设置目标值（偏差量）

2、control函数（trajectoy.py）

这个函数在sim_and_plot中调用了，作用是控制机械臂

保存当前末端执行器位置:使用 np.copy(arm.x) 将当前末端执行器的位置复制到 self.x 中。这确保了即使后续操作修改了 arm.x，原始值仍然被保留下来以供参考。

检查与目标的距离:调用 self.controller.check_distance(arm) 来计算末端执行器当前位置与目标位置之间的距离，并将其与阈值 (self.threshold) 进行比较。如果距离小于阈值，则将 self.not_at_start 设置为 False，表示机械臂已经到达目标位置附近。

判断是否应用控制信号:如果 self.not_at_start 或者 self.done 为 True，则直接调用控制器的 control 方法生成控制信号并返回。否则，进入下一步逻辑。

设置新目标:如果机械臂尚未到达起始位置（即 sef.not_at_start 和 self.done 都为 False），则调用 self.set_target() 方法来设置新的目标位置。

检查是否需要抬起笔:调用 self.check_pen_up() 方法来决定是否应该抬起笔。如果是最后一次动态运动基元 (DMP) 并且需要抬起笔，则结束程序（通过 sys.exit()）。如果不是最后一次 DMP，则更新序列编号 (self.num_seq)，准备下一个 DMP，并重新设置目标位置。

确定控制类型和计算期望位置:根据控制器类型（osc.Control 或 gc.Control），选择使用末端执行器位置 (arm.x) 或关节角度 (arm.q) 作为当前位置 (pos)。计算期望位置偏差 (pos_des)，这是基于目标位置 (self.controller.target) 和当前位置 (pos) 的差异，并乘以增益 (self.gain)。
生成并返回控制信号:最后，调用控制器的 control 方法，传入机械臂模型和期望位置偏差 (pos_des)，生成最终的控制信号 (self.u) 并返回。
pos_des = self.gain * (self.controller.target - pos)
self.u = self.controller.control(arm, pos_des)
这里本质调用的osc.Control类中的control()函数，参数arm, x_des

3、check_pen_up函数

基于 DMP 的相位系统 (canonical system) 判断时间是否已接近轨迹末端，如果是，则返回 True

4、gen_path函数（传入trajectory参数，DMP学习阶段）

根据输入轨迹生成 DMP 模型，每段轨迹可能包含多个子轨迹（如绘图中的笔划分段）

输入：trajectory参数

返回：每段轨迹的DMP学习结果

判断输入轨迹维度,确保统一格式。
找出trajectory轨迹中的中断点(NaN 或 None 标记的断点)
根据分段轨迹使用多个 DMP 模型。
对每段轨迹建立一个DMP模型，并把这段的轨迹（seq从trajectory切片出来的）用于DMP学习，最后把学习后的结果添加到dmp_sets序列中。
dmps = DMP_discrete.DMPs_discrete(n_dmps=num_DOF, n_bfs=self.bfs)
dmps.imitate_path(y_des=seq)
self.dmp_sets.append(dmps)

5、set_next_seq函数

切换到下一段轨迹（多段轨迹的控制）

self.dmps = self.dmp_sets[self.num_seq]

6、set_target()函数（DMP运用阶段）

   def set_target(self):
        """Get the next target in the sequence.动态调整目标，适应环境变化
        """
        error = 0.0
        if self.controller.target is not None:
            error = np.sqrt(np.sum((self.x - self.controller.target)**2)) * 1000
        self.controller.target, _, _ = self.dmps.step(tau=self.tau, error=error)

Shell类中有一个controller属性，在write.py实例化control_shell的时候给到了osc.Control类型，因此set_target()函数这里是为了设置osc.Control类的target属性，是由dmps单步计算出来的。

关于osc.Control类的介绍见下面（五）

（四）sim_and_plot.py

用的时候这么用的

runner = Runner(dt=dt, **runner_pars)
runner.run(arm=arm, control_shell=control_shell,
           end_time=(float(args['--end_time'])
                     if args['--end_time'] is not None else None))
runner.show()

Runner类

用于模拟和可视化机械臂的运动

1、定义参数

title: 设置绘图窗口的标题。
dt: 模拟的时间步长，默认为 1×10−31×10−3 秒。
control_steps: 控制信号更新的频率，默认每一步更新一次。
display_steps: 显示更新的频率，默认每一步更新一次。
t_target: 目标位置更新的时间间隔，默认为1秒。
control_type: 控制类型的标识符，例如 'random' 或其他自定义类型。
trajectory: 预定义的轨迹数据，默认为 None。
infinite_trail: 是否保持无限轨迹，默认为 False。
mouse_control: 是否启用鼠标控制，默认为 False。

2、run函数

设置绘图框:根据机械臂的自由度 (DOF) 设置绘图框的边界 (box)。

创建图形窗口:使用 Matplotlib 创建一个图形窗口，并设置标题和网格线。添加子图并设置坐标轴范围，确保绘图区域是正方形。

设置绘图元素:初始化轨迹、机械臂线条、目标线条和信息文本。

绘制轨迹：如果提供了轨迹数据，则在图中绘制出来作为参考。

连接鼠标事件:如果启用了鼠标控制，则将 move_target 函数绑定到鼠标移动事件上，允许用户通过鼠标动态设置目标位置。

创建动画:使用 FuncAnimation 创建动画，其中 anim_animate 是每一帧的更新函数，而 anim_init 用于初始化动画状态。.
anim = animation.FuncAnimation(fig, self.anim_animate,init_func=self.anim_init, frames=5000, interval=0, blit=True)

除了run还有这些函数：

make_info_text: 构建包含当前仿真时间和关节角度 (q) 及控制信号 (u) 的文本信息。
anim_init: 初始化动画状态，清空所有绘图元素的数据。
anim_animate: 更新每一帧的动画内容，包括机械臂位置、目标位置、信息文本和轨迹。（下面重点讲）
show: 展示图形窗口。

3、anim_animate更新函数

（1）检查结束条件:如果设置了结束时间 (end_time)，并且当前仿真时间超过了该时间，则停止动画并关闭图形窗口。

（2）更新目标位置:根据 control_type 更新目标位置。如果控制类型为 'random'，则在每个目标更新时间间隔后生成新的随机目标；否则使用控制器中的目标。
        if self.control_type == 'random':
            # update target after specified period of time passes
            if self.sim_step % (self.target_steps*self.display_steps) == 0:
                self.target = self.shell.controller.gen_target(self.arm)
        else:
            self.target = self.shell.controller.target
其中的self.shell.controller是osc.Control()类型的

（3）应用控制信号:在每个显示步骤内，根据控制步骤更新控制信号 (tau) 并应用到机械臂上。

（4）更新绘图元素:更新机械臂线条、信息文本、轨迹和目标线条以反映最新的状态。

（5）更新手部轨迹:如果笔处于放下状态 (pen_down)，则更新轨迹数据；如果笔抬起，则在轨迹数据中插入断点。

（五）osc.py

上面提到继承关系osc.py(Control类)<-control.py(Control类)

Control类（osc.py）

1、初始化

这里有个参数target可以修改目标值？

有个参数DOF可以修改任务空间维度

（1）-----control.py(Control)------
# 定义参数
        self.u = np.zeros((2,1)) # control signal
        self.additions = additions ########
        self.kp = kp # 位置误差增益
        self.kv = kv # 速度误差增益
        self.task = task
        self.target = None  ###########目标值！#######
        self.write_to_file = write_to_file
        self.recorders = []
# 定义函数
    def check_distance(self, arm):
        """Checks the distance to target"""
        return np.sum(abs(arm.x - self.target)) + np.sum(abs(arm.dq))
# 声明函数
        control(self)

（2）-----osc.py(Control)-----
# 定义参数
        self.DOF = 2 # task space dimensionality任务空间维度
        self.null_control = null_control
        if self.write_to_file is True:
            from recorder import Recorder
            # set up recorders
            self.u_recorder = Recorder('control signal', self.task, 'osc')
            self.xy_recorder = Recorder('end-effector position', self.task, 'osc')
            self.dist_recorder = Recorder('distance from target', self.task, 'osc')
            self.recorders = [self.u_recorder,
                            self.xy_recorder,
                            self.dist_recorder]
# 定义函数
    def control(self, arm, x_des=None)
    def gen_target(self, arm)

1、control函数

参数：机械臂模型 (arm) ；可选的期望末端执行器位置 (x_des)

返回：控制信号 (self.u)，该信号将被应用于机械臂以实现所需的运动。

计算期望的末端执行器加速度:如果 x_des 是 None，那么使用当前机械臂末端执行器的位置 (arm.x) 和目标位置 (self.target) 来计算期望的末端执行器加速度。使用比例增益 (self.kp) 计算误差并放大，得到期望的末端执行器加速度。
生成任务空间中的质量矩阵:使用机械臂模型的方法 (gen_Mq 和 gen_Mx) 分别生成关节空间和任务空间的质量矩阵 (Mq 和 Mx)。
计算作用力 (Fx):根据任务空间的质量矩阵 (Mx) 和期望的末端执行器加速度 (x_des) 计算作用力 (Fx)。
计算雅可比矩阵 (JEE):雅可比矩阵描述了关节空间速度与末端执行器速度之间的关系。
计算控制信号 (self.u):控制信号是通过将雅可比矩阵转置乘以任务空间的作用力 (Fx) 并减去一个基于关节速度补偿项来获得的。这里假设重力补偿已经包含在机械臂模型中，因此不需要额外添加。
处理空控件 (Null Space Control):如果启用了空控件 (self.null_control) 并且任务空间的自由度 (DOF) 少于机械臂的自由度，则计算一个附加的控制信号 (u_null)，该信号试图将机械臂移动到其休息位置。计算空控件滤波器 (null_filter) 并应用到附加控制信号上，确保它只影响机械臂的空控件。
记录数据:如果设置了写入文件 (self.write_to_file)，则记录控制信号、末端执行器位置以及目标位置与当前位置之间的距离。
添加额外信号:如果有其他需要添加的信号（例如来自外部模块），它们也会被加入到最终的控制信号中。

2、gen_target函数

用于需要动态或随机目标的应用场景

    def gen_target(self, arm):
        """Generate a random target"""
        gain = np.sum(arm.L) * .75
        bias = -np.sum(arm.L) * 0
        self.target = np.random.random(size=(2,)) * gain + bias
        return self.target.tolist()