├── .gitignore ├── README.md ├── img ├── 1.png └── 2.png └── src ├── CMakeLists.txt ├── Piper_ros ├── LICENSE ├── Q&A.MD ├── README(EN).md ├── README.MD ├── asserts │ └── pictures │ │ ├── piper_gazebo.png │ │ ├── piper_gazebo_90.png │ │ ├── piper_rviz.png │ │ ├── piper_urdf_no_gripper_zero.png │ │ ├── piper_urdf_zero.png │ │ ├── piper_zero.png │ │ └── tostopic_list.jpg ├── can_activate.sh ├── can_config.sh ├── find_all_can_port.sh └── src │ ├── piper │ ├── CMakeLists.txt │ ├── launch │ │ ├── start_double_piper.launch │ │ ├── start_single_piper.launch │ │ └── start_single_piper_rviz.launch │ ├── package.xml │ └── scripts │ │ ├── piper_control.py │ │ ├── piper_ctrl_single_node.py │ │ ├── piper_pinocchio.py │ │ └── piper_start_ms_node.py │ ├── piper_description │ ├── CMakeLists.txt │ ├── config │ │ ├── joint_names_agx_arm_description.yaml │ │ ├── piper_gazebo_control.yaml │ │ └── piper_no_gripper_gazebo_control.yaml │ ├── launch │ │ ├── piper_no_gripper │ │ │ ├── display_no_gripper_urdf.launch │ │ │ ├── display_no_gripper_xacro.launch │ │ │ ├── gazebo_no_gripper_xacro.launch │ │ │ ├── piper_no_gripper_gazebo_controller.launch │ │ │ └── piper_no_gripper_world.launch │ │ └── piper_with_gripper │ │ │ ├── display_urdf.launch │ │ │ ├── display_xacro.launch │ │ │ ├── gazebo_xacro.launch │ │ │ ├── piper_gazebo_controller.launch │ │ │ ├── piper_world.launch │ │ │ └── rviz_ctrl_gazebo.launch │ ├── meshes │ │ ├── base_link.STL │ │ ├── gripper_base.STL │ │ ├── link1.STL │ │ ├── link2.STL │ │ ├── link3.STL │ │ ├── link4.STL │ │ ├── link5.STL │ │ ├── link6.STL │ │ ├── link7.STL │ │ └── link8.STL │ ├── package.xml │ ├── rviz │ │ ├── piper_ctrl.rviz │ │ └── piper_no_gripper.rviz │ ├── scripts │ │ └── rviz_ctrl_gazebo.py │ ├── urdf │ │ ├── piper_description.csv │ │ ├── piper_description.urdf │ │ ├── piper_description.xacro │ │ ├── piper_no_gripper_description.csv │ │ ├── piper_no_gripper_description.urdf │ │ └── piper_no_gripper_description.xacro │ └── worlds │ │ └── empty.world │ └── piper_msgs │ ├── CMakeLists.txt │ ├── msg │ ├── PiperStatusMsg.msg │ └── PosCmd.msg │ ├── package.xml │ └── srv │ ├── Enable.srv │ ├── GoZero.srv │ └── Gripper.srv └── oculus_reader ├── APK ├── alvr_client_android.apk └── teleop-debug.apk ├── CMakeLists.txt ├── config └── oculus_reader.rviz ├── launch ├── teleop_double_piper.launch └── teleop_single_piper.launch ├── package.xml └── scripts ├── FPS_counter.py ├── buttons_parser.py ├── install.py ├── oculus_reader.py ├── piper_control.py ├── teleop_double_piper.py ├── teleop_single_piper.py └── tools.py /.gitignore: -------------------------------------------------------------------------------- 1 | /build 2 | /devel 3 | .catkin_workspace 4 | __pycache__ 5 | .vscode 6 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |

quest teleop piper

3 |

Agilex Robotics

4 |

5 | English | 中文 6 |

7 |
8 | 9 | 10 | 11 | 12 | ## 介绍 13 | 14 | 该仓库实现了使用 meta quest2/3 VR 套装对 piper 机械臂进行遥操作。 15 | 16 | ### 准备工作 17 | 18 | 1、安装依赖 19 | 20 | ```bash 21 | sudo apt install android-tools-adb 22 | 23 | conda create -n vt python=3.9 24 | 25 | conda activate vt 26 | 27 | conda install pinocchio==3.2.0 casadi==3.6.7 -c conda-forge 28 | 29 | pip install meshcat rospkg pyyaml pure-python-adb piper-sdk 30 | ``` 31 | 32 | 2、配置您的 quest 设备 33 | 34 | - quest必须支持开发者模式 35 | 36 | 如在 quest 设备上找不到开发者模式，请参考该链接配置 quest 设备。 37 | 38 | 点击：[oculus_reader](https://github.com/rail-berkeley/oculus_reader) 39 | 40 | - 在quest上安装`alvr_client_android.apk`和`teleop-debug.apk`文件，文件在代码里面的`oculus_reader/APK`目录下。 41 | 42 | - [步骤1] 到meta商店安装Mobile VR Station 应用(联网) 43 | 44 | - [步骤2] 将 quest 与 pc 相连，开启 USB调试，在 pc 上显示新设备接入后，把要上述 apk 文件下载并复制到 quest的 Dowanload 目录里面 45 | 46 | - [步骤3] 开启Mobile VR Station => Configuration Wizard => Show All Options => Configuration Scoped Storage => Step1: Request Access => 选择根目录Dowanload 里面刚刚步骤2放的apk 点击类似放大的按钮会弹出一个窗口，在弹出的窗口里点击安装 47 | 48 | - 如果上一步无法安装可以用如下链接中的方案，注意需要windows 49 | - 50 | - 其中的SideQuest app界面会更新，但是一定会有安装apk功能 51 | 52 | 3、将代码克隆下来并编译： 53 | 54 | ```bash 55 | git clone git@github.com:agilexrobotics/questVR_ws.git 56 | 57 | cd questVR_ws 58 | 59 | catkin_make 60 | ``` 61 | 62 | 我们在 Ubuntu 20.04 上测试了我们的代码，其他操作系统可能需要不同的配置。 63 | 64 | 有关更多信息，您可以参考 [開始使用 Meta Quest 2](https://www.meta.com/zh-tw/help/quest/articles/getting-started/getting-started-with-quest-2/?srsltid=AfmBOoqvDcwTtPt2P9o6y3qdXT_9zxz4m8yyej4uwLGEXVXv6KAr3QQz) 、[Piper_ros](https://github.com/agilexrobotics/Piper_ros)、[oculus_reader](https://github.com/rail-berkeley/oculus_reader)。 65 | 66 | 4、进入到 piper_description 功能包下，将 urdf 目录下的 piper_description.urdf 所有 STL 文件的用户名修改成你的用户名，例如： 67 | 68 | ```xml 69 | 70 | 72 | 73 | 74 | 修改成 75 | 76 | 77 | 79 | 80 | ``` 81 | 82 | ### 代码架构说明 83 | 84 | oculus_reader，该存储库提供了从 Quest 设备读取位置和按下按钮的工具。 85 | 86 | 以VR眼镜作为基站，将手柄的与基站的TF关系传输给机械臂。 87 | 88 | ```bash 89 | ├── APK #apk文件 90 | │   ├── alvr_client_android.apk 91 | │   └── teleop-debug.apk 92 | ├── CMakeLists.txt 93 | ├── config 94 | │   └── oculus_reader.rviz 95 | ├── launch #启动文件 96 | │   ├── teleop_double_piper.launch 97 | │   └── teleop_single_piper.launch 98 | ├── package.xml 99 | └── scripts 100 | ├── buttons_parser.py 101 | ├── FPS_counter.py 102 | ├── install.py 103 | ├── oculus_reader.py 104 | ├── piper_control.py #机械臂控制接口 105 | ├── teleop_double_piper.py #遥操作双臂代码 106 | ├── teleop_single_piper.py #遥操作单臂代码 107 | └── tools.py 108 | ``` 109 | 110 | ## 软件启动 111 | 112 | 1、机械臂使能 113 | 114 | **单piper使能**： 115 | 116 | 将机械臂的can线接入电脑 117 | 118 | 然后执行： 119 | 120 | ```bash 121 | cd ~/questVR_ws/src/Piper_ros 122 | 123 | bash can_activate.sh can0 1000000 124 | ``` 125 | 126 | **左右双piper使能**： 127 | 128 | 129 | 先将左机械臂的can线接入电脑 130 | 131 | 然后执行： 132 | 133 | ```bash 134 | cd ~/questVR_ws/src/Piper_ros 135 | 136 | bash find_all_can_port.sh 137 | ``` 138 | 139 | 终端会出现左机械臂的端口号，接着将右机械臂的can线接入电脑 140 | 141 | 再次执行： 142 | 143 | ```bash 144 | bash find_all_can_port.sh 145 | ``` 146 | 147 | 终端会出现左机械臂的端口号。 148 | 149 | 将这左右两个端口号复制到 can_config.sh 文件的 111 和 112 行，如下所示： 150 | 151 | ```bash 152 | # 预定义的 USB 端口、目标接口名称及其比特率（在多个 CAN 模块时使用） 153 | if [ "$EXPECTED_CAN_COUNT" -ne 1 ]; then 154 | declare -A USB_PORTS 155 | USB_PORTS["1-8.1:1.0"]="left_piper:1000000" #左机械臂 156 | USB_PORTS["1-8.2:1.0"]="right_piper:1000000" #右机械臂 157 | fi 158 | ``` 159 | 160 | 保存完毕后，激活左右机械臂使能脚本： 161 | 162 | ```bash 163 | cd ~/questVR_ws/src/Piper_ros 164 | 165 | bash can_config.sh 166 | ``` 167 | 168 | 169 | 2、启动遥操机械臂 170 | 171 | ```bash 172 | source /home/agilex/questVR_ws/devel/setup.bash 173 | 174 | conda activate vt 175 | 176 | roslaunch oculus_reader teleop_single_piper.launch # 单臂遥操 177 | 178 | or 179 | 180 | roslaunch oculus_reader teleop_double_piper.launch # 双臂遥操 181 | ``` 182 | 183 | 在启动遥操代码时出现该错误时： 184 | 185 | ```bash 186 | Device not found. Make sure that device is running and is connected over USB 187 | Run `adb devices` to verify that the device is visible. 188 | ``` 189 | 190 | 说明了VR头盔未开启调试模式，开启调试模式方法步骤如下： 191 | 192 | 1. 使用 USB-C 线将VR头盔连接到计算机，然后佩戴该设备。 193 | 194 | 2. 当在通知中出现“检测到USB”，点击一下该通知。 195 | 196 | ![img error](img/2.png) 197 | 198 | 3. 第一次开启程序，会出现上面的报错。 199 | 200 | 4. 当设备上出现提示时，接受**允许 USB 调试**和**始终允许从此计算机进行。** 201 | 202 | ![img error](img/1.png) 203 | 204 | 5. 关掉程序，再次运行。 205 | 206 | 207 | 208 | 209 | 210 | ## 操作说明 211 | 212 | > 注意⚠️： 213 | > 214 | > - 请一定要确保VR屏幕保持常亮，否则TF会乱飘导致遥操作机械臂乱飞，我们建议在VR眼镜里面拿东西遮住感应器，使其保持常亮状态。 215 | > - 开启程序后，请一定要确保手柄在VR视野里以及rviz里面的坐标稳定不会乱飘，然后按住按键“A”||“X”使机械臂复位，复位后才可进行遥操做，否则机械臂也会乱飞。 216 | > - 在遥操 piper 启动后，请注意观察网页端的机械臂是否乱飘， 217 | 218 | - 遥操单臂使用右手手柄，开始遥操前确保机械臂回到初始姿态，按住按键 “A” 能使机械臂回到初始位置，长按按键 “B” 为遥操机械臂，松开为停止控制。遥操双臂同理。 219 | 220 | - 为了操作的人身安全以及减少对机械臂的损害，在遥操结束后，请确保机械臂回到初始位置附件再按下“A”||“X”键复位。 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | -------------------------------------------------------------------------------- /img/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/img/1.png -------------------------------------------------------------------------------- /img/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/img/2.png -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/Piper_ros/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 RosenYin 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /src/Piper_ros/Q&A.MD: -------------------------------------------------------------------------------- 1 | # piper_ros Q&A 2 | 3 | 1. 录joint的包后播放包，机械臂运行卡顿，并且包播放结束，机械臂还在运动(就是回调函数依旧在处理)： 4 | 5 | 原因: 与电脑性能有关，性能差的电脑处理不过来，导致回调函数处理时间长 6 | 7 | 解决办法: 将回调函数如joint_callback函数中的打印以及log注释 8 | -------------------------------------------------------------------------------- /src/Piper_ros/README(EN).md: -------------------------------------------------------------------------------- 1 | # AgileX Robotic Arm 2 | 3 | [CN](README.MD) 4 | 5 | ![ubuntu](https://img.shields.io/badge/Ubuntu-20.04-orange.svg) 6 | 7 | |ROS |STATE| 8 | |---|---| 9 | |![ros](https://img.shields.io/badge/ROS-noetic-blue.svg)|![Pass](https://img.shields.io/badge/Pass-blue.svg)| 10 | 11 | ## Installation 12 | 13 | ### Install dependencies 14 | 15 | ```shell 16 | pip3 install python-can 17 | ``` 18 | 19 | ```shell 20 | pip3 install piper_sdk 21 | ``` 22 | 23 | ## Quick Start 24 | 25 | ### Enable CAN module 26 | 27 | First, you need to set up the shell script parameters. 28 | 29 | #### Single robotic arm 30 | 31 | ##### PC with Only One USB-to-CAN Module Inserted 32 | 33 | - ##### Use the `can_activate.sh` 34 | 35 | Directly run: 36 | 37 | ```bash 38 | bash can_activate.sh can0 1000000 39 | ``` 40 | 41 | ##### PC with Multiple USB-to-CAN Modules Inserted 42 | 43 | - ##### Use the `can_activate.sh` script 44 | 45 | Disconnect all CAN modules. 46 | 47 | Only connect the CAN module linked to the robotic arm to the PC, and then run the script. 48 | 49 | ```shell 50 | sudo ethtool -i can0 | grep bus 51 | ``` 52 | 53 | and record the `bus-info` value, for example, `1-2:1.0`. 54 | 55 | **Note: Generally, the first inserted CAN module defaults to `can0`. If the CAN interface is not found, use `bash find_all_can_port.sh` to check the CAN names corresponding to the USB addresses.** 56 | 57 | Assuming the recorded `bus-info` value from the above operation is `1-2:1.0`. 58 | 59 | Then execute the command to check if the CAN device has been successfully activated. 60 | 61 | ```bash 62 | bash can_activate.sh can_piper 1000000 "1-2:1.0" 63 | ``` 64 | 65 | **Note: This means that the CAN device connected to the USB port with hardware encoding `1-2:1.0` is renamed to `can_piper`, set to a baud rate of 1,000,000, and activated.** 66 | 67 | Then run`ifconfig` to check if `can_piper` appears. If it does, the CAN module has been successfully configured. 68 | 69 | ### Running the Node 70 | 71 | #### Single Robotic Arm 72 | 73 | Node name: `piper_ctrl_single_node.py` 74 | 75 | param 76 | 77 | ```shell 78 | can_port:he name of the CAN route to open. 79 | auto_enable: Whether to automatically enable the system. If True, the system will automatically enable upon starting the program. 80 | # Set this to False if you want to manually control the enable state. If the program is interrupted and then restarted, the robotic arm will maintain the state it had during the last run. 81 | # If the arm was enabled, it will remain enabled after restarting. 82 | # If the arm was disabled, it will remain disabled after restarting. 83 | girpper_exist:Indicates if there is an end-effector gripper. If True, the gripper control will be enabled. 84 | rviz_ctrl_flag: Whether to use RViz to send joint angle messages. If True, the system will receive joint angle messages sent by rViz. 85 | # Since the joint 7 range in RViz is [0,0.04], but the actual gripper travel is 0.08m, joint 7 values sent by RViz will be multiplied by 2 when controlling the gripper. 86 | ``` 87 | 88 | `start_single_piper_rviz.launch`: 89 | 90 | ```xml 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | ``` 105 | 106 | `start_single_piper.launch`: 107 | 108 | ```xml 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | ``` 123 | 124 | Start control node 125 | 126 | ```shell 127 | # Start node 128 | roscore 129 | rosrun piper piper_ctrl_single_node.py _can_port:=can0 _mode:=0 130 | # Start launch 131 | roslaunch piper start_single_piper.launch can_port:=can0 auto_enable:=true 132 | # Or,the node can be run with default parameters 133 | roslaunch piper start_single_piper.launch 134 | # You can also use RViz to enable control by adjusting the parameters as described above. 135 | roslaunch piper start_single_piper_rviz.launch 136 | ``` 137 | 138 | If you only start control node but don't start rviz: 139 | 140 | `rostopic list` 141 | 142 | ```shell 143 | /arm_status #Status of robotic arm. 144 | /enable_flag #Enable flag, sent to the node, send "true" to enable 145 | /end_pose #Feedback on the end-effector pose of the robotic arm 146 | /joint_states #Subscribe to joint messages, sending joint positions through this message allows control of the robotic arm's movement. 147 | /joint_states_single #Feedback on the robotic arm's joint status 148 | /pos_cmd #End-effector control message 149 | ``` 150 | 151 | ## Note 152 | 153 | - You need to activate the CAN device and set the correct baud rate before you can read messages from or control the robotic arm. 154 | - If you see: 155 | 156 | ```shell 157 | Enable Status: False 158 | Message NOT sent 159 | Message NOT sent 160 | ``` 161 | 162 | It indicates that the robotic arm is not connected to the CAN module. After unplugging and replugging the USB, restart the robotic arm, then activate the CAN module, and try restarting the node again. 163 | 164 | If automatic enablement is enabled and enabling fails after 5 seconds, the program will automatically exit. 165 | 166 | ### piper Custom Messages 167 | 168 | ros package `piper_msgs` 169 | 170 | **Robotic Arm Status Feedback Message**: Corresponds to the feedback message with `id=0x2A1` in the CAN protocol. 171 | 172 | `PiperStatusMsg.msg` 173 | 174 | ```c 175 | uint8 ctrl_mode 176 | /* 177 | 0x00 Standby mode 178 | 0x01 CAN command control mode 179 | 0x02 Teaching mode 180 | 0x03 Ethernet control mode 181 | 0x04 wifi control mode 182 | 0x05 Remote controller control mode 183 | 0x06 Linked teaching mode 184 | 0x07 Offline track mode 185 | */ 186 | uint8 arm_status 187 | /* 188 | 0x00 Normal 189 | 0x01 Emergency stop 190 | 0x02 No solution 191 | 0x03 Singularity 192 | 0x04 Target angle exceeds limit 193 | 0x05 Joint communication error 194 | 0x06 Joint brake not released 195 | 0x07 Robotic arm collision detected 196 | 0x08 Overspeed during drag teaching 197 | 0x09 Joint status abnormal 198 | 0x0A Other abnormality 199 | 0x0B Teaching record 200 | 0x0C Teaching execution 201 | 0x0D Teaching paused 202 | 0x0E Main control NTC overheating 203 | 0x0F Discharge resistor NTC overheating 204 | */ 205 | uint8 mode_feedback 206 | /* 207 | 0x00 MOVE P 208 | 0x01 MOVE J 209 | 0x02 MOVE L 210 | 0x03 MOVE C 211 | */ 212 | uint8 teach_status 213 | /* 214 | 0x00 Close 215 | 0x01 Start teaching recording (enter drag teaching mode) 216 | 0x02 End teaching recording (exit drag teaching mode) 217 | 0x03 Execute teaching trajectory (reproduce drag teaching trajectory) 218 | 0x04 Pause 219 | 0x05 Resume (continue trajectory reproduction) 220 | 0x06 Terminate execution 221 | 0x07 Move to trajectory starting point 222 | */ 223 | uint8 motion_status 224 | /* 225 | 0x00 Reached the specified point 226 | 0x01 Not reached the specified point 227 | */ 228 | uint8 trajectory_num 229 | /* 0~255 (Feedback in offline trajectory mode) */ 230 | int64 err_code // Error code 231 | bool joint_1_angle_limit// Joint 1 communication error (0: Normal, 1: Error) 232 | bool joint_2_angle_limit// Joint 2 communication error (0: Normal, 1: Error) 233 | bool joint_3_angle_limit// Joint 3 communication error (0: Normal, 1: Error) 234 | bool joint_4_angle_limit// Joint 4 communication error (0: Normal, 1: Error) 235 | bool joint_5_angle_limit// Joint 5 communication error (0: Normal, 1: Error) 236 | bool joint_6_angle_limit// Joint 6 communication error (0: Normal, 1: Error) 237 | bool communication_status_joint_1// Joint 1 angle exceeds limit (0: Normal, 1: Error) 238 | bool communication_status_joint_2// Joint 2 angle exceeds limit (0: Normal, 1: Error) 239 | bool communication_status_joint_3// Joint 3 angle exceeds limit (0: Normal, 1: Error) 240 | bool communication_status_joint_4// Joint 4 angle exceeds limit (0: Normal, 1: Error) 241 | bool communication_status_joint_5// Joint 5 angle exceeds limit (0: Normal, 1: Error) 242 | bool communication_status_joint_6// Joint 6 angle exceeds limit (0: Normal, 1: Error) 243 | ``` 244 | 245 | End-effector pose control: Note that some singularities may be unreachable. 246 | 247 | `PosCmd.msg` 248 | 249 | ```c 250 | float64 x 251 | float64 y 252 | float64 z 253 | float64 roll 254 | float64 pitch 255 | float64 yaw 256 | float64 gripper 257 | int32 mode1// Temporarily unused 258 | int32 mode2// Temporarily unused 259 | ``` 260 | 261 | ## Simulation 262 | 263 | `display_xacro.launch` 264 | 265 | open rviz 266 | 267 | ```shell 268 | cd Piper_ros 269 | source devel/setup.bash 270 | roslaunch piper_description display_xacro.launch 271 | ``` 272 | 273 | After running, the `/joint_states` topic will be published. You can view it by `rostopic echo /joint_states` 274 | 275 | ![ ](./asserts/pictures/tostopic_list.jpg) 276 | 277 | Two windows will appear simultaneously as follows. The slider values correspond to the `/joint_states` values. Dragging the sliders will change these values, and the model in rviz will update accordingly. 278 | 279 | Sometimes the slider window is not visible. Check if there is a window in the dock. It may be covered by other pages. 280 | 281 | ![ ](./asserts/pictures/piper_rviz.jpg) 282 | 283 | ## Gazebo 284 | 285 | Run 286 | 287 | ```shell 288 | cd Piper_ros 289 | source devel/setup.bash 290 | roslaunch piper_description gazebo_xacro.launch 291 | ``` 292 | 293 | ![ ](./asserts/pictures/piper_gazebo.jpg) 294 | 295 | After running, these will be published: 296 | 297 | ```shell 298 | /piper_description/joint_states 299 | /piper_description/joint1_position_controller/command 300 | /piper_description/joint2_position_controller/command 301 | /piper_description/joint3_position_controller/command 302 | /piper_description/joint4_position_controller/command 303 | /piper_description/joint5_position_controller/command 304 | /piper_description/joint6_position_controller/command 305 | /piper_description/joint7_position_controller/command 306 | /piper_description/joint8_position_controller/command 307 | ``` 308 | 309 | `/piper_description/joint_states` is the virtual feedback topic for the joint states of the robotic arm in Gazebo, while the others are subscriber nodes in Gazebo. 310 | 311 | These command topics are used to control the movement of the robotic arm in Gazebo. The joint limits in the xacro file are as follows: joints 1 to 6 are in radians, and joints 7 and 8 are in meters, as joints 7 and 8 correspond to the gripper. 312 | 313 | | joint_name | limit | 314 | | ---------- | -------------- | 315 | | joint1 | [-2.618,2.618] | 316 | | joint2 | [0,3.14] | 317 | | joint3 | [-2.697,0] | 318 | | joint4 | [-1.832,1.832] | 319 | | joint5 | [-1.22,1.22] | 320 | | joint6 | [-2.0944,2.0944] | 321 | | joint7 | [0,0.038] | 322 | | joint8 | [-0.038,0] | 323 | 324 | To move Joint 1 by 90 degrees, you would need to send a command: 325 | 326 | ```shell 327 | rostopic pub /piper_description/joint1_position_controller/command std_msgs/Float64 "data: 1.57" 328 | ``` 329 | 330 | You can see that the robot link1 rotates 90 degrees 331 | 332 | ![ ](./asserts/pictures/piper_gazebo_90.jpg) 333 | -------------------------------------------------------------------------------- /src/Piper_ros/README.MD: -------------------------------------------------------------------------------- 1 | # agx 机械臂 2 | 3 | [EN](README(EN).md) 4 | 5 | ![ubuntu](https://img.shields.io/badge/Ubuntu-20.04-orange.svg) 6 | 7 | |ROS |STATE| 8 | |---|---| 9 | |![ros](https://img.shields.io/badge/ROS-noetic-blue.svg)|![Pass](https://img.shields.io/badge/Pass-blue.svg)| 10 | 11 | ## 0 注意URDF版本(以零点区分) 12 | 13 | 若您的机械臂，上电后使用上位机使能回零，j2和j3关节抬起了2度的角度，则为v00版零点，如下图: 14 | 15 | ![ ](./asserts/pictures/piper_zero.png) 16 | 17 | 旧版本零点以j2和j3的限位位置偏移2度得到，现URDF中的文件都是接触限位处 18 | 19 | ## 1 安装方法 20 | 21 | ### 1.1 安装依赖 22 | 23 | ```shell 24 | pip3 install python-can 25 | ``` 26 | 27 | ```shell 28 | pip3 install piper_sdk 29 | ``` 30 | 31 | ## 2 快速使用 32 | 33 | ### 2.2 使能can模块 34 | 35 | 首先需要设置好shell脚本参数 36 | 37 | #### 2.2.1 单条机械臂 38 | 39 | ##### 1）pc只插入一个usb转can模块 40 | 41 | - **此处使用`can_activate.sh`脚本** 42 | 43 | 直接执行 44 | 45 | ```bash 46 | bash can_activate.sh can0 1000000 47 | ``` 48 | 49 | ##### 2）pc插入多个usb转can模块 50 | 51 | - **此处使用`can_activate.sh`脚本** 52 | 53 | 拔掉所有can模块 54 | 55 | 只将连接到机械臂的can模块插入PC，执行 56 | 57 | ```shell 58 | sudo ethtool -i can0 | grep bus 59 | ``` 60 | 61 | 并记录下`bus-info`的数值例如`1-2:1.0` 62 | 63 | ps:**一般第一个插入的can模块会默认是can0，如果没有查询到can可以使用`bash find_all_can_port.sh`来查看刚才usb地址对应的can名称** 64 | 65 | 假设上面的操作记录的`bus-info`数值为`1-2:1.0` 66 | 67 | 然后执行,查看can设备是否激活成功 68 | 69 | ```bash 70 | bash can_activate.sh can_piper 1000000 "1-2:1.0" 71 | ``` 72 | 73 | ps:**此处的意思是，1-2:1.0硬件编码的usb端口插入的can设备，名字被重命名为can_piper,波特率为1000000，并激活** 74 | 75 | 然后执行`ifconfig`查看是否有`can_piper`，如果有则can模块设置成功 76 | 77 | ### 2.3 运行节点 78 | 79 | #### 2.3.1 单机械臂 80 | 81 | 节点名`piper_ctrl_single_node.py` 82 | 83 | param 84 | 85 | ```shell 86 | can_port:要打开的can路由名字 87 | auto_enable:是否自动使能，True则开启程序就自动使能 88 | # 注意这个设置为False，中断程序后再启动节点，机械臂会保持上次启动程序的状态 89 | # 若上次启动程序机械臂状态为使能，则中断程序后再启动节点，机械臂仍为使能 90 | # 若上次启动程序机械臂状态为失能，则中断程序后再启动节点，机械臂仍为失能 91 | girpper_exist:是否有末端夹爪，True则说明有末端夹爪，会开启夹爪控制 92 | rviz_ctrl_flag:是否使用rviz来发送关节角消息，True则接收rviz发送的关节角消息 93 | #由于rviz中的joint7范围是[0,0.04]，而真实夹爪行程为0.08m，打开rviz控制后会将rviz发出的joint7乘2倍 94 | ``` 95 | 96 | `start_single_piper.launch`如下: 97 | 98 | ```xml 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | ``` 115 | 116 | `start_single_piper_rviz.launch`如下: 117 | 118 | ```xml 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | ``` 134 | 135 | ##### （1）启动控制节点 136 | 137 | 提供几种不同的启动方式，启动的都是相同节点 138 | 139 | ```shell 140 | # 启动节点 141 | roscore 142 | rosrun piper piper_ctrl_single_node.py _can_port:=can0 _mode:=0 143 | # 或，使用launch 启动节点 144 | roslaunch piper start_single_piper.launch can_port:=can0 auto_enable:=true 145 | # 或，直接运行launch，会以默认参数运行 146 | roslaunch piper start_single_piper.launch 147 | # 也可以用rviz开启控制,需要更改的参数如上述 148 | roslaunch piper start_single_piper_rviz.launch 149 | ``` 150 | 151 | 如果只是单独启动了控制节点，没有启动rviz 152 | 153 | `rostopic list` 154 | 155 | ```shell 156 | /arm_status #机械臂状态，详见下文 157 | /enable_flag #使能标志位，发送给节点，发送true用来使能 158 | /end_pose #机械臂末端位姿状态反馈，四元数 159 | /end_pose_euler #机械臂末端位姿状态反馈，欧拉角(为自定义消息) 160 | /joint_states #订阅关节消息，给这个消息发送关节位置能控制机械臂运动 161 | /joint_states_single #机械臂关节状态反馈 162 | /pos_cmd #末端控制消息 163 | ``` 164 | 165 | `rosservice list` 166 | 167 | ```shell 168 | /enable_srv #机械臂使能服务端 169 | /go_zero_srv #机械臂归零服务 170 | /gripper_srv #机械臂夹爪控制服务 171 | /reset_srv #机械臂重置服务 172 | /stop_srv #机械臂停止服务 173 | ``` 174 | 175 | ##### （2）使能机械臂 176 | 177 | ```shell 178 | # call 服务端 179 | rosservice call /enable_srv "enable_request: true" 180 | # pub topic 181 | rostopic pub /enable_flag std_msgs/Bool "data: true" 182 | ``` 183 | 184 | ##### （3）失能机械臂 185 | 186 | ```shell 187 | # call 服务端 188 | rosservice call /enable_srv "enable_request: false" 189 | # pub topic 190 | rostopic pub /enable_flag std_msgs/Bool "data: false" 191 | ``` 192 | 193 | ##### （4）发布关节消息 194 | 195 | 注意，机械臂会抬起，请确保机械臂工作范围内无障碍 196 | 197 | ```shell 198 | rostopic pub /joint_states sensor_msgs/JointState "header: 199 | seq: 0 200 | stamp: {secs: 0, nsecs: 0} 201 | frame_id: '' 202 | name: [''] 203 | position: [0.2,0.2,-0.2,0.3,-0.2,0.5,0.01] 204 | velocity: [0,0,0,0,0,0,10] 205 | effort: [0,0,0,0,0,0,0.5]" 206 | ``` 207 | 208 | ##### （5）停止机械臂，注意机械臂会以一个恒定阻尼落下 209 | 210 | ```shell 211 | rosservice call /stop_srv 212 | ``` 213 | 214 | ##### （6）重置机械臂，注意机械臂会立刻掉电落下 215 | 216 | ```shell 217 | rosservice call /reset_srv 218 | ``` 219 | 220 | ##### （7）令机械臂归零 221 | 222 | - 如果机械臂为mit模式，将`is_mit_mode`置为`true` 223 | - 如果机械臂不为mit模式(位置速度控制模式)，将`is_mit_mode`置为`false` 224 | 225 | ```shell 226 | rosservice call /go_zero_srv "is_mit_mode: false" 227 | rosservice call /go_zero_srv "is_mit_mode: true" 228 | ``` 229 | 230 | ### 2.4 piper自定义消息 231 | 232 | ros功能包`piper_msgs` 233 | 234 | 机械臂自身状态反馈消息，对应can协议中`id=0x2A1`的反馈消息 235 | 说明 236 | 237 | `PiperStatusMsg.msg` 238 | 239 | ```c 240 | uint8 ctrl_mode 241 | /* 242 | 0x00 待机模式 243 | 0x01 CAN指令控制模式 244 | 0x02 示教模式 245 | 0x03 以太网控制模式 246 | 0x04 wifi控制模式 247 | 0x05 遥控器控制模式 248 | 0x06 联动示教输入模式 249 | 0x07 离线轨迹模式*/ 250 | uint8 arm_status 251 | /* 252 | 0x00 正常 253 | 0x01 急停 254 | 0x02 无解 255 | 0x03 奇异点 256 | 0x04 目标角度超过限 257 | 0x05 关节通信异常 258 | 0x06 关节抱闸未打开 259 | 0x07 机械臂发生碰撞 260 | 0x08 拖动示教时超速 261 | 0x09 关节状态异常 262 | 0x0A 其它异常 263 | 0x0B 示教记录 264 | 0x0C 示教执行 265 | 0x0D 示教暂停 266 | 0x0E 主控NTC过温 267 | 0x0F 释放电阻NTC过温*/ 268 | uint8 mode_feedback 269 | /* 270 | 0x00 MOVE P 271 | 0x01 MOVE J 272 | 0x02 MOVE L 273 | 0x03 MOVE C*/ 274 | uint8 teach_status 275 | /* 276 | 0x00 关闭 277 | 0x01 开始示教记录（进入拖动示教模式） 278 | 0x02 结束示教记录（退出拖动示教模式） 279 | 0x03 执行示教轨迹（拖动示教轨迹复现） 280 | 0x04 暂停执行 281 | 0x05 继续执行（轨迹复现继续） 282 | 0x06 终止执行 283 | 0x07 运动到轨迹起点*/ 284 | uint8 motion_status 285 | /* 286 | 0x00 到达指定点位 287 | 0x01 未到达指定点位*/ 288 | uint8 trajectory_num 289 | /*0~255 (离线轨迹模式下反馈)*/ 290 | int64 err_code//故障码 291 | bool joint_1_angle_limit//1号关节通信异常（0：正常 1：异常） 292 | bool joint_2_angle_limit//2号关节通信异常（0：正常 1：异常） 293 | bool joint_3_angle_limit//3号关节通信异常（0：正常 1：异常） 294 | bool joint_4_angle_limit//4号关节通信异常（0：正常 1：异常） 295 | bool joint_5_angle_limit//5号关节通信异常（0：正常 1：异常） 296 | bool joint_6_angle_limit//6号关节通信异常（0：正常 1：异常） 297 | bool communication_status_joint_1//1号关节角度超限位（0：正常 1：异常） 298 | bool communication_status_joint_2//2号关节角度超限位（0：正常 1：异常） 299 | bool communication_status_joint_3//3号关节角度超限位（0：正常 1：异常） 300 | bool communication_status_joint_4//4号关节角度超限位（0：正常 1：异常） 301 | bool communication_status_joint_5//5号关节角度超限位（0：正常 1：异常） 302 | bool communication_status_joint_6//6号关节角度超限位（0：正常 1：异常） 303 | ``` 304 | 305 | 机械臂末端位姿控制，注意：有些奇异点无法到达 306 | 307 | `PosCmd.msg` 308 | 309 | ```c 310 | // 单位：m 311 | float64 x 312 | float64 y 313 | float64 z 314 | // 单位：rad 315 | float64 roll 316 | float64 pitch 317 | float64 yaw 318 | float64 gripper 319 | // 暂为无效参数 320 | int32 mode1 321 | int32 mode2 322 | ``` 323 | 324 | ## 3 注意事项 325 | 326 | - 需要先激活can设备，并且设置正确的波特率，才可以读取机械臂消息或者控制机械臂 327 | - 如果出现 328 | 329 | ```shell 330 | 使能状态: False 331 | Message NOT sent 332 | Message NOT sent 333 | ``` 334 | 335 | 说明机械臂没有与can模块连同，拔插usb后，重新启动机械臂，再激活can模块，然后尝试重新启动节点 336 | 337 | - 如果打开了自动使能，尝试使能5s没有成功后，会自动退出程序 338 | 339 | ## 4 使用仿真 340 | 341 | ### 4.1 rviz 342 | 343 | `display_xacro.launch` 打开rviz 344 | 345 | 执行 346 | 347 | ```shell 348 | cd Piper_ros 349 | source devel/setup.bash 350 | roslaunch piper_description display_xacro.launch 351 | ``` 352 | 353 | 运行后会发布`/joint_states`，可以通过`rostopic echo /joint_states` 查看 354 | 355 | ![ ](./asserts/pictures/tostopic_list.jpg) 356 | 357 | 同时会弹出两个页面如下，滑动条数值对应`/joint_states`数值，拖动滑动条可以改变其数值，rviz中的模型也会随动 358 | 359 | 有时滑动条窗口未显示，请查看下dock栏是否有窗口，可能是被其它页面覆盖了 360 | 361 | ![ ](./asserts/pictures/piper_rviz.png) 362 | 363 | ### 4.2 gazebo 364 | 365 | 执行 366 | 367 | ```shell 368 | cd Piper_ros 369 | source devel/setup.bash 370 | roslaunch piper_description gazebo_xacro.launch 371 | ``` 372 | 373 | ![ ](./asserts/pictures/piper_gazebo.png) 374 | 375 | 运行后会发布 376 | 377 | ```shell 378 | /piper_description/joint_states 379 | /piper_description/joint1_position_controller/command 380 | /piper_description/joint2_position_controller/command 381 | /piper_description/joint3_position_controller/command 382 | /piper_description/joint4_position_controller/command 383 | /piper_description/joint5_position_controller/command 384 | /piper_description/joint6_position_controller/command 385 | /piper_description/joint7_position_controller/command 386 | /piper_description/joint8_position_controller/command 387 | ``` 388 | 389 | `/piper_description/joint_states` 是gazebo的机械臂关节状态的虚拟反馈话题，其余的为gazebo的订阅节点 390 | 391 | 这几个command话题用来控制gazebo中的机械臂运动，关节在xacro中的限位如下，其中，1～6关节单位为弧度，7、8关节单位为m，因为7、8关节对应夹爪 392 | 393 | |joint_name| limit(rad) | limit(angle) | limit(rad/s) | limit(rad/s^2) | 394 | |----------| ---------- | ---------- | ---------- | ---------- | 395 | |joint1 | [-2.618, 2.618] | [-150.0, 150.0] | [0, 3.0] | [0, 5.0] | 396 | |joint2 | [0, 3.14] | [0, 180.0] | [0, 3.0] | [0, 5.0] | 397 | |joint3 | [-2.967, 0] | [-170, 0] | [0, 3.0] | [0, 5.0] | 398 | |joint4 | [-1.745, 1.745] | [-100.0, 100.0] | [0, 3.0] | [0, 5.0] | 399 | |joint5 | [-1.22, 1.22] | [-70.0, 70.0] | [0, 3.0] | [0, 5.0] | 400 | |joint6 | [-2.0944, 2.0944]| [-120.0, 120.0] | [0, 3.0] | [0, 5.0] | 401 | 402 | 假如想要令关节1运动90度，发送 403 | 404 | ```shell 405 | rostopic pub /piper_description/joint1_position_controller/command std_msgs/Float64 "data: 1.57" 406 | ``` 407 | 408 | 可以看到机械臂link1旋转了90度 409 | 410 | ![ ](./asserts/pictures/piper_gazebo_90.png) 411 | 412 | ## Q&A 413 | 414 | [Q&A](Q&A.MD) 415 | -------------------------------------------------------------------------------- /src/Piper_ros/asserts/pictures/piper_gazebo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/asserts/pictures/piper_gazebo.png -------------------------------------------------------------------------------- /src/Piper_ros/asserts/pictures/piper_gazebo_90.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/asserts/pictures/piper_gazebo_90.png -------------------------------------------------------------------------------- /src/Piper_ros/asserts/pictures/piper_rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/asserts/pictures/piper_rviz.png -------------------------------------------------------------------------------- /src/Piper_ros/asserts/pictures/piper_urdf_no_gripper_zero.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/asserts/pictures/piper_urdf_no_gripper_zero.png -------------------------------------------------------------------------------- /src/Piper_ros/asserts/pictures/piper_urdf_zero.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/asserts/pictures/piper_urdf_zero.png -------------------------------------------------------------------------------- /src/Piper_ros/asserts/pictures/piper_zero.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/asserts/pictures/piper_zero.png -------------------------------------------------------------------------------- /src/Piper_ros/asserts/pictures/tostopic_list.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/asserts/pictures/tostopic_list.jpg -------------------------------------------------------------------------------- /src/Piper_ros/can_activate.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # 默认的 CAN 名称，用户可以通过命令行参数设定 4 | DEFAULT_CAN_NAME="${1:-can0}" 5 | 6 | # 单个 CAN 模块时的默认比特率，用户可以通过命令行参数设定 7 | DEFAULT_BITRATE="${2:-1000000}" 8 | 9 | # USB 硬件地址（可选参数） 10 | USB_ADDRESS="${3}" 11 | echo "-------------------START-----------------------" 12 | # 检查 ethtool 是否已安装 13 | if ! dpkg -l | grep -q "ethtool"; then 14 | echo "\e[31m错误: 系统中未检测到 ethtool。\e[0m" 15 | echo "请使用以下命令安装 ethtool:" 16 | echo "sudo apt update && sudo apt install ethtool" 17 | exit 1 18 | fi 19 | 20 | # 检查 can-utils 是否已安装 21 | if ! dpkg -l | grep -q "can-utils"; then 22 | echo "\e[31m错误: 系统中未检测到 can-utils。\e[0m" 23 | echo "请使用以下命令安装 can-utils:" 24 | echo "sudo apt update && sudo apt install can-utils" 25 | exit 1 26 | fi 27 | 28 | echo "ethtool 和 can-utils 均已安装。" 29 | 30 | # 获取当前系统中的 CAN 模块数量 31 | CURRENT_CAN_COUNT=$(ip link show type can | grep -c "link/can") 32 | 33 | # 检查当前系统中的 CAN 模块数量是否符合预期 34 | if [ "$CURRENT_CAN_COUNT" -ne "1" ]; then 35 | if [ -z "$USB_ADDRESS" ]; then 36 | # 遍历所有 CAN 接口 37 | for iface in $(ip -br link show type can | awk '{print $1}'); do 38 | # 使用 ethtool 获取 bus-info 39 | BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}') 40 | 41 | if [ -z "$BUS_INFO" ];then 42 | echo "错误: 无法获取接口 $iface 的 bus-info 信息。" 43 | continue 44 | fi 45 | 46 | echo "接口 $iface 插入在 USB 端口 $BUS_INFO" 47 | done 48 | echo -e " \e[31m 错误: 系统检测到的 CAN 模块数量 ($CURRENT_CAN_COUNT) 与预期数量 (1) 不符。\e[0m" 49 | echo -e " \e[31m 请增加usb硬件地址参数，如: \e[0m" 50 | echo -e " bash can_activate.sh can0 1000000 1-2:1.0" 51 | echo "-------------------ERROR-----------------------" 52 | exit 1 53 | fi 54 | fi 55 | 56 | # 加载 gs_usb 模块 57 | # sudo modprobe gs_usb 58 | # if [ $? -ne 0 ]; then 59 | # echo "错误: 无法加载 gs_usb 模块。" 60 | # exit 1 61 | # fi 62 | 63 | if [ -n "$USB_ADDRESS" ]; then 64 | echo "检测到 USB 硬件地址参数: $USB_ADDRESS" 65 | 66 | # 使用 ethtool 查找与 USB 硬件地址对应的 CAN 接口 67 | INTERFACE_NAME="" 68 | for iface in $(ip -br link show type can | awk '{print $1}'); do 69 | BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}') 70 | if [ "$BUS_INFO" == "$USB_ADDRESS" ]; then 71 | INTERFACE_NAME="$iface" 72 | break 73 | fi 74 | done 75 | 76 | if [ -z "$INTERFACE_NAME" ]; then 77 | echo "错误: 无法找到与 USB 硬件地址 $USB_ADDRESS 对应的 CAN 接口。" 78 | exit 1 79 | else 80 | echo "找到与 USB 硬件地址 $USB_ADDRESS 对应的接口: $INTERFACE_NAME" 81 | fi 82 | else 83 | # 获取唯一的 CAN 接口 84 | INTERFACE_NAME=$(ip -br link show type can | awk '{print $1}') 85 | 86 | # 检查是否获取到了接口名称 87 | if [ -z "$INTERFACE_NAME" ]; then 88 | echo "错误: 无法检测到 CAN 接口。" 89 | exit 1 90 | fi 91 | BUS_INFO=$(sudo ethtool -i "$INTERFACE_NAME" | grep "bus-info" | awk '{print $2}') 92 | echo "预期配置单个can模块，检测到接口 $INTERFACE_NAME，对应的usb地址为 $BUS_INFO" 93 | fi 94 | 95 | # 检查当前接口是否已经激活 96 | IS_LINK_UP=$(ip link show "$INTERFACE_NAME" | grep -q "UP" && echo "yes" || echo "no") 97 | 98 | # 获取当前接口的比特率 99 | CURRENT_BITRATE=$(ip -details link show "$INTERFACE_NAME" | grep -oP 'bitrate \K\d+') 100 | 101 | if [ "$IS_LINK_UP" == "yes" ] && [ "$CURRENT_BITRATE" -eq "$DEFAULT_BITRATE" ]; then 102 | echo "接口 $INTERFACE_NAME 已经激活，并且比特率为 $DEFAULT_BITRATE" 103 | 104 | # 检查接口名称是否与默认的名称匹配 105 | if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then 106 | echo "将接口 $INTERFACE_NAME 重命名为 $DEFAULT_CAN_NAME" 107 | sudo ip link set "$INTERFACE_NAME" down 108 | sudo ip link set "$INTERFACE_NAME" name "$DEFAULT_CAN_NAME" 109 | sudo ip link set "$DEFAULT_CAN_NAME" up 110 | echo "接口已重命名为 $DEFAULT_CAN_NAME，并重新激活。" 111 | else 112 | echo "接口名称已经是 $DEFAULT_CAN_NAME" 113 | fi 114 | else 115 | # 如果接口未激活或比特率不同，进行设置 116 | if [ "$IS_LINK_UP" == "yes" ]; then 117 | echo "接口 $INTERFACE_NAME 已经激活，但比特率为 $CURRENT_BITRATE，与设定的 $DEFAULT_BITRATE 不符。" 118 | else 119 | echo "接口 $INTERFACE_NAME 未激活或未设置比特率。" 120 | fi 121 | 122 | # 设置接口比特率并激活 123 | sudo ip link set "$INTERFACE_NAME" down 124 | sudo ip link set "$INTERFACE_NAME" type can bitrate $DEFAULT_BITRATE 125 | sudo ip link set "$INTERFACE_NAME" up 126 | echo "接口 $INTERFACE_NAME 已重新设置为比特率 $DEFAULT_BITRATE 并激活。" 127 | 128 | # 重命名接口为默认名称 129 | if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then 130 | echo "将接口 $INTERFACE_NAME 重命名为 $DEFAULT_CAN_NAME" 131 | sudo ip link set "$INTERFACE_NAME" down 132 | sudo ip link set "$INTERFACE_NAME" name "$DEFAULT_CAN_NAME" 133 | sudo ip link set "$DEFAULT_CAN_NAME" up 134 | echo "接口已重命名为 $DEFAULT_CAN_NAME，并重新激活。" 135 | fi 136 | fi 137 | 138 | echo "-------------------OVER------------------------" 139 | -------------------------------------------------------------------------------- /src/Piper_ros/can_config.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # 使用说明： 4 | 5 | # 1.先决条件 6 | # 需要在系统中安装 ip 工具和 ethtool 工具。 7 | # sudo apt install ethtool can-utils 8 | # 确保 gs_usb 驱动已正确安装。 9 | 10 | # 2.背景 11 | # 本脚本旨在自动管理和重命名并激活 CAN（Controller Area Network）接口。 12 | # 它检查系统中当前的 CAN 模块数量，并根据预定义的 USB 端口重命名 CAN 接口并激活。 13 | # 这对于有多个 CAN 模块的系统，尤其是不同 CAN 模块需要特定名称的情况，非常有用。 14 | 15 | # 3.主要功能 16 | # 检查 CAN 模块数量：确保系统中检测到的 CAN 模块数量与预设的数量一致。 17 | # 获取 USB 端口信息：通过 ethtool 获取每个 CAN 模块的 USB 端口信息。 18 | # 验证 USB 端口：检查每个 CAN 模块的 USB 端口是否符合预定义的端口列表。 19 | # 重命名 CAN 接口：根据预定义的 USB 端口，将 CAN 接口重命名为目标名称。 20 | 21 | # 4.脚本配置说明 22 | # 脚本中的关键配置项包括预期的 CAN 模块数量、默认的 CAN 接口名称和波特率设置： 23 | # 1.预期的 CAN 模块数量： 24 | # EXPECTED_CAN_COUNT=1 25 | # 这个值决定了系统中应该检测到的 CAN 模块数量。 26 | # 2.单个can模块的时候默认的 CAN 接口名称： 27 | # DEFAULT_CAN_NAME="${1:-can0}" 28 | # 可以通过命令行参数指定默认的 CAN 接口名称，如果不提供参数，默认为 can0。 29 | # 3.单个 CAN 模块时的默认比特率： 30 | # DEFAULT_BITRATE="${2:-500000}" 31 | # 可以通过命令行参数指定单个 CAN 模块时的比特率，如果不提供参数，默认为 500000。 32 | # 4.多个 CAN 模块时的配置: 33 | # declare -A USB_PORTS 34 | # USB_PORTS["1-2:1.0"]="can_device_1:500000" 35 | # USB_PORTS["1-3:1.0"]="can_device_2:250000" 36 | # 这里的键表示 USB 端口，值是接口名称和比特率，使用冒号分隔。 37 | 38 | # 5.使用步骤 39 | # 1.编辑脚本： 40 | # 1. 修改预定义值： 41 | # - 预定义的 CAN 模块数量：EXPECTED_CAN_COUNT=2，可以修改为工控机上插入的can模块数量 42 | # - 如果只有一个can模块，设定完上面的参数可以直接跳过此处往后看 43 | # - (多个can模块)预定义的 USB 端口和目标接口名称： 44 | # 先将某个can模块插入到预期的usb口，注意在初次配置时，每次在工控机上插入一个can模块 45 | # 然后执行 sudo ethtool -i can0 | grep bus,并记录下 bus-info: 后面的参数 46 | # 接着插入下一个can模块，注意不可以与上次can模块插入的usb口相同，然后重复执行上一步 47 | # (其实可以用一个can模块去插不同的usb，因为区分模块是根据usb地址来区分的) 48 | # 所有模块都设计好所应该在的usb口并记录完成后， 49 | # 根据实际情况修改 USB 端口（bus-info）和目标接口名称。 50 | # can_device_1:500000，前者为设定的can名称，后者为设定的波特率 51 | # declare -A USB_PORTS 52 | # USB_PORTS["1-2:1.0"]="can_device_1:500000" 53 | # USB_PORTS["1-3:1.0"]="can_device_2:250000" 54 | # 需要修改的是USB_PORTS["1-3:1.0"]内双引号的内容，修改为上面记录的bus-info: 后面的参数 55 | # 2.赋予脚本执行权限： 56 | # 打开终端，导航到脚本所在目录，执行以下命令赋予脚本执行权限： 57 | # chmod +x can_config.sh 58 | # 3.运行脚本： 59 | # 使用 sudo 执行脚本，因为脚本需要管理员权限来修改网络接口： 60 | # 1.单个 CAN 模块 61 | # 1.可以通过命令行参数指定默认的 CAN 接口名称和比特率（默认为 can0 和 500000）： 62 | # sudo bash ./can_config.sh [CAN接口名称] [比特率] 63 | # 例如，指定接口名称为 my_can_interface，比特率为 1000000： 64 | # sudo bash ./can_config.sh my_can_interface 1000000 65 | # 2.可以通过指定的usb硬件地址来指定can名称 66 | # sudo bash ./can_config.sh [CAN接口名称] [比特率] [usb硬件地址] 67 | # 例如，指定接口名称为 my_can_interface，比特率为 1000000，usb硬件地址为 1-3:1.0： 68 | # sudo bash ./can_config.sh my_can_interface 1000000 1-3:1.0 69 | # 也就是将 1-3:1.0 usb地址的can设备指定名称为my_can_interface，比特率为 1000000 70 | # 2.多个 CAN 模块 71 | # 对于多个 CAN 模块，通过在脚本中设置 USB_PORTS 数组来指定每个 CAN 模块的接口名称和比特率。 72 | # 无需额外参数，直接运行脚本： 73 | # sudo ./can_config.sh 74 | 75 | # 注意事项 76 | 77 | # 权限要求： 78 | # 脚本需要使用 sudo 权限，因为网络接口的重命名和设置需要管理员权限。 79 | # 确保你有足够的权限来运行该脚本。 80 | 81 | # 脚本环境： 82 | # 本脚本假设在 bash 环境下运行。确保你的系统使用 bash，而不是其他 Shell（如 sh）。 83 | # 可以通过检查脚本的 Shebang 行（#!/bin/bash）确保使用 bash。 84 | 85 | # USB 端口信息： 86 | # 确保你预定义的 USB 端口信息（bus-info）与实际系统中 ethtool 输出的信息一致。 87 | # 使用命令 sudo ethtool -i can0、sudo ethtool -i can1 等检查每个 CAN 接口的 bus-info。 88 | 89 | # 接口冲突： 90 | # 确保目标接口名称（如 can_device_1、can_device_2）是唯一的，不与系统中其他现有接口名称冲突。 91 | # 如果要修改 USB 端口和接口名称的对应关系，请根据实际情况调整 USB_PORTS 数组。 92 | #-------------------------------------------------------------------------------------------------# 93 | 94 | # 预定义的 CAN 模块数量 95 | EXPECTED_CAN_COUNT=2 96 | 97 | if [ "$EXPECTED_CAN_COUNT" -eq 1 ]; then 98 | # 默认的 CAN 名称，用户可以通过命令行参数设定 99 | DEFAULT_CAN_NAME="${1:-can0}" 100 | 101 | # 单个 CAN 模块时的默认比特率，用户可以通过命令行参数设定 102 | DEFAULT_BITRATE="${2:-1000000}" 103 | 104 | # USB 硬件地址（可选参数） 105 | USB_ADDRESS="${3}" 106 | fi 107 | 108 | # 预定义的 USB 端口、目标接口名称及其比特率（在多个 CAN 模块时使用） 109 | if [ "$EXPECTED_CAN_COUNT" -ne 1 ]; then 110 | declare -A USB_PORTS 111 | USB_PORTS["1-8:1.0"]="left_piper:1000000" 112 | USB_PORTS["1-4:1.0"]="right_piper:1000000" 113 | fi 114 | 115 | # 获取当前系统中的 CAN 模块数量 116 | CURRENT_CAN_COUNT=$(ip link show type can | grep -c "link/can") 117 | 118 | # 检查当前系统中的 CAN 模块数量是否符合预期 119 | if [ "$CURRENT_CAN_COUNT" -ne "$EXPECTED_CAN_COUNT" ]; then 120 | echo "错误: 检测到的 CAN 模块数量 ($CURRENT_CAN_COUNT) 与预期数量 ($EXPECTED_CAN_COUNT) 不符。" 121 | exit 1 122 | fi 123 | 124 | # 加载 gs_usb 模块 125 | sudo modprobe gs_usb 126 | if [ $? -ne 0 ]; then 127 | echo "错误: 无法加载 gs_usb 模块。" 128 | exit 1 129 | fi 130 | 131 | # 判断是否只需要处理一个 CAN 模块 132 | if [ "$EXPECTED_CAN_COUNT" -eq 1 ]; then 133 | if [ -n "$USB_ADDRESS" ]; then 134 | echo "检测到 USB 硬件地址参数: $USB_ADDRESS" 135 | 136 | # 使用 ethtool 查找与 USB 硬件地址对应的 CAN 接口 137 | INTERFACE_NAME="" 138 | for iface in $(ip -br link show type can | awk '{print $1}'); do 139 | BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}') 140 | if [ "$BUS_INFO" == "$USB_ADDRESS" ]; then 141 | INTERFACE_NAME="$iface" 142 | break 143 | fi 144 | done 145 | 146 | if [ -z "$INTERFACE_NAME" ]; then 147 | echo "错误: 无法找到与 USB 硬件地址 $USB_ADDRESS 对应的 CAN 接口。" 148 | exit 1 149 | else 150 | echo "找到与 USB 硬件地址 $USB_ADDRESS 对应的接口: $INTERFACE_NAME" 151 | fi 152 | else 153 | # 获取唯一的 CAN 接口 154 | INTERFACE_NAME=$(ip -br link show type can | awk '{print $1}') 155 | 156 | # 检查是否获取到了接口名称 157 | if [ -z "$INTERFACE_NAME" ]; then 158 | echo "错误: 无法检测到 CAN 接口。" 159 | exit 1 160 | fi 161 | 162 | echo "预期只有一个 CAN 模块，检测到接口 $INTERFACE_NAME" 163 | fi 164 | 165 | # 检查当前接口是否已经激活 166 | IS_LINK_UP=$(ip link show "$INTERFACE_NAME" | grep -q "UP" && echo "yes" || echo "no") 167 | 168 | # 获取当前接口的比特率 169 | CURRENT_BITRATE=$(ip -details link show "$INTERFACE_NAME" | grep -oP 'bitrate \K\d+') 170 | 171 | if [ "$IS_LINK_UP" == "yes" ] && [ "$CURRENT_BITRATE" -eq "$DEFAULT_BITRATE" ]; then 172 | echo "接口 $INTERFACE_NAME 已经激活，并且比特率为 $DEFAULT_BITRATE" 173 | 174 | # 检查接口名称是否与默认的名称匹配 175 | if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then 176 | echo "将接口 $INTERFACE_NAME 重命名为 $DEFAULT_CAN_NAME" 177 | sudo ip link set "$INTERFACE_NAME" down 178 | sudo ip link set "$INTERFACE_NAME" name "$DEFAULT_CAN_NAME" 179 | sudo ip link set "$DEFAULT_CAN_NAME" up 180 | echo "接口已重命名为 $DEFAULT_CAN_NAME，并重新激活。" 181 | else 182 | echo "接口名称已经是 $DEFAULT_CAN_NAME" 183 | fi 184 | else 185 | # 如果接口未激活或比特率不同，进行设置 186 | if [ "$IS_LINK_UP" == "yes" ]; then 187 | echo "接口 $INTERFACE_NAME 已经激活，但比特率为 $CURRENT_BITRATE，与设定的 $DEFAULT_BITRATE 不符。" 188 | else 189 | echo "接口 $INTERFACE_NAME 未激活或未设置比特率。" 190 | fi 191 | 192 | # 设置接口比特率并激活 193 | sudo ip link set "$INTERFACE_NAME" down 194 | sudo ip link set "$INTERFACE_NAME" type can bitrate $DEFAULT_BITRATE 195 | sudo ip link set "$INTERFACE_NAME" up 196 | echo "接口 $INTERFACE_NAME 已重新设置为比特率 $DEFAULT_BITRATE 并激活。" 197 | 198 | # 重命名接口为默认名称 199 | if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then 200 | echo "将接口 $INTERFACE_NAME 重命名为 $DEFAULT_CAN_NAME" 201 | sudo ip link set "$INTERFACE_NAME" down 202 | sudo ip link set "$INTERFACE_NAME" name "$DEFAULT_CAN_NAME" 203 | sudo ip link set "$DEFAULT_CAN_NAME" up 204 | echo "接口已重命名为 $DEFAULT_CAN_NAME，并重新激活。" 205 | fi 206 | fi 207 | else 208 | # 处理多个 CAN 模块 209 | 210 | # 检查 USB 端口和目标接口名称的数量是否与预期 CAN 模块数量匹配 211 | PREDEFINED_COUNT=${#USB_PORTS[@]} 212 | if [ "$EXPECTED_CAN_COUNT" -ne "$PREDEFINED_COUNT" ]; then 213 | echo "错误: 预设的 CAN 模块数量 ($EXPECTED_CAN_COUNT) 与预定义的 USB 端口数量 ($PREDEFINED_COUNT) 不匹配。" 214 | exit 1 215 | fi 216 | 217 | # 遍历所有 CAN 接口 218 | for iface in $(ip -br link show type can | awk '{print $1}'); do 219 | # 使用 ethtool 获取 bus-info 220 | BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}') 221 | 222 | if [ -z "$BUS_INFO" ];then 223 | echo "错误: 无法获取接口 $iface 的 bus-info 信息。" 224 | continue 225 | fi 226 | 227 | echo "接口 $iface 插入在 USB 端口 $BUS_INFO" 228 | 229 | # 检查 bus-info 是否在预定义的 USB 端口列表中 230 | if [ -n "${USB_PORTS[$BUS_INFO]}" ];then 231 | IFS=':' read -r TARGET_NAME TARGET_BITRATE <<< "${USB_PORTS[$BUS_INFO]}" 232 | 233 | # 检查当前接口是否已经激活 234 | IS_LINK_UP=$(ip link show "$iface" | grep -q "UP" && echo "yes" || echo "no") 235 | 236 | # 获取当前接口的比特率 237 | CURRENT_BITRATE=$(ip -details link show "$iface" | grep -oP 'bitrate \K\d+') 238 | 239 | if [ "$IS_LINK_UP" == "yes" ] && [ "$CURRENT_BITRATE" -eq "$TARGET_BITRATE" ]; then 240 | echo "接口 $iface 已经激活，并且比特率为 $TARGET_BITRATE" 241 | 242 | # 检查接口名称是否与目标名称匹配 243 | if [ "$iface" != "$TARGET_NAME" ]; then 244 | echo "将接口 $iface 重命名为 $TARGET_NAME" 245 | sudo ip link set "$iface" down 246 | sudo ip link set "$iface" name "$TARGET_NAME" 247 | sudo ip link set "$TARGET_NAME" up 248 | echo "接口已重命名为 $TARGET_NAME，并重新激活。" 249 | else 250 | echo "接口名称已经是 $TARGET_NAME" 251 | fi 252 | else 253 | # 如果接口未激活或比特率不同，进行设置 254 | if [ "$IS_LINK_UP" == "yes" ]; then 255 | echo "接口 $iface 已经激活，但比特率为 $CURRENT_BITRATE，与设定的 $TARGET_BITRATE 不符。" 256 | else 257 | echo "接口 $iface 未激活或未设置比特率。" 258 | fi 259 | 260 | # 设置接口比特率并激活 261 | sudo ip link set "$iface" down 262 | sudo ip link set "$iface" type can bitrate $TARGET_BITRATE 263 | sudo ip link set "$iface" up 264 | echo "接口 $iface 已重新设置为比特率 $TARGET_BITRATE 并激活。" 265 | 266 | # 重命名接口为目标名称 267 | if [ "$iface" != "$TARGET_NAME" ]; then 268 | echo "将接口 $iface 重命名为 $TARGET_NAME" 269 | sudo ip link set "$iface" down 270 | sudo ip link set "$iface" name "$TARGET_NAME" 271 | sudo ip link set "$TARGET_NAME" up 272 | echo "接口已重命名为 $TARGET_NAME，并重新激活。" 273 | fi 274 | fi 275 | else 276 | echo "错误: 未知的 USB 端口 $BUS_INFO 对应接口 $iface。" 277 | exit 1 278 | fi 279 | done 280 | fi 281 | 282 | echo "所有 CAN 接口已成功重命名并激活。" 283 | -------------------------------------------------------------------------------- /src/Piper_ros/find_all_can_port.sh: -------------------------------------------------------------------------------- 1 | # 检查 ethtool 是否已安装 2 | if ! dpkg -l | grep -q "ethtool"; then 3 | echo "\e[31m错误: 系统中未检测到 ethtool。\e[0m" 4 | echo "请使用以下命令安装 ethtool:" 5 | echo "sudo apt update && sudo apt install ethtool" 6 | exit 1 7 | fi 8 | 9 | # 检查 can-utils 是否已安装 10 | if ! dpkg -l | grep -q "can-utils"; then 11 | echo "\e[31m错误: 系统中未检测到 can-utils。\e[0m" 12 | echo "请使用以下命令安装 can-utils:" 13 | echo "sudo apt update && sudo apt install can-utils" 14 | exit 1 15 | fi 16 | 17 | echo "ethtool 和 can-utils 均已安装。" 18 | 19 | # 遍历所有 CAN 接口 20 | for iface in $(ip -br link show type can | awk '{print $1}'); do 21 | # 使用 ethtool 获取 bus-info 22 | BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}') 23 | 24 | if [ -z "$BUS_INFO" ];then 25 | echo "错误: 无法获取接口 $iface 的 bus-info 信息。" 26 | continue 27 | fi 28 | 29 | echo "接口 $iface 插入在 USB 端口 $BUS_INFO" 30 | done -------------------------------------------------------------------------------- /src/Piper_ros/src/piper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(piper) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | actionlib 12 | actionlib_msgs 13 | control_msgs 14 | roscpp 15 | rospy 16 | sensor_msgs 17 | std_msgs 18 | trajectory_msgs 19 | message_generation 20 | geometry_msgs 21 | ) 22 | 23 | ## System dependencies are found with CMake's conventions 24 | # find_package(Boost REQUIRED COMPONENTS system) 25 | 26 | 27 | ## Uncomment this if the package has a setup.py. This macro ensures 28 | ## modules and global scripts declared therein get installed 29 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 30 | # catkin_python_setup() 31 | 32 | ################################################ 33 | ## Declare ROS messages, services and actions ## 34 | ################################################ 35 | 36 | ## To declare and build messages, services or actions from within this 37 | ## package, follow these steps: 38 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 39 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 40 | ## * In the file package.xml: 41 | ## * add a build_depend tag for "message_generation" 42 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 43 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 44 | ## but can be declared for certainty nonetheless: 45 | ## * add a exec_depend tag for "message_runtime" 46 | ## * In this file (CMakeLists.txt): 47 | ## * add "message_generation" and every package in MSG_DEP_SET to 48 | ## find_package(catkin REQUIRED COMPONENTS ...) 49 | ## * add "message_runtime" and every package in MSG_DEP_SET to 50 | ## catkin_package(CATKIN_DEPENDS ...) 51 | ## * uncomment the add_*_files sections below as needed 52 | ## and list every .msg/.srv/.action file to be processed 53 | ## * uncomment the generate_messages entry below 54 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 55 | 56 | ## Generate messages in the 'msg' folder 57 | # add_message_files( 58 | # FILES 59 | # Message1.msg 60 | # Message2.msg 61 | # ) 62 | 63 | ## Generate services in the 'srv' folder 64 | # add_service_files( 65 | # FILES 66 | # Service1.srv 67 | # Service2.srv 68 | # ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | # add_message_files( 78 | # FILES 79 | # AgxArmStatusMsg.msg 80 | # Joint.msg 81 | # PosCmd.msg 82 | # ) 83 | ## Generate added messages and services with any dependencies listed here 84 | # generate_messages( 85 | # DEPENDENCIES 86 | # actionlib_msgs# control_msgs# sensor_msgs# 87 | # std_msgs# trajectory_msgs 88 | # ) 89 | 90 | ################################################ 91 | ## Declare ROS dynamic reconfigure parameters ## 92 | ################################################ 93 | 94 | ## To declare and build dynamic reconfigure parameters within this 95 | ## package, follow these steps: 96 | ## * In the file package.xml: 97 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 98 | ## * In this file (CMakeLists.txt): 99 | ## * add "dynamic_reconfigure" to 100 | ## find_package(catkin REQUIRED COMPONENTS ...) 101 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 102 | ## and list every .cfg file to be processed 103 | 104 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 105 | # generate_dynamic_reconfigure_options( 106 | # cfg/DynReconf1.cfg 107 | # cfg/DynReconf2.cfg 108 | # ) 109 | 110 | ################################### 111 | ## catkin specific configuration ## 112 | ################################### 113 | ## The catkin_package macro generates cmake config files for your package 114 | ## Declare things to be passed to dependent projects 115 | ## INCLUDE_DIRS: uncomment this if your package contains header files 116 | ## LIBRARIES: libraries you create in this project that dependent projects also need 117 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 118 | ## DEPENDS: system dependencies of this project that dependent projects also need 119 | catkin_package( 120 | # INCLUDE_DIRS include 121 | # LIBRARIES piper 122 | CATKIN_DEPENDS actionlib actionlib_msgs control_msgs roscpp rospy sensor_msgs std_msgs trajectory_msgs message_generation geometry_msgs 123 | # DEPENDS system_lib 124 | ) 125 | 126 | ########### 127 | ## Build ## 128 | ########### 129 | 130 | ## Specify additional locations of header files 131 | ## Your package locations should be listed before other locations 132 | include_directories( 133 | include 134 | ${catkin_INCLUDE_DIRS} 135 | ) 136 | 137 | ## Declare a C++ library 138 | # add_library(${PROJECT_NAME} 139 | # src/${PROJECT_NAME}/piper.cpp 140 | # ) 141 | 142 | ## Add cmake target dependencies of the library 143 | ## as an example, code may need to be generated before libraries 144 | ## either from message generation or dynamic reconfigure 145 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Declare a C++ executable 148 | ## With catkin_make all packages are built within a single CMake context 149 | ## The recommended prefix ensures that target names across packages don't collide 150 | # add_executable(${PROJECT_NAME}_node src/piper_node.cpp) 151 | 152 | ## Rename C++ executable without prefix 153 | ## The above recommended prefix causes long target names, the following renames the 154 | ## target back to the shorter version for ease of user use 155 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 156 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 157 | 158 | ## Add cmake target dependencies of the executable 159 | ## same as for the library above 160 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 161 | 162 | ## Specify libraries to link a library or executable target against 163 | # target_link_libraries(${PROJECT_NAME}_node 164 | # ${catkin_LIBRARIES} 165 | # ) 166 | 167 | ############# 168 | ## Install ## 169 | ############# 170 | 171 | # all install targets should use catkin DESTINATION variables 172 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 173 | 174 | ## Mark executable scripts (Python etc.) for installation 175 | ## in contrast to setup.py, you can choose the destination 176 | # catkin_install_python(PROGRAMS 177 | # scripts/my_python_script 178 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark executables for installation 182 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 183 | # install(TARGETS ${PROJECT_NAME}_node 184 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 185 | # ) 186 | 187 | ## Mark libraries for installation 188 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 189 | # install(TARGETS ${PROJECT_NAME} 190 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 191 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 192 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 193 | # ) 194 | 195 | ## Mark cpp header files for installation 196 | # install(DIRECTORY include/${PROJECT_NAME}/ 197 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 198 | # FILES_MATCHING PATTERN "*.h" 199 | # PATTERN ".svn" EXCLUDE 200 | # ) 201 | 202 | ## Mark other files for installation (e.g. launch and bag files, etc.) 203 | # install(FILES 204 | # # myfile1 205 | # # myfile2 206 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 207 | # ) 208 | 209 | ############# 210 | ## Testing ## 211 | ############# 212 | 213 | ## Add gtest based cpp test target and link libraries 214 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_piper.cpp) 215 | # if(TARGET ${PROJECT_NAME}-test) 216 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 217 | # endif() 218 | 219 | ## Add folders to be run by python nosetests 220 | # catkin_add_nosetests(test) 221 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper/launch/start_double_piper.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper/launch/start_single_piper.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper/launch/start_single_piper_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | piper 4 | 0.0.0 5 | The piper package 6 | 7 | 8 | 9 | 10 | agilex 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | actionlib 53 | actionlib_msgs 54 | control_msgs 55 | roscpp 56 | rospy 57 | sensor_msgs 58 | std_msgs 59 | trajectory_msgs 60 | actionlib 61 | actionlib_msgs 62 | control_msgs 63 | roscpp 64 | rospy 65 | sensor_msgs 66 | std_msgs 67 | trajectory_msgs 68 | actionlib 69 | actionlib_msgs 70 | control_msgs 71 | roscpp 72 | rospy 73 | sensor_msgs 74 | std_msgs 75 | trajectory_msgs 76 | 77 | message_generation 78 | message_runtime 79 | 80 | message_generation 81 | message_runtime 82 | 83 | geometry_msgs 84 | geometry_msgs 85 | geometry_msgs 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper/scripts/piper_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy 5 | from sensor_msgs.msg import JointState 6 | from piper_msgs.msg import PosCmd 7 | from std_msgs.msg import Header 8 | 9 | # float64 x 10 | # float64 y 11 | # float64 z 12 | # float64 roll 13 | # float64 pitch 14 | # float64 yaw 15 | # float64 gripper # 单位：米 范围：0 ~ 0.08米 16 | # int32 mode1 17 | # int32 mode2 18 | 19 | class PIPER: 20 | def __init__(self): 21 | 22 | # 发布控制piper机械臂话题 23 | self.pub_descartes = rospy.Publisher('pos_cmd', PosCmd, queue_size=10) 24 | self.pub_joint = rospy.Publisher('/joint_states', JointState, queue_size=10) 25 | self.left_pub_joint = rospy.Publisher('/left_joint_states', JointState, queue_size=100) 26 | self.right_pub_joint = rospy.Publisher('/right_joint_states', JointState, queue_size=100) 27 | self.descartes_msgs = PosCmd() 28 | 29 | # self.rate = rospy.Rate(80) # 10hz 30 | 31 | def init_pose(self): 32 | joint_states_msgs = JointState() 33 | joint_states_msgs.header = Header() 34 | joint_states_msgs.header.stamp = rospy.Time.now() 35 | joint_states_msgs.name = [f'joint{i+1}' for i in range(7)] 36 | joint_states_msgs.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 37 | self.pub_joint.publish(joint_states_msgs) 38 | # self.rate.sleep() 39 | print("send joint control piper command") 40 | 41 | def left_init_pose(self): 42 | joint_states_msgs = JointState() 43 | joint_states_msgs.header = Header() 44 | joint_states_msgs.header.stamp = rospy.Time.now() 45 | joint_states_msgs.name = [f'joint{i+1}' for i in range(7)] 46 | joint_states_msgs.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 47 | self.left_pub_joint.publish(joint_states_msgs) 48 | # self.rate.sleep() 49 | print("send joint control piper command") 50 | 51 | def right_init_pose(self): 52 | joint_states_msgs = JointState() 53 | joint_states_msgs.header = Header() 54 | joint_states_msgs.header.stamp = rospy.Time.now() 55 | joint_states_msgs.name = [f'joint{i+1}' for i in range(7)] 56 | joint_states_msgs.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 57 | self.right_pub_joint.publish(joint_states_msgs) 58 | # self.rate.sleep() 59 | print("send joint control piper command") 60 | 61 | def descartes_control_piper(self,x,y,z,roll,pitch,yaw,gripper): 62 | self.descartes_msgs.x = x 63 | self.descartes_msgs.y = y 64 | self.descartes_msgs.z = z 65 | self.descartes_msgs.roll = roll 66 | self.descartes_msgs.pitch = pitch 67 | self.descartes_msgs.yaw = yaw 68 | self.descartes_msgs.gripper = gripper 69 | self.pub_descartes.publish(self.descartes_msgs) 70 | # print("send descartes control piper command") 71 | 72 | def joint_control_piper(self,j1,j2,j3,j4,j5,j6,gripper): 73 | joint_states_msgs = JointState() 74 | joint_states_msgs.header = Header() 75 | joint_states_msgs.header.stamp = rospy.Time.now() 76 | joint_states_msgs.name = [f'joint{i+1}' for i in range(7)] 77 | joint_states_msgs.position.append(j1) 78 | joint_states_msgs.position.append(j2) 79 | joint_states_msgs.position.append(j3) 80 | joint_states_msgs.position.append(j4) 81 | joint_states_msgs.position.append(j5) 82 | joint_states_msgs.position.append(j6) 83 | joint_states_msgs.position.append(gripper) 84 | self.pub_joint.publish(joint_states_msgs) 85 | # self.rate.sleep() 86 | print("send joint control piper command") 87 | 88 | def left_joint_control_piper(self,j1,j2,j3,j4,j5,j6,gripper): 89 | joint_states_msgs = JointState() 90 | joint_states_msgs.header = Header() 91 | joint_states_msgs.header.stamp = rospy.Time.now() 92 | joint_states_msgs.name = [f'joint{i+1}' for i in range(7)] 93 | joint_states_msgs.position.append(j1) 94 | joint_states_msgs.position.append(j2) 95 | joint_states_msgs.position.append(j3) 96 | joint_states_msgs.position.append(j4) 97 | joint_states_msgs.position.append(j5) 98 | joint_states_msgs.position.append(j6) 99 | joint_states_msgs.position.append(gripper) 100 | self.left_pub_joint.publish(joint_states_msgs) 101 | # self.rate.sleep() 102 | print("send joint control piper command") 103 | 104 | 105 | def right_joint_control_piper(self,j1,j2,j3,j4,j5,j6,gripper): 106 | joint_states_msgs = JointState() 107 | joint_states_msgs.header = Header() 108 | joint_states_msgs.header.stamp = rospy.Time.now() 109 | joint_states_msgs.name = [f'joint{i+1}' for i in range(7)] 110 | joint_states_msgs.position.append(j1) 111 | joint_states_msgs.position.append(j2) 112 | joint_states_msgs.position.append(j3) 113 | joint_states_msgs.position.append(j4) 114 | joint_states_msgs.position.append(j5) 115 | joint_states_msgs.position.append(j6) 116 | joint_states_msgs.position.append(gripper) 117 | self.right_pub_joint.publish(joint_states_msgs) 118 | # self.rate.sleep() 119 | print("send joint control piper command") 120 | 121 | 122 | 123 | # test code 124 | if __name__ == '__main__': 125 | # piper = PIPER() 126 | rospy.init_node('control_piper_node', anonymous=True) 127 | # piper.control_piper(0.0,0.0,0.0,0.0,0.0,0.0,0.05) 128 | # 保持节点运行并监听外部程序的调用 129 | rospy.spin() 130 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper/scripts/piper_pinocchio.py: -------------------------------------------------------------------------------- 1 | import casadi 2 | import meshcat.geometry as mg 3 | import numpy as np 4 | import pinocchio as pin 5 | import time 6 | 7 | import rospy 8 | from pinocchio import casadi as cpin 9 | from pinocchio.robot_wrapper import RobotWrapper 10 | from pinocchio.visualize import MeshcatVisualizer 11 | from tf.transformations import quaternion_from_euler, euler_from_quaternion 12 | import os 13 | import sys 14 | import cv2 15 | import threading 16 | from piper_control import PIPER 17 | from piper_msgs.msg import PosCmd 18 | 19 | piper_control = PIPER() 20 | 21 | current_dir = os.path.dirname(os.path.abspath(__file__)) 22 | parent_dir = os.path.dirname(current_dir) 23 | sys.path.append(parent_dir) 24 | 25 | 26 | class Arm_IK: 27 | def __init__(self): 28 | np.set_printoptions(precision=5, suppress=True, linewidth=200) 29 | 30 | urdf_path = '/home/agilex/piper_ws/src/piper_description/urdf/piper_description.urdf' 31 | 32 | self.robot = pin.RobotWrapper.BuildFromURDF(urdf_path) 33 | 34 | self.mixed_jointsToLockIDs = ["joint7", 35 | "joint8" 36 | ] 37 | 38 | self.reduced_robot = self.robot.buildReducedRobot( 39 | list_of_joints_to_lock=self.mixed_jointsToLockIDs, 40 | reference_configuration=np.array([0] * self.robot.model.nq), 41 | ) 42 | 43 | q = quaternion_from_euler(0, -1.57, -1.57) 44 | self.reduced_robot.model.addFrame( 45 | pin.Frame('ee', 46 | self.reduced_robot.model.getJointId('joint6'), 47 | pin.SE3( 48 | # pin.Quaternion(1, 0, 0, 0), 49 | pin.Quaternion(q[3], q[0], q[1], q[2]), 50 | np.array([0.0, 0.0, 0.0]), 51 | ), 52 | pin.FrameType.OP_FRAME) 53 | ) 54 | 55 | self.geom_model = pin.buildGeomFromUrdf(self.robot.model, urdf_path, pin.GeometryType.COLLISION) 56 | for i in range(4, 9): 57 | for j in range(0, 3): 58 | self.geom_model.addCollisionPair(pin.CollisionPair(i, j)) 59 | self.geometry_data = pin.GeometryData(self.geom_model) 60 | 61 | self.init_data = np.zeros(self.reduced_robot.model.nq) 62 | self.history_data = np.zeros(self.reduced_robot.model.nq) 63 | 64 | # # Initialize the Meshcat visualizer for visualization 65 | self.vis = MeshcatVisualizer(self.reduced_robot.model, self.reduced_robot.collision_model, self.reduced_robot.visual_model) 66 | self.vis.initViewer(open=True) 67 | self.vis.loadViewerModel("pinocchio") 68 | self.vis.displayFrames(True, frame_ids=[113, 114], axis_length=0.15, axis_width=5) 69 | self.vis.display(pin.neutral(self.reduced_robot.model)) 70 | 71 | # Enable the display of end effector target frames with short axis lengths and greater width. 72 | frame_viz_names = ['ee_target'] 73 | FRAME_AXIS_POSITIONS = ( 74 | np.array([[0, 0, 0], [1, 0, 0], 75 | [0, 0, 0], [0, 1, 0], 76 | [0, 0, 0], [0, 0, 1]]).astype(np.float32).T 77 | ) 78 | FRAME_AXIS_COLORS = ( 79 | np.array([[1, 0, 0], [1, 0.6, 0], 80 | [0, 1, 0], [0.6, 1, 0], 81 | [0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T 82 | ) 83 | axis_length = 0.1 84 | axis_width = 10 85 | for frame_viz_name in frame_viz_names: 86 | self.vis.viewer[frame_viz_name].set_object( 87 | mg.LineSegments( 88 | mg.PointsGeometry( 89 | position=axis_length * FRAME_AXIS_POSITIONS, 90 | color=FRAME_AXIS_COLORS, 91 | ), 92 | mg.LineBasicMaterial( 93 | linewidth=axis_width, 94 | vertexColors=True, 95 | ), 96 | ) 97 | ) 98 | 99 | # Creating Casadi models and data for symbolic computing 100 | self.cmodel = cpin.Model(self.reduced_robot.model) 101 | self.cdata = self.cmodel.createData() 102 | 103 | # Creating symbolic variables 104 | self.cq = casadi.SX.sym("q", self.reduced_robot.model.nq, 1) 105 | self.cTf = casadi.SX.sym("tf", 4, 4) 106 | cpin.framesForwardKinematics(self.cmodel, self.cdata, self.cq) 107 | 108 | # # Get the hand joint ID and define the error function 109 | self.gripper_id = self.reduced_robot.model.getFrameId("ee") 110 | self.error = casadi.Function( 111 | "error", 112 | [self.cq, self.cTf], 113 | [ 114 | casadi.vertcat( 115 | cpin.log6( 116 | self.cdata.oMf[self.gripper_id].inverse() * cpin.SE3(self.cTf) 117 | ).vector, 118 | ) 119 | ], 120 | ) 121 | 122 | # Defining the optimization problem 123 | self.opti = casadi.Opti() 124 | self.var_q = self.opti.variable(self.reduced_robot.model.nq) 125 | # self.var_q_last = self.opti.parameter(self.reduced_robot.model.nq) # for smooth 126 | self.param_tf = self.opti.parameter(4, 4) 127 | self.totalcost = casadi.sumsqr(self.error(self.var_q, self.param_tf)) 128 | self.regularization = casadi.sumsqr(self.var_q) 129 | # self.smooth_cost = casadi.sumsqr(self.var_q - self.var_q_last) # for smooth 130 | 131 | # Setting optimization constraints and goals 132 | self.opti.subject_to(self.opti.bounded( 133 | self.reduced_robot.model.lowerPositionLimit, 134 | self.var_q, 135 | self.reduced_robot.model.upperPositionLimit) 136 | ) 137 | # print("self.reduced_robot.model.lowerPositionLimit:", self.reduced_robot.model.lowerPositionLimit) 138 | # print("self.reduced_robot.model.upperPositionLimit:", self.reduced_robot.model.upperPositionLimit) 139 | self.opti.minimize(20 * self.totalcost + 0.01 * self.regularization) 140 | # self.opti.minimize(20 * self.totalcost + 0.01 * self.regularization + 0.1 * self.smooth_cost) # for smooth 141 | 142 | opts = { 143 | 'ipopt': { 144 | 'print_level': 0, 145 | 'max_iter': 50, 146 | 'tol': 1e-4 147 | }, 148 | 'print_time': False 149 | } 150 | self.opti.solver("ipopt", opts) 151 | 152 | def ik_fun(self, target_pose, gripper=0, motorstate=None, motorV=None): 153 | gripper = np.array([gripper/2.0, -gripper/2.0]) 154 | if motorstate is not None: 155 | self.init_data = motorstate 156 | self.opti.set_initial(self.var_q, self.init_data) 157 | 158 | self.vis.viewer['ee_target'].set_transform(target_pose) # for visualization 159 | 160 | self.opti.set_value(self.param_tf, target_pose) 161 | # self.opti.set_value(self.var_q_last, self.init_data) # for smooth 162 | 163 | try: 164 | # sol = self.opti.solve() 165 | sol = self.opti.solve_limited() 166 | sol_q = self.opti.value(self.var_q) 167 | 168 | if self.init_data is not None: 169 | max_diff = max(abs(self.history_data - sol_q)) 170 | # print("max_diff:", max_diff) 171 | self.init_data = sol_q 172 | if max_diff > 30.0/180.0*3.1415: 173 | # print("Excessive changes in joint angle:", max_diff) 174 | self.init_data = np.zeros(self.reduced_robot.model.nq) 175 | else: 176 | self.init_data = sol_q 177 | self.history_data = sol_q 178 | 179 | self.vis.display(sol_q) # for visualization 180 | 181 | if motorV is not None: 182 | v = motorV * 0.0 183 | else: 184 | v = (sol_q - self.init_data) * 0.0 185 | 186 | tau_ff = pin.rnea(self.reduced_robot.model, self.reduced_robot.data, sol_q, v, 187 | np.zeros(self.reduced_robot.model.nv)) 188 | 189 | is_collision = self.check_self_collision(sol_q, gripper) 190 | 191 | return sol_q, tau_ff, not is_collision 192 | 193 | except Exception as e: 194 | print(f"ERROR in convergence, plotting debug info.{e}") 195 | # sol_q = self.opti.debug.value(self.var_q) # return original value 196 | return sol_q, '', False 197 | 198 | def check_self_collision(self, q, gripper=np.array([0, 0])): 199 | pin.forwardKinematics(self.robot.model, self.robot.data, np.concatenate([q, gripper], axis=0)) 200 | pin.updateGeometryPlacements(self.robot.model, self.robot.data, self.geom_model, self.geometry_data) 201 | collision = pin.computeCollisions(self.geom_model, self.geometry_data, False) 202 | # print("collision:", collision) 203 | return collision 204 | 205 | def get_ik_solution(self, x,y,z,roll,pitch,yaw): 206 | 207 | q = quaternion_from_euler(roll, pitch, yaw) 208 | target = pin.SE3( 209 | pin.Quaternion(q[3], q[0], q[1], q[2]), 210 | np.array([x, y, z]), 211 | ) 212 | sol_q, tau_ff, get_result = self.ik_fun(target.homogeneous,0) 213 | # print("result:", sol_q) 214 | 215 | if get_result : 216 | piper_control.joint_control_piper(sol_q[0],sol_q[1],sol_q[2],sol_q[3],sol_q[4],sol_q[5],0) 217 | else : 218 | print("collision!!!") 219 | 220 | class C_PiperIK(): 221 | def __init__(self): 222 | rospy.init_node('inverse_solution_node', anonymous=True) 223 | # 创建Arm_IK实例 224 | self.arm_ik = Arm_IK() 225 | 226 | # 启动订阅线程 227 | sub_pos_th = threading.Thread(target=self.SubPosThread) 228 | sub_pos_th.daemon = True 229 | sub_pos_th.start() 230 | 231 | def SubPosThread(self): 232 | # 创建订阅者，监听PosCmd类型的消息 233 | rospy.Subscriber('pin_pos_cmd', PosCmd, self.pos_cmd_callback) 234 | rospy.spin() 235 | 236 | def pos_cmd_callback(self, msg): 237 | # 获取PosCmd类型消息中的数据 238 | x = msg.x 239 | y = msg.y 240 | z = msg.z 241 | roll = msg.roll 242 | pitch = msg.pitch 243 | yaw = msg.yaw 244 | 245 | # 调用Arm_IK类的逆解函数 246 | self.arm_ik.get_ik_solution(x, y, z, roll, pitch, yaw) 247 | 248 | if __name__ == "__main__": 249 | piper_ik = C_PiperIK() 250 | while True: 251 | pass 252 | 253 | 254 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(piper_description) 4 | 5 | find_package(catkin REQUIRED) 6 | 7 | catkin_package() 8 | 9 | find_package(roslaunch) 10 | 11 | foreach(dir config launch meshes urdf) 12 | install(DIRECTORY ${dir}/ 13 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 14 | endforeach(dir) 15 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/config/joint_names_agx_arm_description.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: ['', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7', 'joint8', ] 2 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/config/piper_gazebo_control.yaml: -------------------------------------------------------------------------------- 1 | piper_description: 2 | joint_state_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 200 5 | 6 | joint1_position_controller: 7 | type: position_controllers/JointPositionController 8 | joint: joint1 9 | pid: {p: 100.0, i: 0.01, d: 5.0} 10 | joint2_position_controller: 11 | type: position_controllers/JointPositionController 12 | joint: joint2 13 | pid: {p: 100.0, i: 0.01, d: 5.0} 14 | joint3_position_controller: 15 | type: position_controllers/JointPositionController 16 | joint: joint3 17 | pid: {p: 100.0, i: 0.01, d: 5.0} 18 | joint4_position_controller: 19 | type: position_controllers/JointPositionController 20 | joint: joint4 21 | pid: {p: 100.0, i: 0.01, d: 5.0} 22 | joint5_position_controller: 23 | type: position_controllers/JointPositionController 24 | joint: joint5 25 | pid: {p: 100.0, i: 0.01, d: 5.0} 26 | joint6_position_controller: 27 | type: position_controllers/JointPositionController 28 | joint: joint6 29 | pid: {p: 100.0, i: 0.01, d: 5.0} 30 | joint7_position_controller: 31 | type: position_controllers/JointPositionController 32 | joint: joint7 33 | pid: {p: 100, i: 0.001, d: 10.0} 34 | joint8_position_controller: 35 | type: position_controllers/JointPositionController 36 | joint: joint8 37 | pid: {p: 100, i: 0.001, d: 10.0} 38 | # gazebo_ros_control: 39 | # pid_gains: 40 | # joint1: 41 | # p: 0 42 | # i: 0 43 | # d: 0 44 | # joint2: 45 | # p: 0 46 | # i: 0 47 | # d: 0 48 | # joint3: 49 | # p: 0 50 | # i: 0 51 | # d: 0 52 | # joint4: 53 | # p: 0 54 | # i: 0 55 | # d: 0 56 | # joint5: 57 | # p: 0 58 | # i: 0 59 | # d: 0 60 | # joint6: 61 | # p: 0 62 | # i: 0 63 | # d: 0 64 | # joint7: 65 | # p: 0 66 | # i: 0.001 67 | # d: 0 68 | # joint8: 69 | # p: 0 70 | # i: 0.001 71 | # d: 0 -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/config/piper_no_gripper_gazebo_control.yaml: -------------------------------------------------------------------------------- 1 | piper_description: 2 | joint_state_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 200 5 | 6 | joint1_position_controller: 7 | type: position_controllers/JointPositionController 8 | joint: joint1 9 | pid: {p: 100.0, i: 0.01, d: 5.0} 10 | joint2_position_controller: 11 | type: position_controllers/JointPositionController 12 | joint: joint2 13 | pid: {p: 100.0, i: 0.01, d: 5.0} 14 | joint3_position_controller: 15 | type: position_controllers/JointPositionController 16 | joint: joint3 17 | pid: {p: 100.0, i: 0.01, d: 5.0} 18 | joint4_position_controller: 19 | type: position_controllers/JointPositionController 20 | joint: joint4 21 | pid: {p: 100.0, i: 0.01, d: 5.0} 22 | joint5_position_controller: 23 | type: position_controllers/JointPositionController 24 | joint: joint5 25 | pid: {p: 100.0, i: 0.01, d: 5.0} 26 | joint6_position_controller: 27 | type: position_controllers/JointPositionController 28 | joint: joint6 29 | pid: {p: 100.0, i: 0.01, d: 5.0} 30 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/launch/piper_no_gripper/display_no_gripper_urdf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 11 | 15 | 20 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/launch/piper_no_gripper/display_no_gripper_xacro.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 11 | 15 | 20 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/launch/piper_no_gripper/gazebo_no_gripper_xacro.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/launch/piper_no_gripper/piper_no_gripper_gazebo_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/launch/piper_no_gripper/piper_no_gripper_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/launch/piper_with_gripper/display_urdf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | 12 | 13 | 17 | 21 | 26 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/launch/piper_with_gripper/display_xacro.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 8 | 12 | 13 | 14 | 18 | 19 | 20 | 25 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/launch/piper_with_gripper/gazebo_xacro.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/launch/piper_with_gripper/piper_gazebo_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/launch/piper_with_gripper/piper_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/launch/piper_with_gripper/rviz_ctrl_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/meshes/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/src/piper_description/meshes/base_link.STL -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/meshes/gripper_base.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/src/piper_description/meshes/gripper_base.STL -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/meshes/link1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/src/piper_description/meshes/link1.STL -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/meshes/link2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/src/piper_description/meshes/link2.STL -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/meshes/link3.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/src/piper_description/meshes/link3.STL -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/meshes/link4.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/src/piper_description/meshes/link4.STL -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/meshes/link5.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/src/piper_description/meshes/link5.STL -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/meshes/link6.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/src/piper_description/meshes/link6.STL -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/meshes/link7.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/src/piper_description/meshes/link7.STL -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/meshes/link8.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/Piper_ros/src/piper_description/meshes/link8.STL -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | piper_description 3 | 1.0.0 4 | 5 |

URDF Description package for piper_description

6 |

This package contains configuration data, 3D models and launch files 7 | for piper_description robot

8 | 9 | TODO 10 | 11 | BSD 12 | catkin 13 | roslaunch 14 | robot_state_publisher 15 | rviz 16 | joint_state_publisher_gui 17 | gazebo 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/rviz/piper_ctrl.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 549 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Class: rviz/RobotModel 56 | Collision Enabled: false 57 | Enabled: true 58 | Links: 59 | All Links Enabled: true 60 | Expand Joint Details: false 61 | Expand Link Details: false 62 | Expand Tree: false 63 | Link Tree Style: Links in Alphabetic Order 64 | base_link: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | Value: true 69 | link1: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | link2: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | link3: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | link4: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | link5: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | link6: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | link7: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | link8: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | Value: true 109 | world: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | Name: RobotModel 114 | Robot Description: robot_description 115 | TF Prefix: "" 116 | Update Interval: 0 117 | Value: true 118 | Visual Enabled: true 119 | Enabled: true 120 | Global Options: 121 | Background Color: 48; 48; 48 122 | Default Light: true 123 | Fixed Frame: base_link 124 | Frame Rate: 30 125 | Name: root 126 | Tools: 127 | - Class: rviz/Interact 128 | Hide Inactive Objects: true 129 | - Class: rviz/MoveCamera 130 | - Class: rviz/Select 131 | - Class: rviz/FocusCamera 132 | - Class: rviz/Measure 133 | - Class: rviz/SetInitialPose 134 | Theta std deviation: 0.2617993950843811 135 | Topic: /initialpose 136 | X std deviation: 0.5 137 | Y std deviation: 0.5 138 | - Class: rviz/SetGoal 139 | Topic: /move_base_simple/goal 140 | - Class: rviz/PublishPoint 141 | Single click: true 142 | Topic: /clicked_point 143 | Value: true 144 | Views: 145 | Current: 146 | Class: rviz/Orbit 147 | Distance: 1.985201120376587 148 | Enable Stereo Rendering: 149 | Stereo Eye Separation: 0.05999999865889549 150 | Stereo Focal Distance: 1 151 | Swap Stereo Eyes: false 152 | Value: false 153 | Field of View: 0.7853981852531433 154 | Focal Point: 155 | X: 0 156 | Y: 0 157 | Z: 0 158 | Focal Shape Fixed Size: true 159 | Focal Shape Size: 0.05000000074505806 160 | Invert Z Axis: false 161 | Name: Current View 162 | Near Clip Distance: 0.009999999776482582 163 | Pitch: 0.785398006439209 164 | Target Frame: 165 | Yaw: 0.785398006439209 166 | Saved: ~ 167 | Window Geometry: 168 | Displays: 169 | collapsed: false 170 | Height: 846 171 | Hide Left Dock: false 172 | Hide Right Dock: false 173 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 174 | Selection: 175 | collapsed: false 176 | Time: 177 | collapsed: false 178 | Tool Properties: 179 | collapsed: false 180 | Views: 181 | collapsed: false 182 | Width: 1200 183 | X: 72 184 | Y: 27 185 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/rviz/piper_no_gripper.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 549 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Class: rviz/RobotModel 56 | Collision Enabled: false 57 | Enabled: true 58 | Links: 59 | All Links Enabled: true 60 | Expand Joint Details: false 61 | Expand Link Details: false 62 | Expand Tree: false 63 | Link Tree Style: Links in Alphabetic Order 64 | base_link: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | Value: true 69 | link1: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | link2: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | link3: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | link4: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | link5: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | world: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Name: RobotModel 99 | Robot Description: robot_description 100 | TF Prefix: "" 101 | Update Interval: 0 102 | Value: true 103 | Visual Enabled: true 104 | - Class: rviz/TF 105 | Enabled: false 106 | Filter (blacklist): "" 107 | Filter (whitelist): "" 108 | Frame Timeout: 15 109 | Frames: 110 | All Enabled: true 111 | Marker Alpha: 1 112 | Marker Scale: 1 113 | Name: TF 114 | Show Arrows: true 115 | Show Axes: true 116 | Show Names: true 117 | Tree: 118 | {} 119 | Update Interval: 0 120 | Value: false 121 | Enabled: true 122 | Global Options: 123 | Background Color: 48; 48; 48 124 | Default Light: true 125 | Fixed Frame: base_link 126 | Frame Rate: 30 127 | Name: root 128 | Tools: 129 | - Class: rviz/Interact 130 | Hide Inactive Objects: true 131 | - Class: rviz/MoveCamera 132 | - Class: rviz/Select 133 | - Class: rviz/FocusCamera 134 | - Class: rviz/Measure 135 | - Class: rviz/SetInitialPose 136 | Theta std deviation: 0.2617993950843811 137 | Topic: /initialpose 138 | X std deviation: 0.5 139 | Y std deviation: 0.5 140 | - Class: rviz/SetGoal 141 | Topic: /move_base_simple/goal 142 | - Class: rviz/PublishPoint 143 | Single click: true 144 | Topic: /clicked_point 145 | Value: true 146 | Views: 147 | Current: 148 | Class: rviz/Orbit 149 | Distance: 2.1567115783691406 150 | Enable Stereo Rendering: 151 | Stereo Eye Separation: 0.05999999865889549 152 | Stereo Focal Distance: 1 153 | Swap Stereo Eyes: false 154 | Value: false 155 | Field of View: 0.7853981852531433 156 | Focal Point: 157 | X: 0 158 | Y: 0 159 | Z: 0 160 | Focal Shape Fixed Size: true 161 | Focal Shape Size: 0.05000000074505806 162 | Invert Z Axis: false 163 | Name: Current View 164 | Near Clip Distance: 0.009999999776482582 165 | Pitch: 0.785398006439209 166 | Target Frame: 167 | Yaw: 0.785398006439209 168 | Saved: ~ 169 | Window Geometry: 170 | Displays: 171 | collapsed: false 172 | Height: 846 173 | Hide Left Dock: false 174 | Hide Right Dock: false 175 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 176 | Selection: 177 | collapsed: false 178 | Time: 179 | collapsed: false 180 | Tool Properties: 181 | collapsed: false 182 | Views: 183 | collapsed: false 184 | Width: 1200 185 | X: 60 186 | Y: 27 187 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/scripts/rviz_ctrl_gazebo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | from sensor_msgs.msg import JointState 5 | from std_msgs.msg import Float64 6 | 7 | class JointStateRelay: 8 | def __init__(self): 9 | rospy.init_node('rviz_ctrl_gazebo', anonymous=True) 10 | 11 | # 创建发布者来发布到每个关节的控制器话题 12 | self.joint1_pub = rospy.Publisher('/piper_description/joint1_position_controller/command', Float64, queue_size=2) 13 | self.joint2_pub = rospy.Publisher('/piper_description/joint2_position_controller/command', Float64, queue_size=2) 14 | self.joint3_pub = rospy.Publisher('/piper_description/joint3_position_controller/command', Float64, queue_size=2) 15 | self.joint4_pub = rospy.Publisher('/piper_description/joint4_position_controller/command', Float64, queue_size=2) 16 | self.joint5_pub = rospy.Publisher('/piper_description/joint5_position_controller/command', Float64, queue_size=2) 17 | self.joint6_pub = rospy.Publisher('/piper_description/joint6_position_controller/command', Float64, queue_size=2) 18 | self.joint7_pub = rospy.Publisher('/piper_description/joint7_position_controller/command', Float64, queue_size=2) 19 | self.joint8_pub = rospy.Publisher('/piper_description/joint8_position_controller/command', Float64, queue_size=2) 20 | 21 | # 订阅/joint_states话题 22 | rospy.Subscriber('/joint_states', JointState, self.joint_states_callback) 23 | 24 | def joint_states_callback(self, msg): 25 | # 假设joint_states中的关节名称和控制器的话题顺序一致 26 | # 如果顺序不同，需要进行映射 27 | try: 28 | # 逐个发布到相应的话题 29 | self.joint1_pub.publish(msg.position[0]) 30 | self.joint2_pub.publish(msg.position[1]) 31 | self.joint3_pub.publish(msg.position[2]) 32 | self.joint4_pub.publish(msg.position[3]) 33 | self.joint5_pub.publish(msg.position[4]) 34 | self.joint6_pub.publish(msg.position[5]) 35 | self.joint7_pub.publish(msg.position[6]) 36 | self.joint8_pub.publish(msg.position[7]) 37 | except IndexError: 38 | rospy.logerr("Received joint states do not match expected number of joints.") 39 | 40 | def run(self): 41 | rospy.spin() 42 | 43 | if __name__ == '__main__': 44 | try: 45 | relay = JointStateRelay() 46 | relay.run() 47 | except rospy.ROSInterruptException: 48 | pass 49 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/urdf/piper_description.csv: -------------------------------------------------------------------------------- 1 | Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity 2 | base_link,-0.00473641164191482,2.56829134630247E-05,0.041451518036016,0,0,0,0.333893486740112,0.000254953479570424,2.06931238580746E-07,7.30912474718368E-06,0.00027202099920186,2.65131745382267E-07,0.000276534627629623,0,0,0,0,0,0,package://piper_description/meshes/base_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://piper_description/meshes/base_link.STL,,base_link-1,base_link_axis,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, 3 | link1,0.000121504734057468,0.000104632162460536,-0.00438597309559853,0,0,0,0.215052383265765,0.000109639007860341,2.50631260865109E-07,-1.89352789149844E-07,9.95612262461418E-05,1.00634716976093E-08,0.000116363910317385,0,0,0,0,0,0,package://piper_description/meshes/link1.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://piper_description/meshes/link1.STL,,link1-1,axis1,基准轴1,joint1,revolute,0,0,0.123,0,0,0,base_link,0,0,1,100,5,-2.618,2.168,,,,,,,, 4 | link2,0.198666145229743,-0.010926924140076,0.00142121714502687,0,0,0,0.463914239236335,0.000214137415059993,7.26120579340088E-05,-9.88224861011274E-07,0.00100030277518254,-1.32818212212246E-06,0.00104417184176783,0,0,0,0,0,0,package://piper_description/meshes/link2.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://piper_description/meshes/link2.STL,,link2-1,axis2,基准轴2,joint2,revolute,0,0,0,1.5708,-0.10095,-3.1416,link1,0,0,1,100,5,0,3.14,,,,,,,, 5 | link3,-0.0202737662122021,-0.133914995944595,-0.000458682652737356,0,0,0,0.219942452993132,0.00018953849076141,-8.05719205057736E-06,5.10255053956334E-07,7.1424497082494E-05,8.89044974368937E-07,0.000201212938725775,0,0,0,0,0,0,package://piper_description/meshes/link3.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://piper_description/meshes/link3.STL,,link3-1,axis3,基准轴3,joint3,revolute,0.28503,0,0,0,0,-1.759,link2,0,0,1,100,5,-2.697,0,,,,,,,, 6 | link4,-9.66635791618542E-05,0.000876064475651083,-0.00496880904640868,0,0,0,0.131814339939458,3.96965423235175E-05,-2.32268338444837E-08,-1.14702090783249E-07,5.13319789853892E-05,9.92852686264567E-08,4.14768131680711E-05,0,0,0,0,0,0,package://piper_description/meshes/link4.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://piper_description/meshes/link4.STL,,link4-1,axis4,基准轴4,joint4,revolute,-0.021984,-0.25075,0,1.5708,0,0,link3,0,0,1,100,5,-1.832,1.832,,,,,,,, 7 | link5,-4.10554118924211E-05,-0.0566486692356075,-0.0037205791677906,0,0,0,0.134101341225523,4.10994130543451E-05,-2.06433983793957E-08,1.29591347668502E-10,5.27723004189144E-05,1.9140716904272E-07,4.60418752810541E-05,0,0,0,0,0,0,package://piper_description/meshes/link5.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://piper_description/meshes/link5.STL,,link5-1,axis5,基准轴5,joint5,revolute,0,0,0,-1.5708,0,0,link4,0,0,1,100,5,-1.22,1.22,,,,,,,, 8 | link6,-8.82590762930069E-05,9.0598378529832E-06,-0.002,0,0,0,0.00699089613564366,5.43015540542155E-07,-1.98305403089247E-22,-7.2791893904596E-23,5.43015540542155E-07,-3.4146026640245E-24,1.06738869138926E-06,0,0,0,0,0,0,package://piper_description/meshes/link6.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://piper_description/meshes/link6.STL,,link6-1,axis6,基准轴6,joint6,revolute,8.8259E-05,-0.091,0,1.5708,0,0,link5,0,0,1,100,5,-2.0944,2.0944,,,,,,,, 9 | link7,0.000651231850428001,0.0491929869132543,-0.00972258769184212,0,0,0,0.0303534920715453,1.13870358990681E-05,-4.28534109923149E-07,6.45170690452229E-08,6.26108891787295E-06,1.57290362128572E-06,1.57822030592225E-05,0,0,0,0,0,0,package://gripper_description/meshes/link7.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://gripper_description/meshes/link7.STL,,爪夹-夹子-1,坐标系2,基准轴2,joint7,prismatic,0,-0.035,0.1358,-1.5708,0,0,base_link,0,0,1,10,1,0,0.035,,,,,,,, 10 | link8,0.000651231850427983,0.0491929869132549,-0.00972258769184214,0,0,0,0.0303534920715448,1.13870358990674E-05,-4.28534109923206E-07,6.45170690452328E-08,6.26108891787295E-06,1.5729036212857E-06,1.57822030592218E-05,0,0,0,0,0,0,package://gripper_description/meshes/link8.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://gripper_description/meshes/link8.STL,,爪夹-夹子-2,坐标系3,基准轴2,joint8,prismatic,0,0.035,0.1358,-1.5708,0,-3.1416,base_link,0,0,-1,10,1,-0.035,0,,,,,,,, 11 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/urdf/piper_description.urdf: -------------------------------------------------------------------------------- 1 | 2 | 5 | 7 | 9 | 10 | 13 | 15 | 22 | 23 | 24 | 27 | 28 | 30 | 31 | 33 | 35 | 36 | 37 | 38 | 41 | 42 | 44 | 45 | 46 | 47 | 49 | 50 | 53 | 55 | 62 | 63 | 64 | 67 | 68 | 70 | 71 | 73 | 75 | 76 | 77 | 78 | 81 | 82 | 84 | 85 | 86 | 87 | 90 | 93 | 95 | 97 | 99 | 104 | 105 | 107 | 108 | 111 | 113 | 120 | 121 | 122 | 125 | 126 | 128 | 129 | 131 | 133 | 134 | 135 | 136 | 139 | 140 | 142 | 143 | 144 | 145 | 148 | 151 | 153 | 155 | 157 | 162 | 163 | 165 | 166 | 169 | 171 | 178 | 179 | 180 | 183 | 184 | 186 | 187 | 189 | 191 | 192 | 193 | 194 | 197 | 198 | 200 | 201 | 202 | 203 | 206 | 209 | 211 | 213 | 215 | 220 | 221 | 223 | 224 | 227 | 229 | 236 | 237 | 238 | 241 | 242 | 244 | 245 | 247 | 249 | 250 | 251 | 252 | 255 | 256 | 258 | 259 | 260 | 261 | 264 | 267 | 269 | 271 | 273 | 278 | 279 | 281 | 282 | 285 | 287 | 294 | 295 | 296 | 299 | 300 | 302 | 303 | 305 | 307 | 308 | 309 | 310 | 313 | 314 | 316 | 317 | 318 | 319 | 322 | 325 | 327 | 329 | 331 | 336 | 337 | 339 | 340 | 343 | 345 | 352 | 353 | 354 | 357 | 358 | 360 | 361 | 363 | 365 | 366 | 367 | 368 | 371 | 372 | 374 | 375 | 376 | 377 | 380 | 383 | 385 | 387 | 389 | 394 | 395 | 397 | 398 | 401 | 403 | 410 | 411 | 412 | 415 | 416 | 418 | 419 | 421 | 423 | 424 | 425 | 426 | 429 | 430 | 432 | 433 | 434 | 435 | 438 | 441 | 443 | 445 | 446 | 448 | 449 | 452 | 454 | 461 | 462 | 463 | 466 | 467 | 469 | 470 | 472 | 474 | 475 | 476 | 477 | 480 | 481 | 483 | 484 | 485 | 486 | 489 | 492 | 494 | 496 | 498 | 503 | 504 | 506 | 507 | 510 | 512 | 519 | 520 | 521 | 524 | 525 | 527 | 528 | 530 | 532 | 533 | 534 | 535 | 538 | 539 | 541 | 542 | 543 | 544 | 547 | 550 | 552 | 554 | 556 | 561 | 562 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/urdf/piper_no_gripper_description.csv: -------------------------------------------------------------------------------- 1 | Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity 2 | base_link,-0.00473641164191482,2.56829134630247E-05,0.041451518036016,0,0,0,0.333893486740112,0.000254953479570424,2.06931238580746E-07,7.30912474718368E-06,0.00027202099920186,2.65131745382267E-07,0.000276534627629623,0,0,0,0,0,0,package://piper_description/meshes/base_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://piper_description/meshes/base_link.STL,,base_link-1,base_link_axis,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, 3 | link1,0.000121504734057468,0.000104632162460536,-0.00438597309559853,0,0,0,0.215052383265765,0.000109639007860341,2.50631260865109E-07,-1.89352789149844E-07,9.95612262461418E-05,1.00634716976093E-08,0.000116363910317385,0,0,0,0,0,0,package://piper_description/meshes/link1.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://piper_description/meshes/link1.STL,,link1-1,axis1,基准轴1,joint1,revolute,0,0,0.123,0,0,0,base_link,0,0,1,100,5,-2.618,2.168,,,,,,,, 4 | link2,0.198666145229743,-0.010926924140076,0.00142121714502687,0,0,0,0.463914239236335,0.000214137415059993,7.26120579340088E-05,-9.88224861011274E-07,0.00100030277518254,-1.32818212212246E-06,0.00104417184176783,0,0,0,0,0,0,package://piper_description/meshes/link2.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://piper_description/meshes/link2.STL,,link2-1,axis2,基准轴2,joint2,revolute,0,0,0,1.5708,-0.10095,-3.1416,link1,0,0,1,100,5,0,3.14,,,,,,,, 5 | link3,-0.0202737662122021,-0.133914995944595,-0.000458682652737356,0,0,0,0.219942452993132,0.00018953849076141,-8.05719205057736E-06,5.10255053956334E-07,7.1424497082494E-05,8.89044974368937E-07,0.000201212938725775,0,0,0,0,0,0,package://piper_description/meshes/link3.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://piper_description/meshes/link3.STL,,link3-1,axis3,基准轴3,joint3,revolute,0.28503,0,0,0,0,-1.759,link2,0,0,1,100,5,-2.697,0,,,,,,,, 6 | link4,-9.66635791618542E-05,0.000876064475651083,-0.00496880904640868,0,0,0,0.131814339939458,3.96965423235175E-05,-2.32268338444837E-08,-1.14702090783249E-07,5.13319789853892E-05,9.92852686264567E-08,4.14768131680711E-05,0,0,0,0,0,0,package://piper_description/meshes/link4.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://piper_description/meshes/link4.STL,,link4-1,axis4,基准轴4,joint4,revolute,-0.021984,-0.25075,0,1.5708,0,0,link3,0,0,1,100,5,-1.832,1.832,,,,,,,, 7 | link5,-4.10554118924211E-05,-0.0566486692356075,-0.0037205791677906,0,0,0,0.134101341225523,4.10994130543451E-05,-2.06433983793957E-08,1.29591347668502E-10,5.27723004189144E-05,1.9140716904272E-07,4.60418752810541E-05,0,0,0,0,0,0,package://piper_description/meshes/link5.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://piper_description/meshes/link5.STL,,link5-1,axis5,基准轴5,joint5,revolute,0,0,0,-1.5708,0,0,link4,0,0,1,100,5,-1.22,1.22,,,,,,,, 8 | link6,-8.82590762930069E-05,9.0598378529832E-06,-0.002,0,0,0,0.00699089613564366,5.43015540542155E-07,-1.98305403089247E-22,-7.2791893904596E-23,5.43015540542155E-07,-3.4146026640245E-24,1.06738869138926E-06,0,0,0,0,0,0,package://piper_description/meshes/link6.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://piper_description/meshes/link6.STL,,link6-1,axis6,基准轴6,joint6,revolute,8.8259E-05,-0.091,0,1.5708,0,0,link5,0,0,1,100,5,-2.0944,2.0944,,,,,,,, -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/urdf/piper_no_gripper_description.urdf: -------------------------------------------------------------------------------- 1 | 2 | 5 | 7 | 9 | 10 | 13 | 15 | 22 | 23 | 24 | 27 | 28 | 30 | 31 | 33 | 35 | 36 | 37 | 38 | 41 | 42 | 44 | 45 | 46 | 47 | 49 | 50 | 53 | 55 | 62 | 63 | 64 | 67 | 68 | 70 | 71 | 73 | 75 | 76 | 77 | 78 | 81 | 82 | 84 | 85 | 86 | 87 | 90 | 93 | 95 | 97 | 99 | 104 | 105 | 107 | 108 | 111 | 113 | 120 | 121 | 122 | 125 | 126 | 128 | 129 | 131 | 133 | 134 | 135 | 136 | 139 | 140 | 142 | 143 | 144 | 145 | 148 | 151 | 153 | 155 | 157 | 162 | 163 | 165 | 166 | 169 | 171 | 178 | 179 | 180 | 183 | 184 | 186 | 187 | 189 | 191 | 192 | 193 | 194 | 197 | 198 | 200 | 201 | 202 | 203 | 206 | 209 | 211 | 213 | 215 | 220 | 221 | 223 | 224 | 227 | 229 | 236 | 237 | 238 | 241 | 242 | 244 | 245 | 247 | 249 | 250 | 251 | 252 | 255 | 256 | 258 | 259 | 260 | 261 | 264 | 267 | 269 | 271 | 273 | 278 | 279 | 281 | 282 | 285 | 287 | 294 | 295 | 296 | 299 | 300 | 302 | 303 | 305 | 307 | 308 | 309 | 310 | 313 | 314 | 316 | 317 | 318 | 319 | 322 | 325 | 327 | 329 | 331 | 336 | 337 | 339 | 340 | 343 | 345 | 352 | 353 | 354 | 357 | 358 | 360 | 361 | 363 | 365 | 366 | 367 | 368 | 371 | 372 | 374 | 375 | 376 | 377 | 380 | 383 | 385 | 387 | 389 | 394 | 395 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/urdf/piper_no_gripper_description.xacro: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 18 | 20 | 27 | 28 | 29 | 32 | 33 | 35 | 36 | 38 | 40 | 41 | 42 | 43 | 46 | 47 | 49 | 50 | 51 | 52 | 54 | 55 | 58 | 60 | 67 | 68 | 69 | 72 | 73 | 75 | 76 | 78 | 80 | 81 | 82 | 83 | 86 | 87 | 89 | 90 | 91 | 92 | 95 | 98 | 100 | 102 | 104 | 109 | 110 | 112 | 113 | 116 | 118 | 125 | 126 | 127 | 130 | 131 | 133 | 134 | 136 | 138 | 139 | 140 | 141 | 144 | 145 | 147 | 148 | 149 | 150 | 153 | 156 | 158 | 160 | 162 | 167 | 168 | 170 | 171 | 174 | 176 | 183 | 184 | 185 | 188 | 189 | 191 | 192 | 194 | 196 | 197 | 198 | 199 | 202 | 203 | 205 | 206 | 207 | 208 | 211 | 214 | 216 | 218 | 220 | 225 | 226 | 228 | 229 | 232 | 234 | 241 | 242 | 243 | 246 | 247 | 249 | 250 | 252 | 254 | 255 | 256 | 257 | 260 | 261 | 263 | 264 | 265 | 266 | 269 | 272 | 274 | 276 | 278 | 283 | 284 | 286 | 287 | 290 | 292 | 299 | 300 | 301 | 304 | 305 | 307 | 308 | 310 | 312 | 313 | 314 | 315 | 318 | 319 | 321 | 322 | 323 | 324 | 327 | 330 | 332 | 334 | 336 | 341 | 342 | 344 | 345 | 348 | 350 | 357 | 358 | 359 | 362 | 363 | 365 | 366 | 368 | 370 | 371 | 372 | 373 | 376 | 377 | 379 | 380 | 381 | 382 | 385 | 388 | 390 | 392 | 394 | 399 | 400 | 401 | 402 | 403 | 404 | transmission_interface/SimpleTransmission 405 | 406 | hardware_interface/PositionJointInterface 407 | 408 | 409 | hardware_interface/PositionJointInterface 410 | 1 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | /piper_description 425 | true 426 | 427 | 428 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_description/worlds/empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | 11 | model://ground_plane 12 | 13 | 14 | 15 | 16 | 0.01 17 | 1 18 | 100 19 | 0 0 -9.8 20 | 21 | 22 | quick 23 | 500 24 | 1.3 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(piper_msgs) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | actionlib 12 | actionlib_msgs 13 | control_msgs 14 | roscpp 15 | rospy 16 | sensor_msgs 17 | std_msgs 18 | trajectory_msgs 19 | message_generation 20 | geometry_msgs 21 | ) 22 | 23 | ## System dependencies are found with CMake's conventions 24 | # find_package(Boost REQUIRED COMPONENTS system) 25 | 26 | 27 | ## Uncomment this if the package has a setup.py. This macro ensures 28 | ## modules and global scripts declared therein get installed 29 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 30 | # catkin_python_setup() 31 | 32 | ################################################ 33 | ## Declare ROS messages, services and actions ## 34 | ################################################ 35 | 36 | ## To declare and build messages, services or actions from within this 37 | ## package, follow these steps: 38 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 39 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 40 | ## * In the file package.xml: 41 | ## * add a build_depend tag for "message_generation" 42 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 43 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 44 | ## but can be declared for certainty nonetheless: 45 | ## * add a exec_depend tag for "message_runtime" 46 | ## * In this file (CMakeLists.txt): 47 | ## * add "message_generation" and every package in MSG_DEP_SET to 48 | ## find_package(catkin REQUIRED COMPONENTS ...) 49 | ## * add "message_runtime" and every package in MSG_DEP_SET to 50 | ## catkin_package(CATKIN_DEPENDS ...) 51 | ## * uncomment the add_*_files sections below as needed 52 | ## and list every .msg/.srv/.action file to be processed 53 | ## * uncomment the generate_messages entry below 54 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 55 | 56 | ## Generate messages in the 'msg' folder 57 | # add_message_files( 58 | # FILES 59 | # Message1.msg 60 | # Message2.msg 61 | # ) 62 | 63 | ## Generate services in the 'srv' folder 64 | # add_service_files( 65 | # FILES 66 | # Service1.srv 67 | # Service2.srv 68 | # ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | add_message_files( 77 | FILES 78 | PiperStatusMsg.msg 79 | PosCmd.msg 80 | ) 81 | add_service_files( 82 | FILES 83 | Enable.srv 84 | Gripper.srv 85 | GoZero.srv 86 | ) 87 | ## Generate added messages and services with any dependencies listed here 88 | generate_messages( 89 | DEPENDENCIES 90 | actionlib_msgs# control_msgs# sensor_msgs# 91 | std_msgs# trajectory_msgs 92 | ) 93 | 94 | ################################################ 95 | ## Declare ROS dynamic reconfigure parameters ## 96 | ################################################ 97 | 98 | ## To declare and build dynamic reconfigure parameters within this 99 | ## package, follow these steps: 100 | ## * In the file package.xml: 101 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 102 | ## * In this file (CMakeLists.txt): 103 | ## * add "dynamic_reconfigure" to 104 | ## find_package(catkin REQUIRED COMPONENTS ...) 105 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 106 | ## and list every .cfg file to be processed 107 | 108 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 109 | # generate_dynamic_reconfigure_options( 110 | # cfg/DynReconf1.cfg 111 | # cfg/DynReconf2.cfg 112 | # ) 113 | 114 | ################################### 115 | ## catkin specific configuration ## 116 | ################################### 117 | ## The catkin_package macro generates cmake config files for your package 118 | ## Declare things to be passed to dependent projects 119 | ## INCLUDE_DIRS: uncomment this if your package contains header files 120 | ## LIBRARIES: libraries you create in this project that dependent projects also need 121 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 122 | ## DEPENDS: system dependencies of this project that dependent projects also need 123 | catkin_package( 124 | # INCLUDE_DIRS include 125 | # LIBRARIES piper 126 | CATKIN_DEPENDS actionlib actionlib_msgs control_msgs roscpp rospy sensor_msgs std_msgs trajectory_msgs message_generation geometry_msgs 127 | # DEPENDS system_lib 128 | ) 129 | 130 | ########### 131 | ## Build ## 132 | ########### 133 | 134 | ## Specify additional locations of header files 135 | ## Your package locations should be listed before other locations 136 | include_directories( 137 | include 138 | ${catkin_INCLUDE_DIRS} 139 | ) 140 | 141 | ## Declare a C++ library 142 | # add_library(${PROJECT_NAME} 143 | # src/${PROJECT_NAME}/piper.cpp 144 | # ) 145 | 146 | ## Add cmake target dependencies of the library 147 | ## as an example, code may need to be generated before libraries 148 | ## either from message generation or dynamic reconfigure 149 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Declare a C++ executable 152 | ## With catkin_make all packages are built within a single CMake context 153 | ## The recommended prefix ensures that target names across packages don't collide 154 | # add_executable(${PROJECT_NAME}_node src/piper_node.cpp) 155 | 156 | ## Rename C++ executable without prefix 157 | ## The above recommended prefix causes long target names, the following renames the 158 | ## target back to the shorter version for ease of user use 159 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 160 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 161 | 162 | ## Add cmake target dependencies of the executable 163 | ## same as for the library above 164 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 165 | 166 | ## Specify libraries to link a library or executable target against 167 | # target_link_libraries(${PROJECT_NAME}_node 168 | # ${catkin_LIBRARIES} 169 | # ) 170 | 171 | ############# 172 | ## Install ## 173 | ############# 174 | 175 | # all install targets should use catkin DESTINATION variables 176 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 177 | 178 | ## Mark executable scripts (Python etc.) for installation 179 | ## in contrast to setup.py, you can choose the destination 180 | # catkin_install_python(PROGRAMS 181 | # scripts/my_python_script 182 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 183 | # ) 184 | 185 | ## Mark executables for installation 186 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 187 | # install(TARGETS ${PROJECT_NAME}_node 188 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 189 | # ) 190 | 191 | ## Mark libraries for installation 192 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 193 | # install(TARGETS ${PROJECT_NAME} 194 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 195 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 196 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 197 | # ) 198 | 199 | ## Mark cpp header files for installation 200 | # install(DIRECTORY include/${PROJECT_NAME}/ 201 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 202 | # FILES_MATCHING PATTERN "*.h" 203 | # PATTERN ".svn" EXCLUDE 204 | # ) 205 | 206 | ## Mark other files for installation (e.g. launch and bag files, etc.) 207 | # install(FILES 208 | # # myfile1 209 | # # myfile2 210 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 211 | # ) 212 | 213 | ############# 214 | ## Testing ## 215 | ############# 216 | 217 | ## Add gtest based cpp test target and link libraries 218 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_piper.cpp) 219 | # if(TARGET ${PROJECT_NAME}-test) 220 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 221 | # endif() 222 | 223 | ## Add folders to be run by python nosetests 224 | # catkin_add_nosetests(test) 225 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_msgs/msg/PiperStatusMsg.msg: -------------------------------------------------------------------------------- 1 | uint8 ctrl_mode 2 | uint8 arm_status 3 | uint8 mode_feedback 4 | uint8 teach_status 5 | uint8 motion_status 6 | uint8 trajectory_num 7 | int64 err_code 8 | bool joint_1_angle_limit 9 | bool joint_2_angle_limit 10 | bool joint_3_angle_limit 11 | bool joint_4_angle_limit 12 | bool joint_5_angle_limit 13 | bool joint_6_angle_limit 14 | bool communication_status_joint_1 15 | bool communication_status_joint_2 16 | bool communication_status_joint_3 17 | bool communication_status_joint_4 18 | bool communication_status_joint_5 19 | bool communication_status_joint_6 20 | 21 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_msgs/msg/PosCmd.msg: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | float64 z 4 | float64 roll 5 | float64 pitch 6 | float64 yaw 7 | float64 gripper 8 | int32 mode1 9 | int32 mode2 -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | piper_msgs 4 | 0.0.0 5 | The piper_msgs package 6 | 7 | 8 | 9 | 10 | agilex 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | actionlib 53 | actionlib_msgs 54 | control_msgs 55 | roscpp 56 | rospy 57 | sensor_msgs 58 | std_msgs 59 | trajectory_msgs 60 | actionlib 61 | actionlib_msgs 62 | control_msgs 63 | roscpp 64 | rospy 65 | sensor_msgs 66 | std_msgs 67 | trajectory_msgs 68 | actionlib 69 | actionlib_msgs 70 | control_msgs 71 | roscpp 72 | rospy 73 | sensor_msgs 74 | std_msgs 75 | trajectory_msgs 76 | 77 | message_generation 78 | message_runtime 79 | 80 | message_generation 81 | message_runtime 82 | 83 | geometry_msgs 84 | geometry_msgs 85 | geometry_msgs 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_msgs/srv/Enable.srv: -------------------------------------------------------------------------------- 1 | bool enable_request # 请求消息类型为bool 2 | --- 3 | bool enable_response # 响应消息类型为bool 4 | -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_msgs/srv/GoZero.srv: -------------------------------------------------------------------------------- 1 | bool is_mit_mode 2 | --- 3 | int64 code 4 | bool status # 响应消息类型为bool -------------------------------------------------------------------------------- /src/Piper_ros/src/piper_msgs/srv/Gripper.srv: -------------------------------------------------------------------------------- 1 | float64 gripper_angle 2 | float64 gripper_effort 3 | int64 gripper_code 4 | int64 set_zero 5 | --- 6 | int64 code 7 | bool status # 响应消息类型为bool -------------------------------------------------------------------------------- /src/oculus_reader/APK/alvr_client_android.apk: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/oculus_reader/APK/alvr_client_android.apk -------------------------------------------------------------------------------- /src/oculus_reader/APK/teleop-debug.apk: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/questVR_ws/b7a97d3eaa3adfba08af0ecf132dbb88eab4e5b9/src/oculus_reader/APK/teleop-debug.apk -------------------------------------------------------------------------------- /src/oculus_reader/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(oculus_reader) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | geometry_msgs 7 | tf2_ros 8 | ) 9 | 10 | catkin_package( 11 | CATKIN_DEPENDS geometry_msgs 12 | ) 13 | -------------------------------------------------------------------------------- /src/oculus_reader/config/oculus_reader.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /TF1/Frames1 11 | Splitter Ratio: 0.5 12 | Tree Height: 866 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Class: rviz/TF 57 | Enabled: true 58 | Filter (blacklist): "" 59 | Filter (whitelist): "" 60 | Frame Timeout: 1 61 | Frames: 62 | All Enabled: true 63 | right_hand: 64 | Value: true 65 | vr_device: 66 | Value: true 67 | Marker Alpha: 1 68 | Marker Scale: 0.10000000149011612 69 | Name: TF 70 | Show Arrows: true 71 | Show Axes: true 72 | Show Names: true 73 | Tree: 74 | vr_device: 75 | right_hand: 76 | {} 77 | Update Interval: 0 78 | Value: true 79 | Enabled: true 80 | Global Options: 81 | Background Color: 48; 48; 48 82 | Default Light: true 83 | Fixed Frame: vr_device 84 | Frame Rate: 30 85 | Name: root 86 | Tools: 87 | - Class: rviz/Interact 88 | Hide Inactive Objects: true 89 | - Class: rviz/MoveCamera 90 | - Class: rviz/Select 91 | - Class: rviz/FocusCamera 92 | - Class: rviz/Measure 93 | - Class: rviz/SetInitialPose 94 | Theta std deviation: 0.2617993950843811 95 | Topic: /initialpose 96 | X std deviation: 0.5 97 | Y std deviation: 0.5 98 | - Class: rviz/SetGoal 99 | Topic: /move_base_simple/goal 100 | - Class: rviz/PublishPoint 101 | Single click: true 102 | Topic: /clicked_point 103 | Value: true 104 | Views: 105 | Current: 106 | Class: rviz/Orbit 107 | Distance: 0.48121994733810425 108 | Enable Stereo Rendering: 109 | Stereo Eye Separation: 0.05999999865889549 110 | Stereo Focal Distance: 1 111 | Swap Stereo Eyes: false 112 | Value: false 113 | Field of View: 0.7853981852531433 114 | Focal Point: 115 | X: 0 116 | Y: 0 117 | Z: 0 118 | Focal Shape Fixed Size: true 119 | Focal Shape Size: 0.05000000074505806 120 | Invert Z Axis: false 121 | Name: Current View 122 | Near Clip Distance: 0.009999999776482582 123 | Pitch: 0.5447977781295776 124 | Target Frame: 125 | Yaw: 2.668574333190918 126 | Saved: ~ 127 | Window Geometry: 128 | Displays: 129 | collapsed: false 130 | Height: 1376 131 | Hide Left Dock: false 132 | Hide Right Dock: false 133 | QMainWindow State: 000000ff00000000fd0000000400000000000001f70000045afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000045a0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f0000045afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e0000045a0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000005efc0100000002fb0000000800540069006d0065010000000000000a00000006dc00fffffffb0000000800540069006d00650100000000000004500000000000000000000006920000045a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 134 | Selection: 135 | collapsed: false 136 | Time: 137 | collapsed: false 138 | Tool Properties: 139 | collapsed: false 140 | Views: 141 | collapsed: false 142 | Width: 2560 143 | X: 0 144 | Y: 0 145 | -------------------------------------------------------------------------------- /src/oculus_reader/launch/teleop_double_piper.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/oculus_reader/launch/teleop_single_piper.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/oculus_reader/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | oculus_reader 4 | 0.0.1 5 | The oculus_reader package 6 | 7 | TODO 8 | 9 | Apache-2 10 | 11 | catkin 12 | 13 | rospy 14 | geometry_msgs 15 | tf2_ros 16 | 17 | -------------------------------------------------------------------------------- /src/oculus_reader/scripts/FPS_counter.py: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy as np 3 | 4 | 5 | class FPSCounter: 6 | def __init__(self): 7 | current_time = time.time() 8 | self.start_time_for_display = current_time 9 | self.last_time = current_time 10 | self.x = 5 # displays the frame rate every X second 11 | self.time_between_calls = [] 12 | self.elements_for_mean = 50 13 | 14 | def getAndPrintFPS(self, print_fps=True): 15 | current_time = time.time() 16 | self.time_between_calls.append(1.0/(current_time - self.last_time + 1e-9)) 17 | if len(self.time_between_calls) > self.elements_for_mean: 18 | self.time_between_calls.pop(0) 19 | self.last_time = current_time 20 | frequency = np.mean(self.time_between_calls) 21 | if (current_time - self.start_time_for_display) > self.x and print_fps: 22 | print("Frequency: {}Hz".format(int(frequency))) 23 | self.start_time_for_display = current_time 24 | return frequency -------------------------------------------------------------------------------- /src/oculus_reader/scripts/buttons_parser.py: -------------------------------------------------------------------------------- 1 | def parse_buttons(text): 2 | split_text = text.split(',') 3 | buttons = {} 4 | if 'R' in split_text: # right hand if available 5 | split_text.remove('R') # remove marker 6 | buttons.update({'A': False, 7 | 'B': False, 8 | 'RThU': False, # indicates that right thumb is up from the rest position 9 | 'RJ': False, # joystick pressed 10 | 'RG': False, # boolean value for trigger on the grip (delivered by SDK) 11 | 'RTr': False # boolean value for trigger on the index finger (delivered by SDK) 12 | }) 13 | # besides following keys are provided: 14 | # 'rightJS' / 'leftJS' - (x, y) position of joystick. x, y both in range (-1.0, 1.0) 15 | # 'rightGrip' / 'leftGrip' - float value for trigger on the grip in range (0.0, 1.0) 16 | # 'rightTrig' / 'leftTrig' - float value for trigger on the index finger in range (0.0, 1.0) 17 | 18 | if 'L' in split_text: # left hand accordingly 19 | split_text.remove('L') # remove marker 20 | buttons.update({'X': False, 'Y': False, 'LThU': False, 'LJ': False, 'LG': False, 'LTr': False}) 21 | for key in buttons.keys(): 22 | if key in list(split_text): 23 | buttons[key] = True 24 | split_text.remove(key) 25 | for elem in split_text: 26 | split_elem = elem.split(' ') 27 | if len(split_elem) < 2: 28 | continue 29 | key = split_elem[0] 30 | value = tuple([float(x) for x in split_elem[1:]]) 31 | buttons[key] = value 32 | return buttons 33 | 34 | -------------------------------------------------------------------------------- /src/oculus_reader/scripts/install.py: -------------------------------------------------------------------------------- 1 | from oculus_reader import OculusReader 2 | 3 | def main(): 4 | import argparse 5 | 6 | parser = argparse.ArgumentParser(description='Utility to manage teleoperation APK. Installs APK if no arguments are provided.') 7 | parser.add_argument("--reinstall", action="store_true", help='reinstalls APK from the default path') 8 | parser.add_argument("--uninstall", action="store_true", help='uninstalls APK') 9 | args = parser.parse_args() 10 | 11 | reader = OculusReader(run=False) 12 | 13 | if args.reinstall: 14 | reader.install(reinstall=True) 15 | elif args.uninstall: 16 | reader.uninstall() 17 | else: 18 | reader.install() 19 | print('Done.') 20 | 21 | if __name__ == "__main__": 22 | main() 23 | -------------------------------------------------------------------------------- /src/oculus_reader/scripts/oculus_reader.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | from FPS_counter import FPSCounter 4 | from buttons_parser import parse_buttons 5 | import numpy as np 6 | import threading 7 | import time 8 | import os 9 | from ppadb.client import Client as AdbClient 10 | import sys 11 | 12 | def eprint(*args, **kwargs): 13 | RED = "\033[1;31m" 14 | sys.stderr.write(RED) 15 | print(*args, file=sys.stderr, **kwargs) 16 | RESET = "\033[0;0m" 17 | sys.stderr.write(RESET) 18 | 19 | class OculusReader: 20 | def __init__(self, 21 | ip_address=None, 22 | port = 5555, 23 | APK_name='com.rail.oculus.teleop', 24 | print_FPS=False, 25 | run=True 26 | ): 27 | self.running = False 28 | self.last_transforms = {} 29 | self.last_buttons = {} 30 | self._lock = threading.Lock() 31 | self.tag = 'wE9ryARX' 32 | 33 | self.ip_address = ip_address 34 | self.port = port 35 | self.APK_name = APK_name 36 | self.print_FPS = print_FPS 37 | if self.print_FPS: 38 | self.fps_counter = FPSCounter() 39 | self.device = self.get_device() 40 | self.install(verbose=False) 41 | if run: 42 | self.run() 43 | 44 | def __del__(self): 45 | self.stop() 46 | 47 | def run(self): 48 | self.running = True 49 | self.device.shell('am start -n "com.rail.oculus.teleop/com.rail.oculus.teleop.MainActivity" -a android.intent.action.MAIN -c android.intent.category.LAUNCHER') 50 | # self.device.shell('am startservice -n "com.rail.oculus.teleop/com.rail.oculus.teleop.MainService" -a android.intent.action.MAIN -c android.intent.category.DEFAULT') 51 | self.thread = threading.Thread(target=self.device.shell, args=("logcat -T 0", self.read_logcat_by_line)) 52 | self.thread.start() 53 | 54 | def stop(self): 55 | self.running = False 56 | if hasattr(self, 'thread'): 57 | self.thread.join() 58 | 59 | def get_network_device(self, client, retry=0): 60 | try: 61 | client.remote_connect(self.ip_address, self.port) 62 | except RuntimeError: 63 | os.system('adb devices') 64 | client.remote_connect(self.ip_address, self.port) 65 | device = client.device(self.ip_address + ':' + str(self.port)) 66 | 67 | if device is None: 68 | if retry==1: 69 | os.system('adb tcpip ' + str(self.port)) 70 | if retry==2: 71 | eprint('Make sure that device is running and is available at the IP address specified as the OculusReader argument `ip_address`.') 72 | eprint('Currently provided IP address:', self.ip_address) 73 | eprint('Run `adb shell ip route` to verify the IP address.') 74 | exit(1) 75 | else: 76 | self.get_network_device(client=client, retry=retry+1) 77 | return device 78 | 79 | def get_usb_device(self, client): 80 | try: 81 | devices = client.devices() 82 | except RuntimeError: 83 | os.system('adb devices') 84 | devices = client.devices() 85 | for device in devices: 86 | if device.serial.count('.') < 3: 87 | return device 88 | eprint('Device not found. Make sure that device is running and is connected over USB') 89 | eprint('Run `adb devices` to verify that the device is visible.') 90 | exit(1) 91 | 92 | def get_device(self): 93 | # Default is "127.0.0.1" and 5037 94 | client = AdbClient(host="127.0.0.1", port=5037) 95 | if self.ip_address is not None: 96 | return self.get_network_device(client) 97 | else: 98 | return self.get_usb_device(client) 99 | 100 | def install(self, APK_path=None, verbose=True, reinstall=False): 101 | try: 102 | installed = self.device.is_installed(self.APK_name) 103 | if not installed or reinstall: 104 | if APK_path is None: 105 | APK_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'APK', 'teleop-debug.apk') 106 | success = self.device.install(APK_path, test=True, reinstall=reinstall) 107 | installed = self.device.is_installed(self.APK_name) 108 | if installed and success: 109 | print('APK installed successfully.') 110 | else: 111 | eprint('APK install failed.') 112 | elif verbose: 113 | print('APK is already installed.') 114 | except RuntimeError: 115 | eprint('Device is visible but could not be accessed.') 116 | eprint('Run `adb devices` to verify that the device is visible and accessible.') 117 | eprint('If you see "no permissions" next to the device serial, please put on the Oculus Quest and allow the access.') 118 | exit(1) 119 | 120 | def uninstall(self, verbose=True): 121 | try: 122 | installed = self.device.is_installed(self.APK_name) 123 | if installed: 124 | success = self.device.uninstall(self.APK_name) 125 | installed = self.device.is_installed(self.APK_name) 126 | if not installed and success: 127 | print('APK uninstall finished.') 128 | print('Please verify if the app disappeared from the list as described in "UNINSTALL.md".') 129 | print('For the resolution of this issue, please follow https://github.com/Swind/pure-python-adb/issues/71.') 130 | else: 131 | eprint('APK uninstall failed') 132 | elif verbose: 133 | print('APK is not installed.') 134 | except RuntimeError: 135 | eprint('Device is visible but could not be accessed.') 136 | eprint('Run `adb devices` to verify that the device is visible and accessible.') 137 | eprint('If you see "no permissions" next to the device serial, please put on the Oculus Quest and allow the access.') 138 | exit(1) 139 | 140 | @staticmethod 141 | def process_data(string): 142 | try: 143 | transforms_string, buttons_string = string.split('&') 144 | except ValueError: 145 | return None, None 146 | split_transform_strings = transforms_string.split('|') 147 | transforms = {} 148 | for pair_string in split_transform_strings: 149 | transform = np.empty((4,4)) 150 | pair = pair_string.split(':') 151 | if len(pair) != 2: 152 | continue 153 | left_right_char = pair[0] # is r or l 154 | transform_string = pair[1] 155 | values = transform_string.split(' ') 156 | c = 0 157 | r = 0 158 | count = 0 159 | for value in values: 160 | if not value: 161 | continue 162 | transform[r][c] = float(value) 163 | c += 1 164 | if c >= 4: 165 | c = 0 166 | r += 1 167 | count += 1 168 | if count == 16: 169 | transforms[left_right_char] = transform 170 | buttons = parse_buttons(buttons_string) 171 | return transforms, buttons 172 | 173 | def extract_data(self, line): 174 | output = '' 175 | if self.tag in line: 176 | try: 177 | output += line.split(self.tag + ': ')[1] 178 | except ValueError: 179 | pass 180 | return output 181 | 182 | def get_transformations_and_buttons(self): 183 | with self._lock: 184 | return self.last_transforms, self.last_buttons 185 | 186 | def read_logcat_by_line(self, connection): 187 | file_obj = connection.socket.makefile() 188 | while self.running: 189 | try: 190 | line = file_obj.readline().strip() 191 | data = self.extract_data(line) 192 | if data: 193 | transforms, buttons = OculusReader.process_data(data) 194 | with self._lock: 195 | self.last_transforms, self.last_buttons = transforms, buttons 196 | if self.print_FPS: 197 | self.fps_counter.getAndPrintFPS() 198 | except UnicodeDecodeError: 199 | pass 200 | file_obj.close() 201 | connection.close() 202 | 203 | 204 | def main(): 205 | oculus_reader = OculusReader() 206 | 207 | while True: 208 | time.sleep(0.3) 209 | # print(oculus_reader.get_transformations_and_buttons()) 210 | 211 | 212 | if __name__ == '__main__': 213 | main() 214 | -------------------------------------------------------------------------------- /src/oculus_reader/scripts/piper_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy 5 | from sensor_msgs.msg import JointState 6 | # from piper_msgs.msg import PosCmd 7 | from std_msgs.msg import Header 8 | 9 | class PIPER: 10 | def __init__(self): 11 | 12 | # 发布控制piper机械臂话题 13 | # self.pub_descartes = rospy.Publisher('pos_cmd', PosCmd, queue_size=10) 14 | self.pub_joint = rospy.Publisher('/joint_states', JointState, queue_size=1) 15 | self.left_pub_joint = rospy.Publisher('/left_joint_states', JointState, queue_size=1) 16 | self.right_pub_joint = rospy.Publisher('/right_joint_states', JointState, queue_size=1) 17 | # self.descartes_msgs = PosCmd() 18 | 19 | # self.rate = rospy.Rate(80) # 10hz 20 | 21 | def init_pose(self): 22 | joint_states_msgs = JointState() 23 | joint_states_msgs.header = Header() 24 | joint_states_msgs.header.stamp = rospy.Time.now() 25 | joint_states_msgs.name = [f'joint{i+1}' for i in range(7)] 26 | joint_states_msgs.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 27 | self.pub_joint.publish(joint_states_msgs) 28 | # self.rate.sleep() 29 | print("send joint control piper command") 30 | 31 | def left_init_pose(self): 32 | joint_states_msgs = JointState() 33 | joint_states_msgs.header = Header() 34 | joint_states_msgs.header.stamp = rospy.Time.now() 35 | joint_states_msgs.name = [f'joint{i+1}' for i in range(7)] 36 | joint_states_msgs.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 37 | self.left_pub_joint.publish(joint_states_msgs) 38 | # self.rate.sleep() 39 | print("send joint control piper command") 40 | 41 | def right_init_pose(self): 42 | joint_states_msgs = JointState() 43 | joint_states_msgs.header = Header() 44 | joint_states_msgs.header.stamp = rospy.Time.now() 45 | joint_states_msgs.name = [f'joint{i+1}' for i in range(7)] 46 | joint_states_msgs.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 47 | self.right_pub_joint.publish(joint_states_msgs) 48 | # self.rate.sleep() 49 | print("send joint control piper command") 50 | 51 | def descartes_control_piper(self,x,y,z,roll,pitch,yaw,gripper): 52 | self.descartes_msgs.x = x 53 | self.descartes_msgs.y = y 54 | self.descartes_msgs.z = z 55 | self.descartes_msgs.roll = roll 56 | self.descartes_msgs.pitch = pitch 57 | self.descartes_msgs.yaw = yaw 58 | self.descartes_msgs.gripper = gripper 59 | self.pub_descartes.publish(self.descartes_msgs) 60 | # print("send descartes control piper command") 61 | 62 | def joint_control_piper(self,j1,j2,j3,j4,j5,j6,gripper): 63 | joint_states_msgs = JointState() 64 | joint_states_msgs.header = Header() 65 | joint_states_msgs.header.stamp = rospy.Time.now() 66 | joint_states_msgs.name = [f'joint{i+1}' for i in range(7)] 67 | joint_states_msgs.position.append(j1) 68 | joint_states_msgs.position.append(j2) 69 | joint_states_msgs.position.append(j3) 70 | joint_states_msgs.position.append(j4) 71 | joint_states_msgs.position.append(j5) 72 | joint_states_msgs.position.append(j6) 73 | joint_states_msgs.position.append(gripper) 74 | self.pub_joint.publish(joint_states_msgs) 75 | # self.rate.sleep() 76 | print("send joint control piper command") 77 | 78 | def left_joint_control_piper(self,j1,j2,j3,j4,j5,j6,gripper): 79 | joint_states_msgs = JointState() 80 | joint_states_msgs.header = Header() 81 | joint_states_msgs.header.stamp = rospy.Time.now() 82 | joint_states_msgs.name = [f'joint{i+1}' for i in range(7)] 83 | joint_states_msgs.position.append(j1) 84 | joint_states_msgs.position.append(j2) 85 | joint_states_msgs.position.append(j3) 86 | joint_states_msgs.position.append(j4) 87 | joint_states_msgs.position.append(j5) 88 | joint_states_msgs.position.append(j6) 89 | joint_states_msgs.position.append(gripper) 90 | self.left_pub_joint.publish(joint_states_msgs) 91 | # self.rate.sleep() 92 | print("send joint control piper command") 93 | 94 | 95 | def right_joint_control_piper(self,j1,j2,j3,j4,j5,j6,gripper): 96 | joint_states_msgs = JointState() 97 | joint_states_msgs.header = Header() 98 | joint_states_msgs.header.stamp = rospy.Time.now() 99 | joint_states_msgs.name = [f'joint{i+1}' for i in range(7)] 100 | joint_states_msgs.position.append(j1) 101 | joint_states_msgs.position.append(j2) 102 | joint_states_msgs.position.append(j3) 103 | joint_states_msgs.position.append(j4) 104 | joint_states_msgs.position.append(j5) 105 | joint_states_msgs.position.append(j6) 106 | joint_states_msgs.position.append(gripper) 107 | self.right_pub_joint.publish(joint_states_msgs) 108 | # self.rate.sleep() 109 | print("send joint control piper command") 110 | 111 | 112 | 113 | # test code 114 | # if __name__ == '__main__': 115 | # piper = PIPER() 116 | # rospy.init_node('control_piper_node', anonymous=True) 117 | # piper.control_piper(0.0,0.0,0.0,0.0,0.0,0.0,0.05) 118 | # piper.init_pose() 119 | # 保持节点运行并监听外部程序的调用 120 | # rospy.spin() 121 | -------------------------------------------------------------------------------- /src/oculus_reader/scripts/teleop_single_piper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | import tf2_ros 4 | import rospkg 5 | import geometry_msgs.msg 6 | from tf.transformations import quaternion_from_matrix 7 | from tf.transformations import quaternion_from_euler, quaternion_from_matrix 8 | 9 | import casadi 10 | import meshcat.geometry as mg 11 | import pinocchio as pin 12 | from pinocchio import casadi as cpin 13 | from pinocchio.visualize import MeshcatVisualizer 14 | from oculus_reader import OculusReader 15 | 16 | import os 17 | import numpy as np 18 | import math 19 | 20 | from tools import MATHTOOLS 21 | from piper_control import PIPER 22 | 23 | def matrix_to_xyzrpy(matrix): 24 | x = matrix[0, 3] 25 | y = matrix[1, 3] 26 | z = matrix[2, 3] 27 | roll = math.atan2(matrix[2, 1], matrix[2, 2]) 28 | pitch = math.asin(-matrix[2, 0]) 29 | yaw = math.atan2(matrix[1, 0], matrix[0, 0]) 30 | return [x, y, z, roll, pitch, yaw] 31 | 32 | 33 | def create_transformation_matrix(x, y, z, roll, pitch, yaw): 34 | transformation_matrix = np.eye(4) 35 | A = np.cos(yaw) 36 | B = np.sin(yaw) 37 | C = np.cos(pitch) 38 | D = np.sin(pitch) 39 | E = np.cos(roll) 40 | F = np.sin(roll) 41 | DE = D * E 42 | DF = D * F 43 | transformation_matrix[0, 0] = A * C 44 | transformation_matrix[0, 1] = A * DF - B * E 45 | transformation_matrix[0, 2] = B * F + A * DE 46 | transformation_matrix[0, 3] = x 47 | transformation_matrix[1, 0] = B * C 48 | transformation_matrix[1, 1] = A * E + B * DF 49 | transformation_matrix[1, 2] = B * DE - A * F 50 | transformation_matrix[1, 3] = y 51 | transformation_matrix[2, 0] = -D 52 | transformation_matrix[2, 1] = C * F 53 | transformation_matrix[2, 2] = C * E 54 | transformation_matrix[2, 3] = z 55 | transformation_matrix[3, 0] = 0 56 | transformation_matrix[3, 1] = 0 57 | transformation_matrix[3, 2] = 0 58 | transformation_matrix[3, 3] = 1 59 | return transformation_matrix 60 | 61 | def calc_pose_incre(base_pose, pose_data): 62 | begin_matrix = create_transformation_matrix(base_pose[0], base_pose[1], base_pose[2], 63 | base_pose[3], base_pose[4], base_pose[5]) 64 | zero_matrix = create_transformation_matrix(0.19, 0.0, 0.2, 0, 0, 0) 65 | end_matrix = create_transformation_matrix(pose_data[0], pose_data[1], pose_data[2], 66 | pose_data[3], pose_data[4], pose_data[5]) 67 | result_matrix = np.dot(zero_matrix, np.dot(np.linalg.inv(begin_matrix), end_matrix)) 68 | xyzrpy = matrix_to_xyzrpy(result_matrix) 69 | return xyzrpy 70 | 71 | class Arm_IK: 72 | def __init__(self): 73 | np.set_printoptions(precision=5, suppress=True, linewidth=200) 74 | 75 | rospack = rospkg.RosPack() 76 | package_path = rospack.get_path('piper_description') 77 | 78 | urdf_path = os.path.join(package_path, 'urdf', 'piper_description.urdf') 79 | 80 | self.robot = pin.RobotWrapper.BuildFromURDF(urdf_path) 81 | 82 | self.mixed_jointsToLockIDs = ["joint7", 83 | "joint8" 84 | ] 85 | 86 | self.reduced_robot = self.robot.buildReducedRobot( 87 | list_of_joints_to_lock=self.mixed_jointsToLockIDs, 88 | reference_configuration=np.array([0] * self.robot.model.nq), 89 | ) 90 | 91 | self.first_matrix = create_transformation_matrix(0, 0, 0, 0, -1.57, 0) 92 | self.second_matrix = create_transformation_matrix(0.13, 0.0, 0.0, 0, 0, 0) #第六轴到末端夹爪坐标的变换矩阵 93 | self.last_matrix = np.dot(self.first_matrix, self.second_matrix) 94 | q = quaternion_from_matrix(self.last_matrix) 95 | self.reduced_robot.model.addFrame( 96 | pin.Frame('ee', 97 | self.reduced_robot.model.getJointId('joint6'), 98 | pin.SE3( 99 | # pin.Quaternion(1, 0, 0, 0), 100 | pin.Quaternion(q[3], q[0], q[1], q[2]), 101 | np.array([self.last_matrix[0, 3], self.last_matrix[1, 3], self.last_matrix[2, 3]]), # -y 102 | ), 103 | pin.FrameType.OP_FRAME) 104 | ) 105 | 106 | self.geom_model = pin.buildGeomFromUrdf(self.robot.model, urdf_path, pin.GeometryType.COLLISION) 107 | for i in range(4, 9): 108 | for j in range(0, 3): 109 | self.geom_model.addCollisionPair(pin.CollisionPair(i, j)) 110 | self.geometry_data = pin.GeometryData(self.geom_model) 111 | 112 | self.init_data = np.zeros(self.reduced_robot.model.nq) 113 | self.history_data = np.zeros(self.reduced_robot.model.nq) 114 | 115 | # # Initialize the Meshcat visualizer for visualization 116 | self.vis = MeshcatVisualizer(self.reduced_robot.model, self.reduced_robot.collision_model, self.reduced_robot.visual_model) 117 | self.vis.initViewer(open=True) 118 | self.vis.loadViewerModel("pinocchio") 119 | self.vis.displayFrames(True, frame_ids=[113, 114], axis_length=0.15, axis_width=5) 120 | self.vis.display(pin.neutral(self.reduced_robot.model)) 121 | 122 | # Enable the display of end effector target frames with short axis lengths and greater width. 123 | frame_viz_names = ['ee_target'] 124 | FRAME_AXIS_POSITIONS = ( 125 | np.array([[0, 0, 0], [1, 0, 0], 126 | [0, 0, 0], [0, 1, 0], 127 | [0, 0, 0], [0, 0, 1]]).astype(np.float32).T 128 | ) 129 | FRAME_AXIS_COLORS = ( 130 | np.array([[1, 0, 0], [1, 0.6, 0], 131 | [0, 1, 0], [0.6, 1, 0], 132 | [0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T 133 | ) 134 | axis_length = 0.1 135 | axis_width = 10 136 | for frame_viz_name in frame_viz_names: 137 | self.vis.viewer[frame_viz_name].set_object( 138 | mg.LineSegments( 139 | mg.PointsGeometry( 140 | position=axis_length * FRAME_AXIS_POSITIONS, 141 | color=FRAME_AXIS_COLORS, 142 | ), 143 | mg.LineBasicMaterial( 144 | linewidth=axis_width, 145 | vertexColors=True, 146 | ), 147 | ) 148 | ) 149 | 150 | # Creating Casadi models and data for symbolic computing 151 | self.cmodel = cpin.Model(self.reduced_robot.model) 152 | self.cdata = self.cmodel.createData() 153 | 154 | # Creating symbolic variables 155 | self.cq = casadi.SX.sym("q", self.reduced_robot.model.nq, 1) 156 | self.cTf = casadi.SX.sym("tf", 4, 4) 157 | cpin.framesForwardKinematics(self.cmodel, self.cdata, self.cq) 158 | 159 | # # Get the hand joint ID and define the error function 160 | self.gripper_id = self.reduced_robot.model.getFrameId("ee") 161 | self.error = casadi.Function( 162 | "error", 163 | [self.cq, self.cTf], 164 | [ 165 | casadi.vertcat( 166 | cpin.log6( 167 | self.cdata.oMf[self.gripper_id].inverse() * cpin.SE3(self.cTf) 168 | ).vector, 169 | ) 170 | ], 171 | ) 172 | 173 | # Defining the optimization problem 174 | self.opti = casadi.Opti() 175 | self.var_q = self.opti.variable(self.reduced_robot.model.nq) 176 | # self.var_q_last = self.opti.parameter(self.reduced_robot.model.nq) # for smooth 177 | self.param_tf = self.opti.parameter(4, 4) 178 | 179 | # self.totalcost = casadi.sumsqr(self.error(self.var_q, self.param_tf)) 180 | # self.regularization = casadi.sumsqr(self.var_q) 181 | 182 | error_vec = self.error(self.var_q, self.param_tf) 183 | pos_error = error_vec[:3] 184 | ori_error = error_vec[3:] 185 | weight_position = 1.0 186 | weight_orientation = 0.1 187 | self.totalcost = casadi.sumsqr(weight_position * pos_error) + casadi.sumsqr(weight_orientation * ori_error) 188 | self.regularization = casadi.sumsqr(self.var_q) 189 | 190 | # Setting optimization constraints and goals 191 | self.opti.subject_to(self.opti.bounded( 192 | self.reduced_robot.model.lowerPositionLimit, 193 | self.var_q, 194 | self.reduced_robot.model.upperPositionLimit) 195 | ) 196 | # print("self.reduced_robot.model.lowerPositionLimit:", self.reduced_robot.model.lowerPositionLimit) 197 | # print("self.reduced_robot.model.upperPositionLimit:", self.reduced_robot.model.upperPositionLimit) 198 | self.opti.minimize(20 * self.totalcost + 0.01 * self.regularization) 199 | # self.opti.minimize(20 * self.totalcost + 0.01 * self.regularization + 0.1 * self.smooth_cost) # for smooth 200 | 201 | opts = { 202 | 'ipopt': { 203 | 'print_level': 0, 204 | 'max_iter': 50, 205 | 'tol': 1e-4 206 | }, 207 | 'print_time': False 208 | } 209 | self.opti.solver("ipopt", opts) 210 | 211 | def ik_fun(self, target_pose, gripper=0, motorstate=None, motorV=None): 212 | gripper = np.array([gripper/2.0, -gripper/2.0]) 213 | if motorstate is not None: 214 | self.init_data = motorstate 215 | self.opti.set_initial(self.var_q, self.init_data) 216 | 217 | self.vis.viewer['ee_target'].set_transform(target_pose) # for visualization 218 | 219 | self.opti.set_value(self.param_tf, target_pose) 220 | # self.opti.set_value(self.var_q_last, self.init_data) # for smooth 221 | 222 | try: 223 | # sol = self.opti.solve() 224 | sol = self.opti.solve_limited() 225 | sol_q = self.opti.value(self.var_q) 226 | 227 | if self.init_data is not None: 228 | max_diff = max(abs(self.history_data - sol_q)) 229 | # print("max_diff:", max_diff) 230 | self.init_data = sol_q 231 | if max_diff > 30.0/180.0*3.1415: 232 | # print("Excessive changes in joint angle:", max_diff) 233 | self.init_data = np.zeros(self.reduced_robot.model.nq) 234 | else: 235 | self.init_data = sol_q 236 | self.history_data = sol_q 237 | 238 | self.vis.display(sol_q) # for visualization 239 | 240 | if motorV is not None: 241 | v = motorV * 0.0 242 | else: 243 | v = (sol_q - self.init_data) * 0.0 244 | 245 | tau_ff = pin.rnea(self.reduced_robot.model, self.reduced_robot.data, sol_q, v, 246 | np.zeros(self.reduced_robot.model.nv)) 247 | 248 | is_collision = self.check_self_collision(sol_q, gripper) 249 | dist = self.get_dist(sol_q, target_pose[:3, 3]) 250 | # print("dist:", dist) 251 | return sol_q, tau_ff, not is_collision 252 | 253 | except Exception as e: 254 | print(f"ERROR in convergence, plotting debug info.{e}") 255 | # sol_q = self.opti.debug.value(self.var_q) # return original value 256 | return None, '', False 257 | 258 | def check_self_collision(self, q, gripper=np.array([0, 0])): 259 | pin.forwardKinematics(self.robot.model, self.robot.data, np.concatenate([q, gripper], axis=0)) 260 | pin.updateGeometryPlacements(self.robot.model, self.robot.data, self.geom_model, self.geometry_data) 261 | collision = pin.computeCollisions(self.geom_model, self.geometry_data, False) 262 | # print("collision:", collision) 263 | return collision 264 | 265 | def get_dist(self, q, xyz): 266 | # print("q:", q) 267 | pin.forwardKinematics(self.reduced_robot.model, self.reduced_robot.data, np.concatenate([q], axis=0)) 268 | dist = math.sqrt(pow((xyz[0] - self.reduced_robot.data.oMi[6].translation[0]), 2) + pow((xyz[1] - self.reduced_robot.data.oMi[6].translation[1]), 2) + pow((xyz[2] - self.reduced_robot.data.oMi[6].translation[2]), 2)) 269 | return dist 270 | 271 | def get_pose(self, q): 272 | index = 6 273 | pin.forwardKinematics(self.reduced_robot.model, self.reduced_robot.data, np.concatenate([q], axis=0)) 274 | end_pose = create_transformation_matrix(self.reduced_robot.data.oMi[index].translation[0], self.reduced_robot.data.oMi[index].translation[1], self.reduced_robot.data.oMi[index].translation[2], 275 | math.atan2(self.reduced_robot.data.oMi[index].rotation[2, 1], self.reduced_robot.data.oMi[index].rotation[2, 2]), 276 | math.asin(-self.reduced_robot.data.oMi[index].rotation[2, 0]), 277 | math.atan2(self.reduced_robot.data.oMi[index].rotation[1, 0], self.reduced_robot.data.oMi[index].rotation[0, 0])) 278 | end_pose = np.dot(end_pose, self.last_matrix) 279 | return matrix_to_xyzrpy(end_pose) 280 | 281 | 282 | class VR: 283 | def __init__(self): 284 | rospy.init_node('oculus_reader') 285 | self.piper_control = PIPER() 286 | self.tools = MATHTOOLS() 287 | self.inverse_solution = Arm_IK() 288 | self.piper_control.init_pose() 289 | 290 | def adjustment_matrix(self,transform): 291 | 292 | if transform.shape != (4, 4): 293 | raise ValueError("Input transform must be a 4x4 numpy array.") 294 | 295 | adj_mat = np.array([ 296 | [0,0,-1,0], 297 | [-1,0,0,0], 298 | [0,1,0,0], 299 | [0,0,0,1] 300 | ]) 301 | 302 | r_adj = self.tools.xyzrpy2Mat(0,0,0, -np.pi , 0, -np.pi/2) 303 | 304 | transform = adj_mat @ transform 305 | 306 | transform = np.dot(transform, r_adj) 307 | 308 | return transform 309 | 310 | def publish_transform(self,transform, name): 311 | translation = transform[:3, 3] 312 | 313 | br = tf2_ros.TransformBroadcaster() 314 | t = geometry_msgs.msg.TransformStamped() 315 | 316 | t.header.stamp = rospy.Time.now() 317 | t.header.frame_id = 'vr_device' 318 | t.child_frame_id = name 319 | t.transform.translation.x = translation[0] 320 | t.transform.translation.y = translation[1] 321 | t.transform.translation.z = translation[2] 322 | 323 | quat = quaternion_from_matrix(transform) 324 | t.transform.rotation.x = quat[0] 325 | t.transform.rotation.y = quat[1] 326 | t.transform.rotation.z = quat[2] 327 | t.transform.rotation.w = quat[3] 328 | 329 | br.sendTransform(t) 330 | 331 | 332 | def get_ik_solution(self, x,y,z,roll,pitch,yaw,gripper,b): 333 | 334 | q = quaternion_from_euler(roll, pitch, yaw) 335 | target = pin.SE3( 336 | pin.Quaternion(q[3], q[0], q[1], q[2]), 337 | np.array([x, y, z]), 338 | ) 339 | sol_q, tau_ff, is_collision = self.inverse_solution.ik_fun(target.homogeneous,0) 340 | # print("result:", sol_q) 341 | 342 | if b : 343 | self.piper_control.joint_control_piper(sol_q[0],sol_q[1],sol_q[2],sol_q[3],sol_q[4],sol_q[5],gripper) 344 | print("ctrolling!!!") 345 | # else : 346 | # print("collision!!!") 347 | 348 | def Run(self): 349 | # 这里可选为 WIFI连接 或 USB连接 350 | # oculus_reader = OculusReader(ip_address='10.12.11.14') # WIFI连接 351 | oculus_reader = OculusReader() # USB连接 352 | 353 | 354 | rate = rospy.Rate(50) 355 | 356 | # 夹爪坐标系到到基坐标系的变换 357 | base_RR = [0.19, 0.0, 0.2, 0, 0, 0] 358 | 359 | while not rospy.is_shutdown(): 360 | rate.sleep() 361 | transformations, buttons = oculus_reader.get_transformations_and_buttons() 362 | if 'r' not in transformations : 363 | continue 364 | 365 | transformations['r'] = self.adjustment_matrix(transformations['r']) 366 | 367 | right_controller_pose = transformations['r'] 368 | 369 | self.publish_transform(right_controller_pose, 'right_hand') 370 | 371 | RR = self.tools.matrix2Pose(transformations['r']) 372 | 373 | if buttons['A'] == True : 374 | # 按下A键后，机械臂回到初始点位并且记录 右 坐标原点 375 | self.piper_control.init_pose() 376 | base_RR = self.tools.matrix2Pose(transformations['r']) 377 | 378 | RR_ = calc_pose_incre(base_RR,RR) 379 | 380 | r_gripper_value = buttons['rightTrig'][0] * 0.07 381 | # 按下B键后，开始遥操作 382 | self.get_ik_solution(RR_[0],RR_[1],RR_[2],RR_[3],RR_[4],RR_[5],r_gripper_value,buttons['B']) 383 | 384 | if __name__ == '__main__': 385 | vr = VR() 386 | vr.Run() 387 | 388 | -------------------------------------------------------------------------------- /src/oculus_reader/scripts/tools.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | from tf.transformations import (quaternion_from_matrix, euler_from_matrix, 4 | quaternion_from_euler, euler_from_quaternion) 5 | 6 | class MATHTOOLS: 7 | 8 | # 从xyzrpy创建旋转矩阵 9 | def xyzrpy2Mat(self,x, y, z, roll, pitch, yaw): 10 | transformation_matrix = np.eye(4) 11 | A = np.cos(yaw) 12 | B = np.sin(yaw) 13 | C = np.cos(pitch) 14 | D = np.sin(pitch) 15 | E = np.cos(roll) 16 | F = np.sin(roll) 17 | DE = D * E 18 | DF = D * F 19 | transformation_matrix[0, 0] = A * C 20 | transformation_matrix[0, 1] = A * DF - B * E 21 | transformation_matrix[0, 2] = B * F + A * DE 22 | transformation_matrix[0, 3] = x 23 | transformation_matrix[1, 0] = B * C 24 | transformation_matrix[1, 1] = A * E + B * DF 25 | transformation_matrix[1, 2] = B * DE - A * F 26 | transformation_matrix[1, 3] = y 27 | transformation_matrix[2, 0] = -D 28 | transformation_matrix[2, 1] = C * F 29 | transformation_matrix[2, 2] = C * E 30 | transformation_matrix[2, 3] = z 31 | transformation_matrix[3, 0] = 0 32 | transformation_matrix[3, 1] = 0 33 | transformation_matrix[3, 2] = 0 34 | transformation_matrix[3, 3] = 1 35 | return transformation_matrix 36 | 37 | # 将矩阵转换为位姿 38 | def matrix2Pose(self,transform): 39 | # 提取平移部分 x, y, z 40 | x = transform[0, 3] 41 | y = transform[1, 3] 42 | z = transform[2, 3] 43 | 44 | # 提取旋转矩阵 45 | rotation_matrix = transform[:3, :3] 46 | 47 | # 将旋转矩阵转换为欧拉角 (roll, pitch, yaw) 48 | roll, pitch, yaw = euler_from_matrix(rotation_matrix) 49 | 50 | return x, y, z, roll, pitch, yaw 51 | 52 | def mat2xyzrpy(self,matrix): 53 | x = matrix[0, 3] 54 | y = matrix[1, 3] 55 | z = matrix[2, 3] 56 | roll = math.atan2(matrix[2, 1], matrix[2, 2]) 57 | pitch = math.asin(-matrix[2, 0]) 58 | yaw = math.atan2(matrix[1, 0], matrix[0, 0]) 59 | return [x, y, z, roll, pitch, yaw] 60 | 61 | def xyzQuaternion2matrix(x, y, z, qx, qy, qz, qw): 62 | # 四元数到旋转矩阵：根据标准公式，将四元数分量转换为一个 3x3 的旋转矩阵。 63 | R = np.array([ 64 | [1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)], 65 | [2 * (qx * qy + qz * qw), 1 - 2 * (qx ** 2 + qz ** 2), 2 * (qy * qz - qx * qw)], 66 | [2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx ** 2 + qy ** 2)] 67 | ]) 68 | 69 | # 构造齐次变换矩阵：在右下角补上 1，在第四列填入位置向量 [x, y, z]。 70 | T = np.eye(4) 71 | T[:3, :3] = R 72 | T[:3, 3] = [x, y, z] 73 | 74 | return T --------------------------------------------------------------------------------