项目描述
该项目的目的是利用 ROS2 框架展示实时功能。本项目基于开放机器人组织之前的工作。本项目以这些软件包为基础。
这就是使用倒立摆进行实时演示的动机：
实时计算通常解决的控制问题的一个典型例子是平衡倒立摆。如果控制器意外地阻塞很长时间，摆就会倒下或变得不稳定。但是，如果控制器可靠地以比控制摆锤的电机运行速度更快的速度更新，摆锤就会成功地适应传感器数据，从而达到平衡摆锤的目的。
有关项目设计演示的详细介绍，请点击此处： 设计文章
一、
Cart-Pole 示例展示了如何用 Python 编写 Gazebo 系统。它实现了一个 LQR 控制器来平衡小车杆，使其始终处于垂直状态。
插件订阅 cart_pole/reset_angle 主题，该主题可用于反复重置杆角，并让控制器平衡车杆。
gz topic -t "cart_pole/reset_angle" -m gz.msgs.Double -p "data: 0.3"
注意:大角度会导致控制器失效，在这种情况下，模拟必须重置。
二、
这个演示还表明，通过将 LQR 控制器实现放在一个单独的模块中，插件可以提供一种在模拟器运行时热重新加载控制器的机制。
例如，这可以用于调整参数或快速迭代控制器设计。
为此，修改 lqr_controller.py 中的控制器增益 R 和 Q ，并通过发布到 cart_pole/reload_controller 主题来重新加载控制器
gz topic -t "cart_pole/reload_controller" -m gz.msgs.Empty -p ""
还可以通过订阅 cart_pole/state/position 和 cart_pole/state/velocity 来监控被控对象的状态。
这些主题各自发布一个 Vector2d ，其中 x 对应于小车的状态，y 对应于摆杆的状态。
三、仿真运行
需要设置插件的路径
cd CartPole export GZ_SIM_RESOURCE_PATH=`pwd`/:$GZ_SIM_RESOURCE_PATH export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`/plugins:$GZ_SIM_SYSTEM_PLUGIN_PATH
gz sim -v 4 "harmonic_demo/harmonic.sdf"
四、代码阅读
lqr_controller.py
4.1 导入库
import numpy as np from scipy import linalg
4.2 定义 LQR 控制器类
lqr 类有两个方法，一个初始化方法，一个计算控制输入。
class LqrController(object): def __init__(self, mass_cart, mass_point_mass, pole_length): def compute(self, x):
初始化函数定义了倒立摆的状态空间方程，求解 ricatti 方程及计算最优控制器增益；
def __init__(self, mass_cart, mass_point_mass, pole_length): a = 9.81 / (pole_length * 4.0 / 3 - mass_point_mass / (mass_point_mass + mass_cart)) A = np.array([[0, 1, 0, 0], [0, 0, a, 0], [0, 0, 0, 1], [0, 0, a, 0]]) b = -1 / (pole_length * (4.0 / 3 - mass_point_mass / (mass_point_mass + mass_cart))) B = np.array([[0], [1 / (mass_point_mass + mass_cart)], [0], [b]]) R = np.eye(1) Q = np.eye(4) Q[0, 0] = 10 Q[1, 1] = 10 # solve ricatti equation 计算 ricatti 方程 P = linalg.solve_continuous_are(A, B, Q, R) # calculate optimal controller gain 计算最优控制器增益 self.K = np.dot(np.linalg.inv(R), np.dot(B.T, P))
计算控制输入函数
def compute(self, x): # 计算控制输入 u = -np.dot(self.K, x) return u
lqr 类由 LQR 控制器公式。
cart_pole_system.py
4.3 导入库
from gz.sim8 import Model, Link, Joint from gz.transport13 import Node from gz.msgs10.double_pb2 import Double from gz.msgs10.vector2d_pb2 import Vector2d from gz.msgs10.empty_pb2 import Empty import sdformat14 as sdformat import numpy as np from threading import Lock import importlib from . import lqr_controller
4.4 定义插件类
倒立摆模型插件类有 6 个方法
class CartPoleSystem(object): def configure(self, entity, sdf, ecm, event_mgr): def init_controller(self): def pre_update(self, info, ecm): def reset_angle_cb(self, msg): def reload_controller_cb(self, msg): def publish_state(self, x):
configure() 方法主要完成实例化Model,Joint,Link类，检查Joint,Link位置，速度，获取.sdf文件中倒立摆各参数的值，发布倒立摆位置，速度等话题。
def configure(self, entity, sdf, ecm, event_mgr): self.model = Model(entity) self.cart_link = Link(self.model.link_by_name(ecm, "cart")) self.point_mass_link = Link(self.model.link_by_name(ecm, "point_mass")) self.cart_joint = Joint(self.model.joint_by_name(ecm, "cart_joint")) self.pole_joint = Joint(self.model.joint_by_name(ecm, "pole_joint")) initial_angle = sdf.get_double("initial_angle", 0)[0] assert self.cart_joint.valid(ecm) assert self.pole_joint.valid(ecm) self.cart_joint.enable_position_check(ecm) self.pole_joint.enable_position_check(ecm) self.cart_joint.enable_velocity_check(ecm) self.pole_joint.enable_velocity_check(ecm) self.pole_joint.reset_position(ecm, [initial_angle]) self.init_controller() self.node = Node() reset_angle_topic = sdf.get_string("reset_angle_topic", "reset_angle")[0] print("Subscribing to", reset_angle_topic) self.node.subscribe(Double, reset_angle_topic, self.reset_angle_cb) self.new_reset_angle = None self.reset_angle_lock = Lock() reload_controller_topic = sdf.get_string("reload_controller_topic", "reload_controller")[0] print("Subscribing to", reload_controller_topic) self.node.subscribe(Empty, reload_controller_topic, self.reload_controller_cb) self.controller_lock = Lock() state_topic = sdf.get_string("state_topic", "state")[0] position_topic = state_topic + "/position" velocity_topic = state_topic + "/velocity" print(f"Advertising to {position_topic} and {velocity_topic}") self.position_pub = self.node.advertise(position_topic, Vector2d) self.velocity_pub = self.node.advertise(velocity_topic, Vector2d)
init_controller() 方法主要完成小车、摆杆质量，摆杆长度赋值，初始化控制器
def init_controller(self): # TODO Get these from the model # TODO (azeey) Add API in sim::Link to get the mass of the link cart_mass = 0.25 point_mass = 0.03 pole_length = 0.4 self.controller = lqr_controller.LqrController(cart_mass, point_mass, pole_length)
pre_update() 方法主要完成发布小车位置速度话题，给小车施加作用力
def pre_update(self, info, ecm): if info.paused: return if len(self.cart_joint.position(ecm)) == 0: return with self.reset_angle_lock: if self.new_reset_angle is not None: self.pole_joint.reset_position(ecm, [self.new_reset_angle]) self.new_reset_angle = None x = np.array([ self.cart_joint.position(ecm)[0], self.cart_joint.velocity(ecm)[0], self.pole_joint.position(ecm)[0], self.pole_joint.velocity(ecm)[0] ]) self.publish_state(x) # 发布位置、速度话题 with self.controller_lock: u = self.controller.compute(x) self.cart_joint.set_force(ecm, [u]) #在这个关节上施加力。
reset_angle_cb() 方法接收重置角度
def reset_angle_cb(self, msg): with self.reset_angle_lock: self.new_reset_angle = msg.data print("Resetting angle to", self.new_reset_angle)
reload_controller_cb() 方法重新加载控制器
def reload_controller_cb(self, msg): with self.controller_lock: print("Reloading controller") importlib.reload(lqr_controller) self.init_controller()
publish_state() 方法发布位置速度话题
def publish_state(self, x): position_msg = Vector2d() position_msg.x = x[0] position_msg.y = x[2] velocity_msg = Vector2d() velocity_msg.x = x[1] velocity_msg.y = x[3] self.position_pub.publish(position_msg) self.velocity_pub.publish(velocity_msg)
类外定义了 get_system() 方法
def get_system(): return CartPoleSystem()
4.5 model.sdf
<?xml version="1.0" encoding="UTF-8"?> <sdf version="1.11"> <model name="CartPole"> <joint name="fix_to_world" type="fixed"> <parent>world</parent> <child>base</child> </joint> <link name="base"> <visual name="shaft"> <pose degrees="true">0 0 0 0 90 0</pose> <geometry> <cylinder> <length>1.70</length> <radius>0.01</radius> </cylinder> </geometry> <material> <diffuse>0.0 1.0 1.0 0.4</diffuse> <specular>1.0 1.0 1.0 0.4</specular> </material> </visual> <visual name="lower_limit"> <pose>-0.870 0 0 0 0 0</pose> <geometry> <box> <size>0.05 0.05 0.15</size> </box> </geometry> <material> <diffuse>0.2 0.1 0.1</diffuse> <specular>0.2 0.2 0.2</specular> </material> </visual> <visual name="upper_limit"> <pose>0.870 0 0 0 0 0</pose> <geometry> <box> <size>0.05 0.05 0.15</size> </box> </geometry> <material> <diffuse>0.2 0.1 0.1</diffuse> <specular>0.2 0.2 0.2</specular> </material> </visual> </link> <joint name="cart_joint" type="prismatic"> <parent>base</parent> <child>cart</child> <axis> <xyz>1 0 0</xyz> <limit> <lower>-0.80</lower> <upper>0.80</upper> <effort>100</effort> </limit> </axis> </joint> <link name="cart"> <inertial auto="true"/> <visual name="visual"> <geometry> <box> <size>0.1 0.05 0.05</size> </box> </geometry> <material> <diffuse>0.3 0.1 0.1</diffuse> <specular>0.1 0.1 0.1</specular> </material> </visual> <collision name="collision"> <density>1000</density> <geometry> <box> <size>0.1 0.05 0.05</size> </box> </geometry> </collision> </link> <joint name="pole_joint" type="revolute"> <pose relative_to="cart"/> <parent>cart</parent> <child>pole</child> <axis> <xyz>0 1 0</xyz> </axis> </joint> <link name="pole"> <pose>0 0.040 0.2 0 0 0</pose> <inertial auto="true"/> <visual name="visual"> <geometry> <cylinder> <length>0.4</length> <radius>0.01</radius> </cylinder> </geometry> <material> <diffuse>0.3 0.3 0.3</diffuse> </material> </visual> <collision name="collision"> <density>1</density> <geometry> <cylinder> <length>0.4</length> <radius>0.01</radius> </cylinder> </geometry> </collision> </link> <joint name="pole_point_mass_joint" type="fixed"> <parent>pole</parent> <child>point_mass</child> </joint> <link name="point_mass"> <pose relative_to="pole">0 0.0 0.2 0 0 0</pose> <inertial auto="true"/> <visual name="visual"> <geometry> <sphere> <radius>0.02</radius> </sphere> </geometry> <material> <diffuse>0.3 0.3 0.3</diffuse> </material> </visual> <collision name="collision"> <density>1000</density> <geometry> <sphere> <radius>0.02</radius> </sphere> </geometry> </collision> </link> <plugin filename="gz-sim-python-system-loader-system" name="gz::sim::systems::PythonSystemLoader"> <module_name>cart_pole_controller</module_name> <initial_angle>0.2</initial_angle> <reset_angle_topic>cart_pole/reset_angle</reset_angle_topic> <reload_controller_topic>cart_pole/reload_controller</reload_controller_topic> <state_topic>cart_pole/state</state_topic> </plugin> </model> </sdf>
