# ocs2_wbc

**Repository Path**: pj_penuel/ocs2_wbc

## Basic Information

- **Project Name**: ocs2_wbc
- **Description**: 未来不远机械臂ocs2_wbc
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2024-03-18
- **Last Updated**: 2024-05-25

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# ocs2_wbc

此repo包含了四个文件夹：**ocs2_wbc,ocs2_wbc_model,ocs2_wbc_tutorials,zmebot_hardware**

ocs2_wbc 中包含了构建一个机器人优化问题所需要的基本工具。

ocs2_wbc_model 中包含了一些机器人模型。

ocs2_wbc_tutorials中包含了了一些针对ocs2_wbc的测试案例。

zmebot_hardware 中包含了第二代样机的与下位机通信，以及一些开放的topic，service接口。



运行环境推荐使用系统组搭建的orin系统，或者根据文件夹ocs2_wbc中提供的install.sh自行搭建环境，或者下载docker镜像： yaskawakuka/ros2_ocs2:v2.0。

## **ocs2_wbc**

本包通过开源项目：https://github.com/ARC-OPT/wbc修改得到。

**base**中包含了机器人问题涉及的基本数据结构，例如se3，twist，wrench等。

**constraints**中定义了机器人问题中涉及的各种约束，例如关节位置约束。

**controllers**中实现了各种控制器，例如关节pd控制器，笛卡尔pd控制器。

**core** 中定义了各种基类

**model** 中实现了利用pinocchio计算机器人正运动学、雅可比、海森、碰撞等

**scene**中实现了各种qp问题，可以在这里构建机器人wbc控制器

**solvers**中实现了利用各种求解器求解qp问题。

**tasks**中实现了各种任务，目前主要使用了笛卡尔空间速度任务

**tools**中实现了一些基本功能。

目前在第二代机器人中主要利用了scenes/VelocitySceneQPSP.cpp构建了末端跟踪控制器，该控制器求解的qp问题如以下形式：
$$
minimize & \| \mathbf{J}_w\dot{\mathbf{q}} - \mathbf{v}_d\|_2 -J_m\dot{\mathbf{q}} \\
             \mathbf{\dot{q}} & & \\
              & & \\
            s.t. 
                 & \dot{\mathbf{q}}_{m} \leq \dot{\mathbf{q}} \leq \dot{\mathbf{q}}_{M} & \\
                 & I\dot{\mathbf{q}}\leq\gamma \frac{h(q)}{\dot{h(q)}}
$$
其中Jw是机器人雅可比，jm是机器人可操作度雅可比，同时在约束中约束关节速度，并将关节位置约束也融合到关节速度约束中，还利用了控制障碍函数对关节速度进行约束，从而使得机器人可以避开障碍物。h(q)代表当前关节状态下距离最近的障碍物的距离值。

## ocs2_wbc_model

该文件夹中包含了使用ocs2_wbc时需要的机器人文件，主要包含urdf和srdf文件，如果要使用scenes/VelocitySceneQPSP.cpp，请保证urdf文件中碰撞体只含有球形碰撞体，并必须提供srdf文件。

## ocs2_tutorials

包含了一些简单的测试案例，测试了各种scenes。

## zmebot_hardware

第二代机器人使用的核心包，负责与机器人32控制器进行串口通信，并对上层开放了关节控制以及笛卡尔空间控制的接口，同时提供了各种service供外界调用。涉及串口通信可与威博沟通

**AtomicVariable** 是一个包含各种原子量的结构体，结构体如下定义：

```
    struct AtomicVariable

​    {

​        // true if get a response from slave arm ,set false after deal the response . 

​        //When send command to slave arm ,this should be false .

​        std::atomic<bool> recv_slave = false;

​        // trajectory following mode ,set the correct mode when trajectory following .

​        std::atomic<uint8_t> trajectory_following_mode = (uint8_t)motion_control_interfaces::srv::TrajectoryFollowing::Request::IDLE;

​        // in any trajectory following mode , when get a  command , this will become true .

​        // when trajectory following mode changed,this will become false .

​        std::atomic<bool> get_reference_configuration = false;

​        // flag wether the slave robot is connected .

​        std::atomic<bool> is_connect= false;

​        std::atomic<bool> uncontrollable= false;

​    };
```

SharedOperateData 是一个配备读写锁的共享数据结构，32通过串口传上来的数据以及ros topic传输的命令都会先存入该结构体中，并在程序需要的时候再从中取用。

TeleoperationJointControlData、AutonomyJointControlData、AutonomyCartesianControlData、TeleoperationCartesianControlData是外界可能给到的topic命令。

**zmebot_hardware**提供的**服务**有：

collision_checking_service_、drag_service_、joint_admittance_service_、open_close_joint_service_、restart32_service_、switch_joint_mode_service_、trajectory_following_service_、reset_gripper_service_

提供的**topic命令接口**有：

tele_joint_control_sub_、tele_cart_control_sub_、auto_cart_control_sub_、auto_joint_control_sub_

可以提供关节遥操，笛卡尔遥操，关节自主、笛卡尔自主控制。

同时zmebot_hardware还会**向外发布**机械臂当前的状态：

slave_arm_publisher_



slave_arm_node.cpp中，get_serial_data 负责从串口进行数据解析，具体咨询威博。

tele_joint_generate_cmd，负责生成关节遥操命令，当当前模式为关节遥操时，该函数生成关节位置命令。

auto_joint_generate_cmd 负责生成关节自主命令，当当前模式为关节自主时，该函数生成关节位置命令。

tele_cart_generate_cmd，负责生成笛卡尔遥操命令，当当前模式为笛卡尔遥操时，该函数利用ocs_wbc中的末端跟踪算法生成关节位置命令。

auto_cart_generate_cmd类似。