├── .gitignore ├── CHANGELOG(ZH).MD ├── CHANGELOG.MD ├── DESCRIPTION.MD ├── LICENSE ├── MANIFEST.in ├── README(ZH).MD ├── README.MD ├── asserts ├── Q&A.MD ├── V2 │ ├── CHANGELOG-V2.MD │ └── INTERFACE_V2.MD ├── can_config.MD ├── double_piper.MD └── pictures │ ├── host_computer_set_ms.png │ ├── mit_formula.png │ ├── mit_mode.png │ └── wire_connection.PNG ├── piper_sdk ├── __init__.py ├── can_activate.sh ├── can_config.sh ├── can_find_and_config.sh ├── can_muti_activate.sh ├── demo │ ├── V2 │ │ ├── README.MD │ │ ├── V2_piper_ctrl_joint_mit.py │ │ ├── V2_piper_ctrl_motor_max_spd.py │ │ ├── V2_piper_read_gripper_param_feedback.py │ │ ├── V2_piper_set_gripper_param.py │ │ ├── V2_piper_set_installation_pos.py │ │ ├── __init__.py │ │ ├── piper_ctrl_disable.py │ │ ├── piper_ctrl_enable.py │ │ ├── piper_ctrl_end_pose.py │ │ ├── piper_ctrl_go_zero.py │ │ ├── piper_ctrl_gripper.py │ │ ├── piper_ctrl_joint.py │ │ ├── piper_ctrl_line.py │ │ ├── piper_ctrl_moveC.py │ │ ├── piper_ctrl_moveJ.py │ │ ├── piper_ctrl_moveL.py │ │ ├── piper_ctrl_moveP.py │ │ ├── piper_ctrl_reset.py │ │ ├── piper_ctrl_stop.py │ │ ├── piper_read_all_fps.py │ │ ├── piper_read_arm_motor_max_acc_limit.py │ │ ├── piper_read_arm_motor_max_angle_spd.py │ │ ├── piper_read_crash_protectation.py │ │ ├── piper_read_end_pose.py │ │ ├── piper_read_firmware.py │ │ ├── piper_read_fk.py │ │ ├── piper_read_gripper_status.py │ │ ├── piper_read_high_msg.py │ │ ├── piper_read_joint_ctrl.py │ │ ├── piper_read_joint_state.py │ │ ├── piper_read_low_msg.py │ │ ├── piper_read_mode_ctrl.py │ │ ├── piper_read_mode_ctrl_canid_151.py │ │ ├── piper_read_resp_instruction.py │ │ ├── piper_read_status.py │ │ ├── piper_read_tcp_pose.py │ │ ├── piper_read_version.py │ │ ├── piper_set_can.py │ │ ├── piper_set_gripper_zero.py │ │ ├── piper_set_init_default.py │ │ ├── piper_set_joint_zero.py │ │ ├── piper_set_joint_zero_cpv.py │ │ ├── piper_set_load.py │ │ ├── piper_set_log_level.py │ │ ├── piper_set_master.py │ │ ├── piper_set_mit.py │ │ ├── piper_set_motor_angle_limit.py │ │ ├── piper_set_motor_max_acc_limit.py │ │ ├── piper_set_sdk_param.py │ │ ├── piper_set_slave.py │ │ ├── piper_test_interface_disconnect.py │ │ └── piper_test_multi_interface_instance.py │ ├── __init__.py │ ├── detect_arm.MD │ ├── detect_arm.py │ └── manage_vcan.py ├── find_all_can_port.sh ├── hardware_port │ ├── __init__.py │ ├── can_encapsulation.py │ └── can_encapsulation_v0_4_0.py ├── interface │ ├── __init__.py │ ├── interface_version.py │ ├── piper_interface.py │ └── piper_interface_v2.py ├── kinematics │ ├── __init__.py │ └── piper_fk.py ├── log │ └── __init__.py ├── piper_msgs │ ├── __init__.py │ └── msg_v2 │ │ ├── CHANGELOG.MD │ │ ├── __init__.py │ │ ├── arm_id_type_map.py │ │ ├── arm_messages.py │ │ ├── arm_msg_type.py │ │ ├── can_id.py │ │ ├── feedback │ │ ├── __init__.py │ │ ├── arm_feedback_crash_protection_rating.py │ │ ├── arm_feedback_current_end_vel_acc_param.py │ │ ├── arm_feedback_current_motor_angle_limit_max_spd.py │ │ ├── arm_feedback_current_motor_max_acc_limit.py │ │ ├── arm_feedback_end_pose.py │ │ ├── arm_feedback_gripper.py │ │ ├── arm_feedback_gripper_teaching_param.py │ │ ├── arm_feedback_high_spd.py │ │ ├── arm_feedback_joint_states.py │ │ ├── arm_feedback_joint_vel_acc.py │ │ ├── arm_feedback_low_spd.py │ │ ├── arm_feedback_set_instruction_response.py │ │ └── arm_feedback_status.py │ │ └── transmit │ │ ├── __init__.py │ │ ├── arm_circular_pattern.py │ │ ├── arm_crash_protection_rating_config.py │ │ ├── arm_end_vel_acc_param_config.py │ │ ├── arm_gripper_ctrl.py │ │ ├── arm_gripper_teaching_param_config.py │ │ ├── arm_joint_config.py │ │ ├── arm_joint_ctrl.py │ │ ├── arm_joint_mit_ctrl.py │ │ ├── arm_light_ctrl.py │ │ ├── arm_master_slave_config.py │ │ ├── arm_motion_ctrl_1.py │ │ ├── arm_motion_ctrl_2.py │ │ ├── arm_motion_ctrl_cartesian.py │ │ ├── arm_motor_angle_limit_max_spd_config.py │ │ ├── arm_motor_enable_disable.py │ │ ├── arm_param_enquiry_and_config.py │ │ ├── arm_search_motor_max_angle_spd_acc_limit.py │ │ └── arm_set_instruction_response.py ├── piper_param │ ├── __init__.py │ └── piper_param_manager.py ├── protocol │ ├── __init__.py │ ├── piper_protocol_base.py │ └── protocol_v2 │ │ ├── __init__.py │ │ └── piper_protocol_v2.py ├── pyproject.toml ├── utils │ ├── __init__.py │ ├── fps.py │ ├── logger_mag.py │ └── tf.py └── version.py ├── rm_tmp.sh └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | .VSCodeCounter 2 | .vscode 3 | *.log 4 | __pycache__ 5 | *.7z 6 | *.zip 7 | *.tar 8 | piper_sdk.egg-info 9 | dist 10 | build -------------------------------------------------------------------------------- /DESCRIPTION.MD: -------------------------------------------------------------------------------- 1 | # Piper SDK 2 | 3 | [![GitHub](https://img.shields.io/badge/GitHub-blue.svg)](https://github.com/agilexrobotics/piper_sdk) 4 | 5 | |Ubuntu |STATE| 6 | |---|---| 7 | |![ubuntu18.04](https://img.shields.io/badge/Ubuntu-18.04-orange.svg)|![Pass](https://img.shields.io/badge/Pass-blue.svg)| 8 | |![ubuntu20.04](https://img.shields.io/badge/Ubuntu-20.04-orange.svg)|![Pass](https://img.shields.io/badge/Pass-blue.svg)| 9 | |![ubuntu22.04](https://img.shields.io/badge/Ubuntu-22.04-orange.svg)|![Pass](https://img.shields.io/badge/Pass-blue.svg)| 10 | 11 | Test: 12 | 13 | |PYTHON |STATE| 14 | |---|---| 15 | |![python3.6](https://img.shields.io/badge/Python-3.6-blue.svg)|![Pass](https://img.shields.io/badge/Pass-blue.svg)| 16 | |![python3.8](https://img.shields.io/badge/Python-3.8-blue.svg)|![Pass](https://img.shields.io/badge/Pass-blue.svg)| 17 | |![python3.10](https://img.shields.io/badge/Python-3.10-blue.svg)|![Pass](https://img.shields.io/badge/Pass-blue.svg)| 18 | 19 | ## 1 Installation Instructions 20 | 21 | ### 1.1 Install dependencies 22 | 23 | The python-can version should be higher than 3.3.4. 24 | 25 | ```shell 26 | pip3 install python-can 27 | ``` 28 | 29 | ```shell 30 | pip3 install piper_sdk 31 | ``` 32 | 33 | View piper_sdk Details, Such as Installation Path, Version, etc. 34 | 35 | ```shell 36 | pip3 show piper_sdk 37 | ``` 38 | 39 | 0.0.x provides SDK support for robotic arm firmware versions prior to V1.5-2. 40 | 41 | To uninstall 42 | 43 | ```shell 44 | pip3 uninstall piper_sdk 45 | ``` 46 | 47 | ## Notes 48 | 49 | - The CAN device must be activated and the correct baud rate set before reading messages from or controlling the robotic arm. 50 | - The `C_PiperInterface` interface class can accept the activated CAN route name as a parameter when instantiated. 51 | 52 | ## Contact Us 53 | 54 | You can open an issue on GitHub. 55 | 56 | Alternatively, you can join our Discord at 57 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Agilex Robotice Co., Ltd. 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 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | include LICENSE 2 | include *.sh 3 | include *.MD 4 | include piper_sdk/*.MD 5 | include piper_sdk/*.sh 6 | include piper_sdk/LICENSE 7 | 8 | recursive-include piper_sdk/asserts * -------------------------------------------------------------------------------- /asserts/Q&A.MD: -------------------------------------------------------------------------------- 1 | # Q&A 2 | 3 | ## 1. First-time Use: Gripper/Teach Pendant Cannot Be Controlled and No Feedback 4 | 5 | Note: **This feature is not available in the V1 version interface** 6 | 7 | > The gripper/teach pendant are end effectors, hereafter referred to as end effectors. 8 | 9 | In the new version, the gripper needs to have its stroke range set at the factory. If this is not set, you will experience issues where the feedback value is always zero, and control is not possible. 10 | 11 | The V2 version interface and above should be used to set this. 12 | 13 | 1. Execute [piper_disable.py](../demo/V2/piper_disable.py) to disable the robotic arm. Note that disabling the arm will cut power to all motors immediately. 14 | 2. Execute [V2_gripper_param_config.py](../demo/V2/V2_gripper_param_config.py) to set the stroke range for the end effector. 15 | 3. Then, execute [read_gripper_status.py](../demo/V2/read_gripper_status.py) to check if the end effector provides any feedback. 16 | 4. If you find that the stroke data for the end effector is too large when closed, you can use [piper_gripper_zero_set.py](../demo/V2/piper_gripper_zero_set.py) to set the end effector’s zero position 17 | 18 | ## 2. Jetson Orin Nano cannot detect official CAN module 19 | 20 | For Jetpack 6.0 and above, you need to use the command line Jetpack and then compile the `gs_usb` module into the kernel during the kernel compilation phase. 21 | 22 | Reference Links: 23 | 24 | You need to add `CONFIG_CAN_GS_USB=m`, etc., that is, refer to the `defconfig.jetpack_5_1_2.txt` file, add what is missing in `defconfig.jetpack_6_0.txt`, complete the `gs_usb` module related to CAN, and then recompile the kernel. 25 | 26 | ## 3. Answers to questions regarding torque parameters in MIT and how to obtain motor torque 27 | 28 | ### 3.1 Torque control in MIT 29 | 30 | ![ ](./pictures/mit_mode.png) 31 | 32 | The MIT (Mixed-Input, Mixed-Input) mode enables hybrid control of torque, position, and velocity. In the diagram above, the position loop and velocity loop are connected in parallel. The output values ​​of the position and velocity loops are added to the feedforward torque `t_ff` to obtain the reference torque `T_ref`. 33 | 34 | ![ ](./pictures/mit_formula.png) 35 | 36 | Where: 37 | 38 | - `T_ref` is the reference torque, in `N·m`. 39 | - `kp` is the position gain. 40 | - `kd` is the velocity gain. 41 | - `p_des` is the desired position of the motor output shaft, in `rad`. 42 | - `θ_m` is the current position of the motor output shaft, in `rad`. 43 | - `v_des` is the desired velocity of the motor output shaft, in `rad/s`. 44 | - `dθ_m` is the current velocity of the motor output shaft, in `rad/s`. 45 | 46 | **Note: When you send a target torque, the actual torque the motor executes is 4 times the input value. For example, if you send 1N, the motor executes 4N.** 47 | 48 | ### 3.2 SDK to obtain motor feedback torque 49 | 50 | In the SDK, you can use `GetArmHighSpdInfoMsgs().motor_1.effort` to get the torque of `motor_1`, and then you need to scale it down by a factor of 1000 to get the torque in Nm. The 'x' in `motor_x` represents the nth joint. 51 | 52 | However, note that for Piper's underlying firmware versions `1.8-2` and earlier, the feedback torque requires the following handling. 53 | 54 | - J1-3 = GetArmHighSpdInfoMsgs().motor_x.effort / 4 55 | - J4-6 = GetArmHighSpdInfoMsgs().motor_x.effort 56 | 57 | ## 4. How to use the Piper SDK on a Mac to control a robotic arm or read messages from it? 58 | 59 | Version 0.6.0 and above added support for serial port CAN. A demo is shown below. 60 | 61 | Note that before running the demo, you need to grant read and write permissions to the serial port. The command in Ubuntu is: 62 | 63 | ```bash 64 | sudo chmod 777 /dev/ttyACM0 65 | ``` 66 | 67 | Note that if `can_auto_init` in the interface is `False`, you need to execute `CreateCanBus` to initialize the internal `__arm_can` before executing `ConnectPort`; otherwise, an error will occur. 68 | 69 | If using a PCIe-to-CAN or serial CAN module, you need to set `judge_flag` to `False`; otherwise, it will detect the `socketcan` module under the Linux system. 70 | 71 | The normal frame rate is around 3040 (connecting a single arm). 72 | 73 | ```python 74 | #!/usr/bin/env python3 75 | # -*-coding:utf8-*- 76 | # Read the CAN ID frame rate related to the robotic arm 77 | import time 78 | from piper_sdk import * 79 | 80 | # 测试代码 81 | if __name__ == "__main__": 82 | piper = C_PiperInterface_V2(can_auto_init=False) 83 | piper.CreateCanBus(can_name="/dev/ttyACM0", 84 | bustype="slcan", 85 | expected_bitrate=1000000, 86 | judge_flag=False 87 | ) 88 | piper.ConnectPort(piper_init=False) 89 | while(True): 90 | print(f"all_fps: {piper.GetCanFps()}") 91 | time.sleep(0.01) 92 | ``` 93 | -------------------------------------------------------------------------------- /asserts/can_config.MD: -------------------------------------------------------------------------------- 1 | # Agilex CAN Module Configuration Guide 2 | 3 | ## Install CAN Utilities 4 | 5 | ```shell 6 | sudo apt update && sudo apt install can-utils ethtool 7 | ``` 8 | 9 | These two utilities are used to configure the CAN module. 10 | 11 | If running a bash script results in `ip: command not found`, 12 | please install the `ip` command, usually with: 13 | 14 | ```shell 15 | sudo apt-get install iproute2 16 | ``` 17 | 18 | ## Quick Usage 19 | 20 | ### Enable CAN Module 21 | 22 | First, set up the shell script parameters properly. 23 | 24 | #### Single Robotic Arm 25 | 26 | ##### PC with only one USB-to-CAN module inserted 27 | 28 | - **Use the `can_activate.sh` script in this case.** 29 | 30 | Simply execute: 31 | 32 | ```bash 33 | bash can_activate.sh can0 1000000 34 | ``` 35 | 36 | ##### PC with multiple USB-to-CAN modules inserted 37 | 38 | - **Use the `can_activate.sh` script in this case.** 39 | 40 | Unplug all CAN modules. 41 | 42 | Insert only the CAN module connected to the robotic arm into the PC, then execute: 43 | 44 | ```shell 45 | sudo ethtool -i can0 | grep bus 46 | ``` 47 | 48 | Record the `bus-info` value, e.g., `1-2:1.0`. 49 | 50 | **Note:** Usually, the first inserted CAN module defaults to `can0`. If no CAN device is found, use `bash find_all_can_port.sh` to check the corresponding CAN name for the USB address. 51 | 52 | Assuming the recorded `bus-info` value is `1-2:1.0`, execute the following command to check if the CAN device is successfully activated: 53 | 54 | ```bash 55 | bash can_activate.sh can_piper 1000000 "1-2:1.0" 56 | ``` 57 | 58 | **Note:** This means that the CAN device inserted into the USB port with hardware encoding `1-2:1.0` is renamed to `can_piper`, set to a baud rate of `1000000`, and activated. 59 | 60 | Then, run `ifconfig` to check if `can_piper` is listed. If it appears, the CAN module has been set up successfully. 61 | 62 | #### Enabling Multiple CAN Modules Simultaneously 63 | 64 | - **Use the `can_config.sh` script in this case.** 65 | 66 | In `can_config.sh`, the `EXPECTED_CAN_COUNT` parameter represents the number of CAN modules to be activated. Assume this is set to `2`. 67 | 68 | First, insert one CAN module into the PC and execute: 69 | 70 | ```shell 71 | sudo ethtool -i can0 | grep bus 72 | ``` 73 | 74 | Record the `bus-info` value, e.g., `1-2:1.0`. 75 | 76 | Next, insert another CAN module (ensure it is plugged into a different USB port than the previous one), then execute: 77 | 78 | ```shell 79 | sudo ethtool -i can1 | grep bus 80 | ``` 81 | 82 | **Note:** Typically, the first inserted CAN module will be `can0`, and the second will be `can1`. If no CAN device is found, use `bash find_all_can_port.sh` to check the corresponding CAN name for the USB address. 83 | 84 | Assume the recorded `bus-info` values are `1-2:1.0` and `1-4:1.0`. 85 | 86 | Modify the script accordingly: 87 | 88 | Replace: 89 | 90 | ```shell 91 | USB_PORTS["1-9:1.0"]="can_left:1000000" 92 | ``` 93 | 94 | with: 95 | 96 | ```shell 97 | USB_PORTS["1-2:1.0"]="can_left:1000000" 98 | ``` 99 | 100 | Similarly, replace: 101 | 102 | ```shell 103 | USB_PORTS["1-5:1.0"]="can_right:1000000" 104 | ``` 105 | 106 | with: 107 | 108 | ```shell 109 | USB_PORTS["1-4:1.0"]="can_right:1000000" 110 | ``` 111 | 112 | **Note:** This means that the CAN device inserted into the USB port with hardware encoding `1-2:1.0` is renamed to `can_left`, set to a baud rate of `1000000`, and activated. 113 | 114 | Then, execute: 115 | 116 | ```bash 117 | bash can_config.sh 118 | ``` 119 | 120 | Check the terminal output to verify if activation was successful. 121 | 122 | Finally, run `ifconfig` to check if `can_left` and `can_right` appear. If they do, the CAN modules have been successfully set up. 123 | -------------------------------------------------------------------------------- /asserts/double_piper.MD: -------------------------------------------------------------------------------- 1 | # Use two arm to remote control 2 | 3 | ## 1 Hardware connection 4 | 5 | - Schematic diagram 6 | 7 | ![ ](../asserts/pictures/wire_connection.PNG) 8 | 9 | 1. Set one of the robot arms as the active arm and the other as the slave arm. There are two ways to set it: use the host computer or use the SDK 10 | 1. Use the host computer: 11 | ![ ](./pictures/host_computer_set_ms.png) 12 | 2. Use the sdk: 13 | 14 | ```python 15 | # Set the robot arm as the master arm 16 | from piper_sdk import * 17 | C_PiperInterface(can_name='can0', judge_flag=True).MasterSlaveConfig(0xFA, 0, 0, 0) 18 | ``` 19 | 20 | ```python 21 | # Set the robot arm as a slave arm 22 | from piper_sdk import * 23 | C_PiperInterface(can_name='can0', judge_flag=True).MasterSlaveConfig(0xFC, 0, 0, 0) 24 | ``` 25 | 26 | 2. Connect the two robotic arms to a CAN module, and connect the CANs of the master and slave arms. 27 | 3. Power on the slave arm first, then the master arm, wait a few seconds, and then you can perform remote control. 28 | 4. If it fails, check whether the CAN line is connected properly, and try to power it off and on again. 29 | 30 | ## 2 Read the master-slave data 31 | 32 | - Master-slave arm control principle: 33 | 34 | The master arm sends control instructions with canid 0x155, 0x156, 0x157, 0x159, and 0x151, sets the slave arm to joint control mode and high follow mode, and the slave arm receives the mode control instructions and target joint angles sent by the master arm and moves. 35 | 36 | See the interface description in the SDK for details. 37 | 38 | [Interface_V1 README](./V1/INTERFACE_V1.MD)
[Interface_V2 README](./V2/INTERFACE_V2.MD) 39 | 40 | Here is just a brief description 41 | 42 | ### 2.1 Reading Master Arm Messages 43 | 44 | The message reporting of the master and slave arms has the following characteristics: 45 | 46 | ● Master arm: only sends control frame messages, canid 0x155, 0x156, 0x157, 0x159, 0x151. 47 | 48 | ● Slave arm: all feedback messages except control messages. 49 | 50 | The functions `GetArmJointMsgs` and `GetArmGripperMsgs` are used to obtain the joint feedback message and gripper feedback message sent by the slave robot. The joint unit is 0.001 degrees and the gripper distance unit is µm. 51 | 52 | The functions `GetArmJointCtrl` and `GetArmGripperCtrl` are used to obtain the joint control message and gripper control message sent by the active robot arm. The joint unit is 0.001 degree and the gripper distance unit is µm. 53 | 54 | ### 2.2 Control slave arm 55 | 56 | Pay attention to the slave arm mode here. If you switch from the master-slave arm control mode to the position speed control mode, the robot arm needs to be reset before controlling it; if you still use the high follow mode when controlling the slave arm, you need to set the is_mit_mode parameter of MotionCtrl_2 to 0xAD. 57 | 58 | `MotionCtrl_2` is used to set the control command mode, taking joint mode as an example. 59 | 60 | ● Set the robot position speed control mode + joint mode and enable 61 | 62 | ```python 63 | piper = C_PiperInterface() 64 | piper.ConnectPort() 65 | # reset arm 66 | piper.MotionCtrl_1(0x02,0,0) 67 | # 0x01 Set the can command control mode 68 | # 0x01 Set the joint control mode to Joint control 69 | # 50 Set the robot arm movement speed ratio 70 | piper.MotionCtrl_2(0x01, 0x01, 50) 71 | piper.EnableArm() 72 | # Later call joint control and gripper control 73 | # ... 74 | ``` 75 | 76 | ● Set the robot arm to high following mode 77 | 78 | ```python 79 | piper = C_PiperInterface() 80 | piper.ConnectPort() 81 | # 0x01 sets the can command control mode 82 | # 0x01 sets the joint control mode to Joint control 83 | # The speed ratio is invalid in high follow mode 84 | # 0xAD sets the robot arm to high follow mode 85 | piper.MotionCtrl_2(0x01, 0x01, 100, 0xAD) 86 | piper.EnableArm() 87 | # Later call joint control and gripper control 88 | # ... 89 | ``` 90 | 91 | `JointCtrl` is used to control six joints. The robot arm needs to be set to joint control mode in advance for this to take effect. 92 | 93 | Note that the unit is 0.001 degrees. For example, if you want to control joint 2 to move to 5 degrees and other joints to move to 0 degrees. 94 | 95 | ```python 96 | joint_1 = joint_3 = joint_4 = joint_5 = joint_6 97 | joint_2 = int(round(5.0 * 1000)) 98 | piper.JointCtrl(joint_1, joint_2, joint_3, joint_4, joint_5, joint_6) 99 | ``` 100 | 101 | `GripperCtrl` is used to control the gripper. 102 | 103 | For example, set the gripper torque to 1N/m and enable the gripper, then control the gripper to a position of 0.05m. 104 | 105 | ```python 106 | range = int(0.05 * 1e6) 107 | piper.GripperCtrl(abs(range), 1000, 0x01, 0) 108 | ``` 109 | 110 | Note!!!: **When controlling the slave arm, the master arm and the slave arm need to be disconnected. You can unplug the master arm's aviation plug and then run the slave arm control program. Otherwise, even if the program is executed, the slave arm will be out of control because the master arm is always sending slave arm control commands** 111 | -------------------------------------------------------------------------------- /asserts/pictures/host_computer_set_ms.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/piper_sdk/081e7c588e5b79eeaefa67a0469bcc701c81014f/asserts/pictures/host_computer_set_ms.png -------------------------------------------------------------------------------- /asserts/pictures/mit_formula.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/piper_sdk/081e7c588e5b79eeaefa67a0469bcc701c81014f/asserts/pictures/mit_formula.png -------------------------------------------------------------------------------- /asserts/pictures/mit_mode.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/piper_sdk/081e7c588e5b79eeaefa67a0469bcc701c81014f/asserts/pictures/mit_mode.png -------------------------------------------------------------------------------- /asserts/pictures/wire_connection.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/piper_sdk/081e7c588e5b79eeaefa67a0469bcc701c81014f/asserts/pictures/wire_connection.PNG -------------------------------------------------------------------------------- /piper_sdk/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | from .hardware_port import * 3 | from .utils import * 4 | from .protocol import * 5 | from .piper_msgs.msg_v2 import * 6 | from .protocol.protocol_v2 import * 7 | from .interface import * 8 | from .kinematics.piper_fk import C_PiperForwardKinematics 9 | from .version import PiperSDKVersion 10 | 11 | __all__ = [ 12 | 'C_PiperParserBase', 13 | 'C_FPSCounter', 14 | 'LogManager', 15 | 'LogLevel', 16 | 'C_PiperForwardKinematics', 17 | 'C_STD_CAN', 18 | 'C_PiperInterface', 19 | 'C_PiperInterface_V2', 20 | 'PiperSDKVersion', 21 | 'quat_convert_euler', 22 | 'euler_convert_quat', 23 | 'ArmMsgFeedbackStatusEnum', 24 | ] 25 | -------------------------------------------------------------------------------- /piper_sdk/can_activate.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # The default CAN name can be set by the user via command-line parameters. 4 | DEFAULT_CAN_NAME="${1:-can0}" 5 | 6 | # The default bitrate for a single CAN module can be set by the user via command-line parameters. 7 | DEFAULT_BITRATE="${2:-1000000}" 8 | 9 | # USB hardware address (optional parameter) 10 | USB_ADDRESS="${3}" 11 | echo "-------------------START-----------------------" 12 | # Check if ethtool is installed. 13 | if ! dpkg -l | grep -q "ethtool"; then 14 | echo "Error: ethtool not detected in the system." 15 | echo "Please use the following command to install ethtool:" 16 | echo "sudo apt update && sudo apt install ethtool" 17 | exit 1 18 | fi 19 | 20 | # Check if can-utils is installed. 21 | if ! dpkg -l | grep -q "can-utils"; then 22 | echo "Error: can-utils not detected in the system." 23 | echo "Please use the following command to install ethtool:" 24 | echo "sudo apt update && sudo apt install can-utils" 25 | exit 1 26 | fi 27 | 28 | echo "Both ethtool and can-utils are installed." 29 | 30 | # Retrieve the number of CAN modules in the current system. 31 | CURRENT_CAN_COUNT=$(ip link show type can | grep -c "link/can") 32 | 33 | # Verify if the number of CAN modules in the current system matches the expected value. 34 | if [ "$CURRENT_CAN_COUNT" -ne "1" ]; then 35 | if [ -z "$USB_ADDRESS" ]; then 36 | # Iterate through all CAN interfaces. 37 | for iface in $(ip -br link show type can | awk '{print $1}'); do 38 | # Use ethtool to retrieve bus-info. 39 | BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}') 40 | 41 | if [ -z "$BUS_INFO" ];then 42 | echo "Error: Unable to retrieve bus-info for interface $iface." 43 | continue 44 | fi 45 | 46 | echo "Interface $iface is inserted into USB port $BUS_INFO" 47 | done 48 | echo -e " Error: The number of CAN modules detected by the system ($CURRENT_CAN_COUNT) does not match the expected number (1). " 49 | echo -e " Please add the USB hardware address parameter, such as: " 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 | # Load the gs_usb module. 57 | # sudo modprobe gs_usb 58 | # if [ $? -ne 0 ]; then 59 | # echo "Error: Unable to load the gs_usb module." 60 | # exit 1 61 | # fi 62 | 63 | if [ -n "$USB_ADDRESS" ]; then 64 | echo "Detected USB hardware address parameter: $USB_ADDRESS" 65 | 66 | # Use ethtool to find the CAN interface corresponding to the USB hardware address. 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 "Error: Unable to find CAN interface corresponding to USB hardware address $USB_ADDRESS." 78 | exit 1 79 | else 80 | echo "Found the interface corresponding to USB hardware address $USB_ADDRESS: $INTERFACE_NAME." 81 | fi 82 | else 83 | # Retrieve the unique CAN interface. 84 | INTERFACE_NAME=$(ip -br link show type can | awk '{print $1}') 85 | 86 | # Check if the interface name has been retrieved. 87 | if [ -z "$INTERFACE_NAME" ]; then 88 | echo "Error: Unable to detect CAN interface." 89 | exit 1 90 | fi 91 | BUS_INFO=$(sudo ethtool -i "$INTERFACE_NAME" | grep "bus-info" | awk '{print $2}') 92 | echo "Expected to configure a single CAN module, detected interface $INTERFACE_NAME with corresponding USB address $BUS_INFO." 93 | fi 94 | 95 | # Check if the current interface is already activated. 96 | IS_LINK_UP=$(ip link show "$INTERFACE_NAME" | grep -q "UP" && echo "yes" || echo "no") 97 | 98 | # Retrieve the bitrate of the current interface. 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 $INTERFACE_NAME is already activated with a bitrate of $DEFAULT_BITRATE." 103 | 104 | # Check if the interface name matches the default name. 105 | if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then 106 | echo "Rename interface $INTERFACE_NAME to $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 "The interface has been renamed to $DEFAULT_CAN_NAME and reactivated." 111 | else 112 | echo "The interface name is already $DEFAULT_CAN_NAME." 113 | fi 114 | else 115 | # If the interface is not activated or the bitrate is different, configure it. 116 | if [ "$IS_LINK_UP" = "yes" ]; then 117 | echo "Interface $INTERFACE_NAME is already activated, but the bitrate is $CURRENT_BITRATE, which does not match the set value of $DEFAULT_BITRATE." 118 | else 119 | echo "Interface $INTERFACE_NAME is not activated or bitrate is not set." 120 | fi 121 | 122 | # Set the interface bitrate and activate it. 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 $INTERFACE_NAME has been reset to bitrate $DEFAULT_BITRATE and activated." 127 | 128 | # Rename the interface to the default name. 129 | if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then 130 | echo "Rename interface $INTERFACE_NAME to $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 "The interface has been renamed to $DEFAULT_CAN_NAME and reactivated." 135 | fi 136 | fi 137 | 138 | echo "-------------------OVER------------------------" 139 | -------------------------------------------------------------------------------- /piper_sdk/can_find_and_config.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Usage: ./check_and_setup_can.sh 4 | 5 | # 默认参数 6 | CAN_NAME="${1:-can0}" 7 | BITRATE="${2:-1000000}" 8 | USB_ADDRESS="${3}" 9 | TIMEOUT="${4:-120}" # 超时时间，默认 120 秒 10 | 11 | sudo -v 12 | 13 | ROOT="$(dirname "$(readlink -f "$0")")" 14 | 15 | if [ -z "$USB_ADDRESS" ]; then 16 | echo "错误: 需要提供 USB 硬件地址。" 17 | exit 1 18 | fi 19 | 20 | # 开始时间 21 | START_TIME=$(date +%s) 22 | 23 | # 超时标志 24 | TIMED_OUT=false 25 | 26 | echo "开始检查 USB 硬件地址 $USB_ADDRESS 是否有 CAN 设备..." 27 | 28 | while true; do 29 | # 获取当前时间 30 | CURRENT_TIME=$(date +%s) 31 | ELAPSED_TIME=$((CURRENT_TIME - START_TIME)) 32 | 33 | # 检查是否超时 34 | if [ "$ELAPSED_TIME" -ge "$TIMEOUT" ]; then 35 | echo "超时: 在 $TIMEOUT 秒内未找到 CAN 设备。" 36 | TIMED_OUT=true 37 | break 38 | fi 39 | 40 | # 查找是否有设备连接在指定的 USB 硬件地址 41 | DEVICE_FOUND=false 42 | for iface in $(ip -br link show type can | awk '{print $1}'); do 43 | BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}') 44 | if [ "$BUS_INFO" = "$USB_ADDRESS" ]; then 45 | DEVICE_FOUND=true 46 | break 47 | fi 48 | done 49 | 50 | if [ "$DEVICE_FOUND" = "true" ]; then 51 | echo "找到 CAN 设备，调用配置脚本..." 52 | sudo bash $ROOT/can_activate.sh "$CAN_NAME" "$BITRATE" "$USB_ADDRESS" 53 | if [ $? -eq 0 ]; then 54 | echo "CAN 设备配置成功。" 55 | exit 0 56 | else 57 | echo "配置脚本执行失败。" 58 | exit 1 59 | fi 60 | fi 61 | 62 | echo "未找到 CAN 设备，等待并重试..." 63 | 64 | # 每 5 秒检查一次 65 | sleep 5 66 | done 67 | 68 | # 如果循环结束并且超时，输出超时信息 69 | if [ "$TIMED_OUT" = "true" ]; then 70 | echo "未能在规定时间内找到 CAN 设备，脚本退出。" 71 | exit 1 72 | fi 73 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/V2_piper_ctrl_joint_mit.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # V2版本sdk 5 | # 单独设定某个电机的mit控制 6 | import time 7 | from piper_sdk import * 8 | 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2("can0") 11 | piper.ConnectPort() 12 | while( not piper.EnablePiper()): 13 | time.sleep(0.01) 14 | 15 | while True: 16 | piper.MotionCtrl_2(0x01, 0x04, 0, 0xAD) 17 | piper.JointMitCtrl(6,-0.5,0,10,0.8,0) 18 | print(1) 19 | time.sleep(1) 20 | piper.MotionCtrl_2(0x01, 0x04, 0, 0xAD) 21 | piper.JointMitCtrl(6,0,0,10,0.8,0) 22 | print(2) 23 | time.sleep(1) 24 | piper.MotionCtrl_2(0x01, 0x04, 0, 0xAD) 25 | piper.JointMitCtrl(6,0.5,0,10,0.8,0) 26 | print(3) 27 | time.sleep(1) 28 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/V2_piper_ctrl_motor_max_spd.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # V2版本sdk 5 | # 单独设定某个电机的最大速度 6 | # 注意这个指令是通过协议直接写入到驱动flash中，不可实时更新，如果需要动态调整速度，请使用位置速度模式中的速度百分比 7 | import time 8 | from piper_sdk import * 9 | 10 | if __name__ == "__main__": 11 | piper = C_PiperInterface_V2("can0") 12 | piper.ConnectPort() 13 | while( not piper.EnablePiper()): 14 | time.sleep(0.01) 15 | # 3rad/s 16 | for i in range(1,7): 17 | piper.MotorMaxSpdSet(i, 3000) 18 | time.sleep(0.1) 19 | while True: 20 | piper.SearchAllMotorMaxAngleSpd() 21 | print(piper.GetAllMotorAngleLimitMaxSpd()) 22 | time.sleep(0.01) 23 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/V2_piper_read_gripper_param_feedback.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # V2版本sdk 5 | # 夹爪/示教器参数设置指令 6 | # 第一次使用夹爪或者示教器时，需要设定一下这两个末端执行器参数，否则会出现数据没有反馈并且执行器无法控制的情况 7 | # 一般情况下 GripperTeachingPendantParamConfig 函数第二个参数 max_range_config 是70 8 | import time 9 | from piper_sdk import * 10 | 11 | if __name__ == "__main__": 12 | piper = C_PiperInterface_V2("can0") 13 | piper.ConnectPort() 14 | while True: 15 | piper.ArmParamEnquiryAndConfig(4) 16 | print(piper.GetGripperTeachingPendantParamFeedback()) 17 | time.sleep(0.05) 18 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/V2_piper_set_gripper_param.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # V2版本sdk 5 | # 夹爪/示教器参数设置指令 6 | # 第一次使用夹爪或者示教器时，需要设定一下这两个末端执行器参数，否则会出现数据没有反馈并且执行器无法控制的情况 7 | # 一般情况下 GripperTeachingPendantParamConfig 函数第二个参数 max_range_config 是70 8 | import time 9 | from piper_sdk import * 10 | 11 | if __name__ == "__main__": 12 | piper = C_PiperInterface_V2("can0") 13 | piper.ConnectPort() 14 | piper.GripperTeachingPendantParamConfig(100, 70, 1) 15 | piper.ArmParamEnquiryAndConfig(4) 16 | while True: 17 | print(piper.GetGripperTeachingPendantParamFeedback()) 18 | time.sleep(0.05) 19 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/V2_piper_set_installation_pos.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # V2版本sdk 5 | # 设定安装位置为水平正装 6 | # 如需设定为侧装左/右 7 | # MotionCtrl_2(0x01,0x01,0,0,0,0x02) 8 | # MotionCtrl_2(0x01,0x01,0,0,0,0x03) 9 | from piper_sdk import * 10 | 11 | if __name__ == "__main__": 12 | piper = C_PiperInterface_V2("can0") 13 | piper.ConnectPort() 14 | piper.MotionCtrl_2(0x01,0x01,0,0,0,0x01) 15 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/piper_sdk/081e7c588e5b79eeaefa67a0469bcc701c81014f/piper_sdk/demo/V2/__init__.py -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_ctrl_disable.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 使能机械臂 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | while(piper.DisablePiper()): 13 | time.sleep(0.01) 14 | print("失能成功!!!!") 15 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_ctrl_enable.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 使能机械臂 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | time.sleep(0.1) 13 | while( not piper.EnablePiper()): 14 | time.sleep(0.01) 15 | print("使能成功!!!!") 16 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_ctrl_end_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | import time 5 | from piper_sdk import * 6 | 7 | if __name__ == "__main__": 8 | piper = C_PiperInterface_V2("can0") 9 | piper.ConnectPort() 10 | while( not piper.EnablePiper()): 11 | time.sleep(0.01) 12 | piper.GripperCtrl(0,1000,0x01, 0) 13 | factor = 1000 14 | position = [ 15 | 57.0, \ 16 | 0.0, \ 17 | 215.0, \ 18 | 0, \ 19 | 85.0, \ 20 | 0, \ 21 | 0] 22 | 23 | count = 0 24 | while True: 25 | print(piper.GetArmEndPoseMsgs()) 26 | count = count + 1 27 | if(count == 0): 28 | print("1-----------") 29 | position = [ 30 | 57.0, \ 31 | 0.0, \ 32 | 215.0, \ 33 | 0, \ 34 | 85.0, \ 35 | 0, \ 36 | 0] 37 | elif(count == 200): 38 | print("2-----------") 39 | position = [ 40 | 57.0, \ 41 | 0.0, \ 42 | 260.0, \ 43 | 0, \ 44 | 85.0, \ 45 | 0, \ 46 | 0] 47 | elif(count == 400): 48 | print("1-----------") 49 | position = [ 50 | 57.0, \ 51 | 0.0, \ 52 | 215.0, \ 53 | 0, \ 54 | 85.0, \ 55 | 0, \ 56 | 0] 57 | count = 0 58 | 59 | X = round(position[0]*factor) 60 | Y = round(position[1]*factor) 61 | Z = round(position[2]*factor) 62 | RX = round(position[3]*factor) 63 | RY = round(position[4]*factor) 64 | RZ = round(position[5]*factor) 65 | joint_6 = round(position[6]*factor) 66 | print(X,Y,Z,RX,RY,RZ) 67 | piper.MotionCtrl_2(0x01, 0x00, 100, 0x00) 68 | piper.EndPoseCtrl(X,Y,Z,RX,RY,RZ) 69 | piper.GripperCtrl(abs(joint_6), 1000, 0x01, 0) 70 | time.sleep(0.01) 71 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_ctrl_go_zero.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | import time 5 | from piper_sdk import * 6 | 7 | if __name__ == "__main__": 8 | piper = C_PiperInterface_V2("can0") 9 | piper.ConnectPort() 10 | while( not piper.EnablePiper()): 11 | time.sleep(0.01) 12 | factor = 57295.7795 #1000*180/3.1415926 13 | position = [0,0,0,0,0,0,0] 14 | 15 | joint_0 = round(position[0]*factor) 16 | joint_1 = round(position[1]*factor) 17 | joint_2 = round(position[2]*factor) 18 | joint_3 = round(position[3]*factor) 19 | joint_4 = round(position[4]*factor) 20 | joint_5 = round(position[5]*factor) 21 | joint_6 = round(position[6]*1000*1000) 22 | piper.ModeCtrl(0x01, 0x01, 30, 0x00) 23 | piper.JointCtrl(joint_0, joint_1, joint_2, joint_3, joint_4, joint_5) 24 | piper.GripperCtrl(abs(joint_6), 1000, 0x01, 0) 25 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_ctrl_gripper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | import time 5 | from piper_sdk import * 6 | 7 | if __name__ == "__main__": 8 | piper = C_PiperInterface_V2("can0") 9 | piper.ConnectPort() 10 | while( not piper.EnablePiper()): 11 | time.sleep(0.01) 12 | piper.GripperCtrl(0,1000,0x02, 0) 13 | piper.GripperCtrl(0,1000,0x01, 0) 14 | range = 0 15 | count = 0 16 | while True: 17 | print(piper.GetArmGripperMsgs()) 18 | count = count + 1 19 | if(count == 0): 20 | print("1-----------") 21 | range = 0 22 | elif(count == 300): 23 | print("2-----------") 24 | range = 0.05 * 1000 * 1000 # 0.05m = 50mm 25 | elif(count == 600): 26 | print("3-----------") 27 | range = 0 28 | count = 0 29 | range = round(range) 30 | piper.GripperCtrl(abs(range), 1000, 0x01, 0) 31 | time.sleep(0.005) 32 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_ctrl_joint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | import time 5 | from piper_sdk import * 6 | 7 | if __name__ == "__main__": 8 | piper = C_PiperInterface_V2("can0") 9 | piper.ConnectPort() 10 | while( not piper.EnablePiper()): 11 | time.sleep(0.01) 12 | piper.GripperCtrl(0,1000,0x01, 0) 13 | factor = 57295.7795 #1000*180/3.1415926 14 | position = [0,0,0,0,0,0,0] 15 | count = 0 16 | while True: 17 | count = count + 1 18 | # print(count) 19 | if(count == 0): 20 | print("1-----------") 21 | position = [0,0,0,0,0,0,0] 22 | elif(count == 300): 23 | print("2-----------") 24 | position = [0.2,0.2,-0.2,0.3,-0.2,0.5,0.08] 25 | elif(count == 600): 26 | print("1-----------") 27 | position = [0,0,0,0,0,0,0] 28 | count = 0 29 | 30 | joint_0 = round(position[0]*factor) 31 | joint_1 = round(position[1]*factor) 32 | joint_2 = round(position[2]*factor) 33 | joint_3 = round(position[3]*factor) 34 | joint_4 = round(position[4]*factor) 35 | joint_5 = round(position[5]*factor) 36 | joint_6 = round(position[6]*1000*1000) 37 | piper.MotionCtrl_2(0x01, 0x01, 100, 0x00) 38 | piper.JointCtrl(joint_0, joint_1, joint_2, joint_3, joint_4, joint_5) 39 | piper.GripperCtrl(abs(joint_6), 1000, 0x01, 0) 40 | print(piper.GetArmStatus()) 41 | print(position) 42 | time.sleep(0.005) 43 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_ctrl_line.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | import time 5 | from piper_sdk import * 6 | 7 | if __name__ == "__main__": 8 | piper = C_PiperInterface_V2("can0") 9 | piper.ConnectPort() 10 | while( not piper.EnablePiper()): 11 | time.sleep(0.01) 12 | piper.GripperCtrl(0,1000,0x01, 0) 13 | factor = 1000 14 | position = [ 15 | 57.0, \ 16 | 0.0, \ 17 | 215.0, \ 18 | 0, \ 19 | 85.0, \ 20 | 0, \ 21 | 0] 22 | 23 | count = 0 24 | while True: 25 | print(piper.GetArmEndPoseMsgs()) 26 | count = count + 1 27 | if(count == 0): 28 | print("1-----------") 29 | position = [ 30 | 57.0, \ 31 | 0.0, \ 32 | 215.0, \ 33 | 0, \ 34 | 85.0, \ 35 | 0, \ 36 | 0] 37 | elif(count == 2): 38 | print("2-----------") 39 | position = [ 40 | 57.0, \ 41 | 0.0, \ 42 | 260.0, \ 43 | 0, \ 44 | 85.0, \ 45 | 0, \ 46 | 0] 47 | elif(count == 3): 48 | print("1-----------") 49 | position = [ 50 | 57.0, \ 51 | 0.0, \ 52 | 215.0, \ 53 | 0, \ 54 | 85.0, \ 55 | 0, \ 56 | 0] 57 | count = 0 58 | 59 | X = round(position[0]*factor) 60 | Y = round(position[1]*factor) 61 | Z = round(position[2]*factor) 62 | RX = round(position[3]*factor) 63 | RY = round(position[4]*factor) 64 | RZ = round(position[5]*factor) 65 | joint_6 = round(position[6]*factor) 66 | print(X,Y,Z,RX,RY,RZ) 67 | piper.MotionCtrl_2(0x01, 0x02, 100, 0x00) 68 | piper.EndPoseCtrl(X,Y,Z,RX,RY,RZ) 69 | piper.GripperCtrl(abs(joint_6), 1000, 0x01, 0) 70 | time.sleep(2) 71 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_ctrl_moveC.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # piper机械臂圆弧模式demo 5 | # 注意机械臂工作空间内不要有障碍 6 | import time 7 | from piper_sdk import * 8 | 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2("can0") 11 | piper.ConnectPort() 12 | while( not piper.EnablePiper()): 13 | time.sleep(0.01) 14 | piper.GripperCtrl(0,1000,0x01, 0) 15 | # 切换至MOVEC模式 16 | piper.MotionCtrl_2(0x01, 0x03, 30, 0x00) 17 | # X:135.481 18 | piper.EndPoseCtrl(135481,9349,161129,178756,6035,-178440) 19 | piper.MoveCAxisUpdateCtrl(0x01) 20 | time.sleep(0.001) 21 | piper.EndPoseCtrl(222158,128758,142126,175152,-1259,-157235) 22 | piper.MoveCAxisUpdateCtrl(0x02) 23 | time.sleep(0.001) 24 | piper.EndPoseCtrl(359079,3221,153470,179038,1105,179035) 25 | piper.MoveCAxisUpdateCtrl(0x03) 26 | time.sleep(0.001) 27 | piper.MotionCtrl_2(0x01, 0x03, 30, 0x00) 28 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_ctrl_moveJ.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | import time 5 | from piper_sdk import * 6 | 7 | if __name__ == "__main__": 8 | piper = C_PiperInterface_V2("can0") 9 | piper.ConnectPort() 10 | while( not piper.EnablePiper()): 11 | time.sleep(0.01) 12 | piper.GripperCtrl(0,1000,0x01, 0) 13 | factor = 57295.7795 #1000*180/3.1415926 14 | position = [0,0,0,0,0,0,0] 15 | count = 0 16 | while True: 17 | count = count + 1 18 | # print(count) 19 | if(count == 0): 20 | print("1-----------") 21 | position = [0,0,0,0,0,0,0] 22 | elif(count == 300): 23 | print("2-----------") 24 | position = [0.2,0.2,-0.2,0.3,-0.2,0.5,0.08] 25 | elif(count == 600): 26 | print("1-----------") 27 | position = [0,0,0,0,0,0,0] 28 | count = 0 29 | 30 | joint_0 = round(position[0]*factor) 31 | joint_1 = round(position[1]*factor) 32 | joint_2 = round(position[2]*factor) 33 | joint_3 = round(position[3]*factor) 34 | joint_4 = round(position[4]*factor) 35 | joint_5 = round(position[5]*factor) 36 | joint_6 = round(position[6]*1000*1000) 37 | piper.MotionCtrl_2(0x01, 0x01, 100, 0x00) 38 | piper.JointCtrl(joint_0, joint_1, joint_2, joint_3, joint_4, joint_5) 39 | piper.GripperCtrl(abs(joint_6), 1000, 0x01, 0) 40 | print(piper.GetArmStatus()) 41 | print(position) 42 | time.sleep(0.005) 43 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_ctrl_moveL.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # piper机械臂直线模式demo 5 | # 注意机械臂工作空间内不要有障碍 6 | import time 7 | from piper_sdk import * 8 | 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2("can0") 11 | piper.ConnectPort() 12 | while( not piper.EnablePiper()): 13 | time.sleep(0.01) 14 | 15 | # 在XOY平面上画正方形 16 | # 切换至MOVEP模式，移动到初始位置 17 | piper.MotionCtrl_2(0x01, 0x00, 100, 0x00) 18 | piper.EndPoseCtrl(150000, -50000, 150000, -179900, 0, -179900) 19 | time.sleep(2) 20 | 21 | # 切换至MOVEL模式 22 | piper.MotionCtrl_2(0x01, 0x02, 100, 0x00) 23 | piper.EndPoseCtrl(150000, 50000, 150000, -179900, 0, -179900) 24 | time.sleep(2) 25 | 26 | piper.MotionCtrl_2(0x01, 0x02, 100, 0x00) 27 | piper.EndPoseCtrl(250000, 50000, 150000, -179900, 0, -179900) 28 | time.sleep(2) 29 | 30 | piper.MotionCtrl_2(0x01, 0x02, 100, 0x00) 31 | piper.EndPoseCtrl(250000, -50000, 150000, -179900, 0, -179900) 32 | time.sleep(2) 33 | 34 | piper.MotionCtrl_2(0x01, 0x02, 100, 0x00) 35 | piper.EndPoseCtrl(150000, -50000, 150000, -179900, 0, -179900) 36 | 37 | 38 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_ctrl_moveP.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | import time 5 | from piper_sdk import * 6 | 7 | if __name__ == "__main__": 8 | piper = C_PiperInterface_V2("can0") 9 | piper.ConnectPort() 10 | while( not piper.EnablePiper()): 11 | time.sleep(0.01) 12 | piper.GripperCtrl(0,1000,0x01, 0) 13 | factor = 1000 14 | position = [ 15 | 57.0, \ 16 | 0.0, \ 17 | 215.0, \ 18 | 0, \ 19 | 85.0, \ 20 | 0, \ 21 | 0] 22 | 23 | count = 0 24 | while True: 25 | print(piper.GetArmEndPoseMsgs()) 26 | count = count + 1 27 | if(count == 0): 28 | print("1-----------") 29 | position = [ 30 | 57.0, \ 31 | 0.0, \ 32 | 215.0, \ 33 | 0, \ 34 | 85.0, \ 35 | 0, \ 36 | 0] 37 | elif(count == 200): 38 | print("2-----------") 39 | position = [ 40 | 57.0, \ 41 | 0.0, \ 42 | 260.0, \ 43 | 0, \ 44 | 85.0, \ 45 | 0, \ 46 | 0] 47 | elif(count == 400): 48 | print("1-----------") 49 | position = [ 50 | 57.0, \ 51 | 0.0, \ 52 | 215.0, \ 53 | 0, \ 54 | 85.0, \ 55 | 0, \ 56 | 0] 57 | count = 0 58 | 59 | X = round(position[0]*factor) 60 | Y = round(position[1]*factor) 61 | Z = round(position[2]*factor) 62 | RX = round(position[3]*factor) 63 | RY = round(position[4]*factor) 64 | RZ = round(position[5]*factor) 65 | joint_6 = round(position[6]*factor) 66 | print(X,Y,Z,RX,RY,RZ) 67 | piper.MotionCtrl_2(0x01, 0x00, 100, 0x00) 68 | piper.EndPoseCtrl(X,Y,Z,RX,RY,RZ) 69 | piper.GripperCtrl(abs(joint_6), 1000, 0x01, 0) 70 | time.sleep(0.01) 71 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_ctrl_reset.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 设置机械臂重置，需要在mit或者示教模式切换为位置速度控制模式时执行 5 | from piper_sdk import * 6 | 7 | # 测试代码 8 | if __name__ == "__main__": 9 | piper = C_PiperInterface_V2() 10 | piper.ConnectPort() 11 | piper.MotionCtrl_1(0x02,0,0)#恢复 12 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_ctrl_stop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 设置机械臂重置，需要在mit或者示教模式切换为位置速度控制模式时执行 5 | # 使用后需要reset，并重新使能两次 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | piper.MotionCtrl_1(0x01,0,0) 13 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_all_fps.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂消息并打印,需要先安装piper_sdk 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2("can0") 11 | piper.ConnectPort(True) 12 | count = 0 13 | while True: 14 | print() 15 | print(f"isOK: {piper.isOk()}") 16 | print(f"all_fps: {piper.GetCanFps()}") 17 | print(f"status: {piper.GetArmStatus().Hz}") 18 | print(f"end_pose: {piper.GetArmEndPoseMsgs().Hz}") 19 | print(f"joint_states: {piper.GetArmJointMsgs().Hz}") 20 | print(f"gripper_msg: {piper.GetArmGripperMsgs().Hz}") 21 | print(f"high_spd: {piper.GetArmHighSpdInfoMsgs().Hz}") 22 | print(f"low_spd: {piper.GetArmLowSpdInfoMsgs().Hz}") 23 | print(f"joint_ctrl: {piper.GetArmJointCtrl().Hz}") 24 | print(f"gripper_ctrl: {piper.GetArmGripperCtrl().Hz}") 25 | print(f"mode_ctrl: {piper.GetArmModeCtrl().Hz}") 26 | time.sleep(0.01) 27 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_arm_motor_max_acc_limit.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂的所有电机的最大加速度限制 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | while True: 13 | piper.SearchAllMotorMaxAccLimit() 14 | print(piper.GetAllMotorMaxAccLimit()) 15 | time.sleep(0.01) 16 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_arm_motor_max_angle_spd.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂的所有电机的最大加速度限制 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | while True: 13 | piper.SearchAllMotorMaxAngleSpd() 14 | print(piper.GetAllMotorAngleLimitMaxSpd()) 15 | time.sleep(0.01) 16 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_crash_protectation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 设定机械臂碰撞防护等级并打印 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2("can0",False) 11 | piper.ConnectPort() 12 | # piper.CrashProtectionConfig(1,1,1,1,1,1) 13 | piper.CrashProtectionConfig(0,0,0,0,0,0) 14 | while True: 15 | piper.ArmParamEnquiryAndConfig(0x02, 0x00, 0x00, 0x00, 0x03) 16 | print(piper.GetCrashProtectionLevelFeedback()) 17 | time.sleep(0.01) 18 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_end_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | import time 5 | from piper_sdk import * 6 | 7 | # 测试代码 8 | if __name__ == "__main__": 9 | piper = C_PiperInterface_V2() 10 | piper.ConnectPort() 11 | while True: 12 | print(piper.GetArmEndPoseMsgs()) 13 | time.sleep(0.01) 14 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_firmware.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂软件固件版本并打印 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | time.sleep(0.03) # 需要时间去读取固件反馈帧，否则会反馈-0x4AF 13 | print(piper.GetPiperFirmwareVersion()) 14 | # while True: 15 | # piper.SearchPiperFirmwareVersion() 16 | # time.sleep(0.025) 17 | # print(piper.GetPiperFirmwareVersion()) 18 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_fk.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | import time 4 | from piper_sdk import * 5 | 6 | # 测试代码 7 | if __name__ == "__main__": 8 | piper = C_PiperInterface_V2(dh_is_offset=1) 9 | piper.ConnectPort() 10 | # 使用前需要使能 11 | piper.EnableFkCal() 12 | # 注意，由于计算在单一线程中十分耗费资源，打开后会导致cpu占用率上升接近一倍 13 | while True: 14 | # 反馈6个浮点数的列表，表示 1-6 号关节的位姿，-1表示joint6的位姿 15 | print(f"feedback:{piper.GetFK('feedback')[-1]}") 16 | print(f"control:{piper.GetFK('control')}") 17 | time.sleep(0.01) 18 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_gripper_status.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂消息并打印,需要先安装piper_sdk 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | while True: 13 | print(piper.GetArmGripperMsgs()) 14 | time.sleep(0.005) 15 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_high_msg.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂消息并打印,需要先安装piper_sdk 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | while True: 13 | print(piper.GetArmHighSpdInfoMsgs()) 14 | time.sleep(0.1) 15 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_joint_ctrl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂消息并打印,需要先安装piper_sdk 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | while True: 13 | print(piper.GetArmJointCtrl()) 14 | time.sleep(0.005) 15 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_joint_state.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂消息并打印,需要先安装piper_sdk 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | while True: 13 | print(piper.GetArmJointMsgs()) 14 | print(piper.GetArmGripperMsgs()) 15 | time.sleep(0.005) 16 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_low_msg.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂消息并打印,需要先安装piper_sdk 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | while True: 13 | print(piper.GetArmLowSpdInfoMsgs()) 14 | time.sleep(0.005) 15 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_mode_ctrl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂消息并打印,需要先安装piper_sdk 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | while True: 13 | print(piper.GetArmModeCtrl()) 14 | time.sleep(0.01) 15 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_mode_ctrl_canid_151.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂消息并打印,需要先安装piper_sdk 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | while True: 13 | print(piper.GetArmCtrlCode151()) 14 | time.sleep(0.01) 15 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_resp_instruction.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂消息并打印,需要先安装piper_sdk 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | print("1-----------") 13 | print(piper.GetRespInstruction()) 14 | print("1-----------") 15 | piper.EnableArm() 16 | while True: 17 | print("------------") 18 | print(piper.GetRespInstruction()) 19 | print("------------") 20 | if piper.GetRespInstruction().instruction_response.instruction_index == 0x71: 21 | # 捕获到设置指令0x471的应答时(使能机械臂发送的指令id为471)，等待3s后清除SDK保存的应答信息 22 | # When the response to the setting command 0x471 is captured (the command ID sent to enable the robot arm is 471), 23 | # wait for 3 seconds and then clear the response information saved by the SDK 24 | time.sleep(3) 25 | print("3-----------") 26 | piper.ClearRespSetInstruction() 27 | print(piper.GetRespInstruction()) 28 | exit(0) 29 | time.sleep(0.005) 30 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_status.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂消息并打印,需要先安装piper_sdk 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | while True: 13 | print(piper.GetArmStatus()) 14 | time.sleep(0.005) 15 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_tcp_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | """ 4 | ------------------------------------------------- 5 | File Name: piper_read_tcp_pose.py 6 | Description: 测试TCP偏移 7 | Author: Jack 8 | Date: 2025-08-15 9 | Version: 1.0 10 | License: MIT License 11 | ------------------------------------------------- 12 | """ 13 | import math 14 | import time 15 | from piper_sdk import * 16 | 17 | 18 | def apply_tool_offset(pose, tool_offset): 19 | ''' 20 | 计算TCP在基座系下的位置 21 | 22 | Args: 23 | pose: (X, Y, Z, RX, RY, RZ) 24 | X,Y,Z: 单位0.001mm 25 | RX,RY,RZ: 单位0.001度 (XYZ旋转顺序) 26 | tool_offset: (tool_x, tool_y, tool_z) 单位米 27 | 在J6坐标系下工具中心点的坐标值 28 | 29 | Returns: 30 | TCP_XYZ: (X_tcp, Y_tcp, Z_tcp) 单位0.001mm 31 | ''' 32 | ''' 33 | Calculate TCP position in base coordinate system 34 | 35 | Args: 36 | pose: Tuple (X, Y, Z, RX, RY, RZ) 37 | X,Y,Z: Position in 0.001mm units 38 | RX,RY,RZ: Rotation in 0.001 degrees (XYZ rotation order) 39 | tool_offset: Tuple (tool_x, tool_y, tool_z) in meters 40 | TCP coordinates in J6 coordinate system 41 | 42 | Returns: 43 | TCP_XYZ: Tuple (X_tcp, Y_tcp, Z_tcp) in 0.001mm units 44 | ''' 45 | X, Y, Z, RX, RY, RZ = pose 46 | 47 | # Convert angles: 0.001 degrees → radians 48 | rx_rad = math.radians(RX / 1000.0) 49 | ry_rad = math.radians(RY / 1000.0) 50 | rz_rad = math.radians(RZ / 1000.0) 51 | 52 | # Precompute trig values 53 | cx, sx = math.cos(rx_rad), math.sin(rx_rad) 54 | cy, sy = math.cos(ry_rad), math.sin(ry_rad) 55 | cz, sz = math.cos(rz_rad), math.sin(rz_rad) 56 | 57 | # Compute rotation matrix (XYZ order: R = Rz * Ry * Rx) 58 | r00 = cy * cz 59 | r01 = sx * sy * cz - cx * sz 60 | r02 = cx * sy * cz + sx * sz 61 | 62 | r10 = cy * sz 63 | r11 = sx * sy * sz + cx * cz 64 | r12 = cx * sy * sz - sx * cz 65 | 66 | r20 = -sy 67 | r21 = sx * cy 68 | r22 = cx * cy 69 | 70 | # Convert tool offset: meters → 0.001mm 71 | tool_x_mm = tool_offset[0] * 1000000.0 72 | tool_y_mm = tool_offset[1] * 1000000.0 73 | tool_z_mm = tool_offset[2] * 1000000.0 74 | 75 | # Apply rotation to tool offset 76 | offset_x = r00 * tool_x_mm + r01 * tool_y_mm + r02 * tool_z_mm 77 | offset_y = r10 * tool_x_mm + r11 * tool_y_mm + r12 * tool_z_mm 78 | offset_z = r20 * tool_x_mm + r21 * tool_y_mm + r22 * tool_z_mm 79 | 80 | # Calculate TCP position with rounding 81 | return ( 82 | round(X + offset_x), 83 | round(Y + offset_y), 84 | round(Z + offset_z) 85 | ) 86 | 87 | 88 | if __name__ == "__main__": 89 | # 初始化机械臂连接 90 | piper = C_PiperInterface_V2() 91 | piper.ConnectPort() 92 | 93 | # # 等待机械臂启用 94 | # while not piper.DisablePiper(): 95 | # time.sleep(0.01) 96 | 97 | # # 设置初始位置 98 | # piper.MotionCtrl_2(0x01, 0x01, 100, 0x00) 99 | # piper.JointCtrl(0,0,0,0,0,0) 100 | 101 | # 获取末端位姿并设置工具偏移 102 | end_pose = piper.GetArmEndPoseMsgs().end_pose 103 | tool_offset = (0, 0, 0.145) # Z轴偏移145mm 104 | 105 | # 实时计算并显示TCP位置 106 | while True: 107 | pose = ( 108 | end_pose.X_axis, 109 | end_pose.Y_axis, 110 | end_pose.Z_axis, 111 | end_pose.RX_axis, 112 | end_pose.RY_axis, 113 | end_pose.RZ_axis 114 | ) 115 | 116 | x_tcp, y_tcp, z_tcp = apply_tool_offset(pose, tool_offset) 117 | 118 | print("="*50) 119 | print("J6 Pos (0.001mm):") 120 | print(f" X: {pose[0]}, Y: {pose[1]}, Z: {pose[2]}") 121 | 122 | print("\nTCP Pos (0.001mm):") 123 | print(f" X: {x_tcp}, Y: {y_tcp}, Z: {z_tcp}\n") 124 | 125 | time.sleep(0.005) -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_read_version.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | import time 5 | from piper_sdk import * 6 | 7 | # 测试代码 8 | if __name__ == "__main__": 9 | piper = C_PiperInterface_V2() 10 | piper.ConnectPort(piper_init=True, start_thread=True) 11 | time.sleep(0.1) # 需要时间去读取固件反馈帧，否则会反馈-0x4AF,如果不需要读取固件版本，可以不设置延时 12 | print(f'=====>> Piper Current Interface Version is {piper.GetCurrentInterfaceVersion()} <<=====') 13 | print(f'=====>> Piper Current Interface Version is {piper.GetCurrentInterfaceVersion().value} <<=====') 14 | print(f'=====>> Piper Current Protocol Version is {piper.GetCurrentProtocolVersion()} <<=====') 15 | print(f'=====>> Piper Current Protocol Version is {piper.GetCurrentProtocolVersion().value} <<=====') 16 | print(f'=====>> Piper Current SDK Version is {piper.GetCurrentSDKVersion()} <<=====') 17 | print(f'=====>> Piper Current SDK Version is {piper.GetCurrentSDKVersion().value} <<=====') 18 | # 只有当 ConnectPort(piper_init=True, start_thread=True) 时，才能读取到固件版本 19 | print(f'=====>> Piper Current Firmware Version is {piper.GetPiperFirmwareVersion()} <<=====') 20 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_set_can.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 本demo读取的是串口can消息 4 | # 使用指定参数的can bus 5 | # 注意，如果interface中的can_auto_init为False，需要先执行CreateCanBus初始化内部的__arm_can才能执行ConnectPort，否则会报错 6 | # 如果使用pcie转can或者串口can模块，需要将judge_flag置为False，否则检测的是Linux系统下的socketcan模块 7 | # 注意需要先赋予串口权限: sudo chmod 777 /dev/ttyACM0 8 | # 正常帧率为3040左右(链接单条臂) 9 | import time 10 | from piper_sdk import * 11 | 12 | # 测试代码 13 | if __name__ == "__main__": 14 | piper = C_PiperInterface_V2(can_auto_init=False) 15 | piper.CreateCanBus(can_name="/dev/ttyACM0", 16 | bustype="slcan", 17 | expected_bitrate=1000000, 18 | judge_flag=False 19 | ) 20 | piper.ConnectPort(piper_init=False) 21 | while(True): 22 | print(f"all_fps: {piper.GetCanFps()}") 23 | time.sleep(0.01) 24 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_set_gripper_zero.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 夹爪设定零点demo 5 | import time 6 | from piper_sdk import * 7 | 8 | if __name__ == "__main__": 9 | piper = C_PiperInterface_V2("can0") 10 | piper.ConnectPort() 11 | piper.GripperCtrl(0,1000,0x00, 0) 12 | time.sleep(1.5) 13 | piper.GripperCtrl(0,1000,0x00, 0xAE) 14 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_set_init_default.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # V2版本sdk 5 | # 机械臂 设置全部关节限位、关节最大速度、关节加速度为默认值： 0x02 6 | import time 7 | from piper_sdk import * 8 | 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2("can0") 11 | piper.ConnectPort() 12 | piper.ArmParamEnquiryAndConfig(0x01,0x02,0,0,0x02) 13 | while True: 14 | piper.SearchAllMotorMaxAngleSpd() 15 | print(piper.GetAllMotorAngleLimitMaxSpd()) 16 | time.sleep(0.01) 17 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_set_joint_zero.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | """ 4 | ------------------------------------------------- 5 | File Name: piper_set_joint_zero.py 6 | Description: 设置关节电机零点位置 7 | Author: Jack 8 | Date: 2025-08-14 9 | Version: 1.0 10 | License: MIT License 11 | ------------------------------------------------- 12 | """ 13 | import time 14 | from piper_sdk import * 15 | 16 | # 测试代码 17 | if __name__ == "__main__": 18 | piper = C_PiperInterface_V2() 19 | piper.ConnectPort() 20 | time.sleep(0.1) 21 | print("设置过程输入'q'可以退出程序") 22 | print("During setup, enter 'q' to exit the program") 23 | print("设置零点前会对指定的电机失能，请保护好机械臂") 24 | print("Before setting zero position, the specified motor will be disabled. Please protect the robotic arm.") 25 | while( not piper.EnablePiper()): 26 | time.sleep(0.01) 27 | piper.ModeCtrl(0x01, 0x01, 30, 0x00) 28 | piper.JointCtrl(0, 0, 0, 0, 0, 0) 29 | piper.GripperCtrl(0, 1000, 0x01, 0) 30 | mode = -1 31 | while True: 32 | # 模式选择 33 | if mode == -1: 34 | print("\nStep 1: 请选择设置模式(0: 指定电机; 1: 顺序设置): ") 35 | print("Step 1: Select setting mode (0: Single motor; 1: Sequential setting): ") 36 | mode = input("> ") 37 | if mode == '0': 38 | mode = 0 39 | elif mode == '1': 40 | mode = 1 41 | elif mode == 'q': 42 | break 43 | else: 44 | mode = -1 45 | 46 | # 单电机设置 47 | elif mode == 0: 48 | print("\nStep 2: 输入需要设置零点的电机序号(1~7), 7代表所有电机: ") 49 | print("Step 2: Enter motor number to set zero (1~7), 7 represents all motors: ") 50 | motor_num = input("> ") 51 | if motor_num == 'q': 52 | mode = -1 53 | continue 54 | try: 55 | motor_num = int(motor_num) 56 | if motor_num < 1 or motor_num > 7: 57 | print("Tip: 输入超出范围") 58 | print("Tip: Input out of range") 59 | continue 60 | except: 61 | print("Tip: 请输入整数") 62 | print("Tip: Please enter an integer") 63 | continue 64 | piper.DisableArm(motor_num) 65 | print(f"\nInfo: 第{motor_num}号电机失能成功，请手动纠正电机的零点位置") 66 | print(f"Info: Motor {motor_num} disabled successfully. Please manually adjust to zero position") 67 | 68 | print(f"\nStep 3: 回车设置第{motor_num}号电机零点: ") 69 | print(f"Step 3: Press Enter to set zero for motor {motor_num}: ") 70 | if input("(按回车继续/Press Enter) ") == 'q': 71 | mode = -1 72 | continue 73 | piper.JointConfig(motor_num, 0xAE) 74 | piper.EnableArm(motor_num) 75 | print(f"\nInfo: 第{motor_num}号电机零点设置成功") 76 | print(f"Info: Motor {motor_num} zero position set successfully") 77 | 78 | # 顺序设置 79 | elif mode == 1: 80 | print("\nStep 2: 输入从第几号电机开始设置(1~6): ") 81 | print("Step 2: Enter starting motor number (1~6): ") 82 | motor_num = input("> ") 83 | if motor_num == 'q': 84 | mode = -1 85 | continue 86 | try: 87 | motor_num = int(motor_num) 88 | if motor_num < 1 or motor_num > 6: 89 | print("Tip: 输入超出范围") 90 | print("Tip: Input out of range") 91 | continue 92 | except: 93 | print("Tip: 请输入整数") 94 | print("Tip: Please enter an integer") 95 | continue 96 | for i in range(motor_num, 7): 97 | piper.DisableArm(i) 98 | print(f"\nInfo: 第{i}号电机失能成功，请手动纠正电机的零点位置") 99 | print(f"Info: Motor {i} disabled successfully. Please manually adjust to zero position") 100 | 101 | print(f"\nStep 3: 回车设置第{i}号电机零点: ") 102 | print(f"Step 3: Press Enter to set zero for motor {i}: ") 103 | if input("(按回车继续/Press Enter) ") == 'q': 104 | mode = -1 105 | break 106 | piper.JointConfig(i, 0xAE) 107 | piper.EnableArm(i) 108 | print(f"\nInfo: 第{i}号电机零点设置成功") 109 | print(f"Info: Motor {i} zero position set successfully") -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_set_load.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 设置机械臂为mit控制模式，这个模式下，机械臂相应最快 5 | from piper_sdk import * 6 | 7 | # 测试代码 8 | if __name__ == "__main__": 9 | piper = C_PiperInterface_V2() 10 | piper.ConnectPort() 11 | load = 2 # 0，1，2 12 | piper.ArmParamEnquiryAndConfig(0, 0, 0, 0xAE, load) # 0xFC 13 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_set_log_level.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-* 3 | from piper_sdk import * 4 | from piper_sdk.utils import global_area 5 | 6 | # 测试代码 7 | if __name__ == "__main__": 8 | piper = C_PiperInterface_V2(can_name="can0", logger_level=LogLevel.DEBUG, log_to_file=True) 9 | piper.ConnectPort() 10 | # 打印日志文件路径 11 | print("log file path:", LogManager.get_log_file_path(global_area)) 12 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_set_master.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 设置机械臂为主动臂，直接设置即可 5 | from piper_sdk import * 6 | 7 | # 测试代码 8 | if __name__ == "__main__": 9 | piper = C_PiperInterface_V2() 10 | piper.ConnectPort() 11 | piper.MasterSlaveConfig(0xFA, 0, 0, 0) 12 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_set_mit.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 设置机械臂为mit控制模式，这个模式下，机械臂相应最快 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | while True: 13 | piper.MotionCtrl_2(1, 1, 0, 0xAD)# 0xFC 14 | time.sleep(1) 15 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_set_motor_angle_limit.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # V2版本sdk 5 | # 单独设定某个电机的关节限位 6 | # 注意这个指令是通过协议直接写入到驱动flash中，不可实时更新 7 | import time 8 | from piper_sdk import * 9 | 10 | if __name__ == "__main__": 11 | piper = C_PiperInterface_V2("can0") 12 | piper.ConnectPort() 13 | piper.EnableArm(7) 14 | while( not piper.EnablePiper()): 15 | time.sleep(0.01) 16 | 17 | piper.MotorAngleLimitMaxSpdSet(1, 1500, -1500) 18 | piper.MotorAngleLimitMaxSpdSet(2, 1800, 0) 19 | piper.MotorAngleLimitMaxSpdSet(3, 0, -1700) 20 | piper.MotorAngleLimitMaxSpdSet(4, 1000, -1000) 21 | piper.MotorAngleLimitMaxSpdSet(5, 700, -700) 22 | piper.MotorAngleLimitMaxSpdSet(6, 1700, -1700) 23 | 24 | while True: 25 | print(piper.GetAllMotorAngleLimitMaxSpd()) 26 | time.sleep(0.1) 27 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_set_motor_max_acc_limit.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # V2版本sdk 5 | # 单独设定某个电机的最大加速度 6 | # 注意这个指令是通过协议直接写入到驱动flash中，不可实时更新 7 | import time 8 | from piper_sdk import * 9 | 10 | if __name__ == "__main__": 11 | piper = C_PiperInterface_V2("can0") 12 | piper.ConnectPort() 13 | piper.EnableArm(7) 14 | while( not piper.EnablePiper()): 15 | time.sleep(0.01) 16 | # 5rad/s 17 | for i in range(1,7): 18 | piper.JointMaxAccConfig(i, 500) 19 | print(i) 20 | time.sleep(0.5) # 数据的写入需要时间，发送完上一帧设定指令，需要延时一会 21 | while True: 22 | piper.SearchAllMotorMaxAccLimit() 23 | print(piper.GetAllMotorMaxAccLimit()) 24 | time.sleep(0.1) 25 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_set_sdk_param.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | from piper_sdk import * 5 | 6 | if __name__ == "__main__": 7 | piper = C_PiperInterface_V2("can0", 8 | start_sdk_gripper_limit=True, 9 | start_sdk_joint_limit=True 10 | ) 11 | piper.ConnectPort() 12 | print(piper.GetSDKJointLimitParam('j6')) 13 | print(piper.GetSDKGripperRangeParam()) 14 | piper.SetSDKGripperRangeParam(0, 0.05) 15 | piper.SetSDKJointLimitParam('j6',-2.09, 2.09) 16 | print(piper.GetSDKJointLimitParam('j6')) 17 | print(piper.GetSDKGripperRangeParam()) 18 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_set_slave.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 设置机械臂为从动臂 5 | # 注意，如果是在机械臂处于主动臂模式下，发送设置指令后需要重新启动机械臂 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2() 11 | piper.ConnectPort() 12 | piper.MasterSlaveConfig(0xFC, 0, 0, 0) 13 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_test_interface_disconnect.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 注意demo无法直接运行，需要pip安装sdk后才能运行 4 | # 读取机械臂消息并打印,需要先安装piper_sdk 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2("can0") 11 | piper.ConnectPort(True) 12 | count = 0 13 | while True: 14 | count += 1 15 | print("---------------",count) 16 | if(count > 200 and count < 400): 17 | piper.DisconnectPort() 18 | elif(count > 400): 19 | piper.ConnectPort() 20 | print() 21 | print(f"isOK: {piper.isOk()}") 22 | print(f"can: {piper.GetCanFps()}") 23 | print(f"status: {piper.GetArmStatus().Hz}") 24 | print(f"end_pose: {piper.GetArmEndPoseMsgs().Hz}") 25 | print(f"joint_states: {piper.GetArmJointMsgs().Hz}") 26 | print(f"gripper_msg: {piper.GetArmGripperMsgs().Hz}") 27 | print(f"high_spd: {piper.GetArmHighSpdInfoMsgs().Hz}") 28 | print(f"low_spd: {piper.GetArmLowSpdInfoMsgs().Hz}") 29 | print(f"joint_ctrl: {piper.GetArmJointCtrl().Hz}") 30 | print(f"gripper_ctrl: {piper.GetArmGripperCtrl().Hz}") 31 | print(f"ctrl_151: {piper.GetArmCtrlCode151().Hz}") 32 | print() 33 | time.sleep(0.01) 34 | -------------------------------------------------------------------------------- /piper_sdk/demo/V2/piper_test_multi_interface_instance.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | # 当有多个机械臂的时候，可以创建多实例，通过识别can_port避免创建读取相同can端口的实例 4 | # 注意需要多个can模块 5 | import time 6 | from piper_sdk import * 7 | 8 | # 测试代码 9 | if __name__ == "__main__": 10 | piper = C_PiperInterface_V2("can_0") 11 | piper.ConnectPort(True) 12 | piper1 = C_PiperInterface_V2("can_1") 13 | piper1.ConnectPort(True) 14 | while True: 15 | print(piper.GetCanFps()) 16 | time.sleep(1) 17 | pass 18 | -------------------------------------------------------------------------------- /piper_sdk/demo/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/piper_sdk/081e7c588e5b79eeaefa67a0469bcc701c81014f/piper_sdk/demo/__init__.py -------------------------------------------------------------------------------- /piper_sdk/demo/detect_arm.MD: -------------------------------------------------------------------------------- 1 | # detect arm 文件使用文档 2 | 3 | ## 1 首先需要使用0.3.0以上的piper_sdk版本 4 | 5 | 查看pip包版本 6 | 7 | ```bash 8 | pip3 show piper_sdk 9 | ``` 10 | 11 | 输出 12 | 13 | ```bash 14 | Name: piper_sdk 15 | Version: 0.3.0 16 | Summary: A sdk to control Agilex piper arm 17 | Home-page: https://github.com/agilexrobotics/piper_sdk 18 | Author: RosenYin 19 | Author-email: 20 | License: MIT License 21 | Location: .../.local/lib/python3.8/site-packages 22 | Requires: python-can 23 | Required-by: 24 | ``` 25 | 26 | ## 2 执行detect_arm.py 27 | 28 | 注意路径，使用上述`Location`路径内部的`piper_sdk`包，可以通过下述指令获取 29 | 30 | ```bash 31 | export PIPER_SDK_PATH=$(pip3 show piper_sdk | grep ^Location: | awk '{print $2}') 32 | ``` 33 | 34 | ```bash 35 | echo $PIPER_SDK_PATH 36 | ``` 37 | 38 | 文件有三个输入参数： 39 | 40 | - `--can_port`用来设定读取的can名称 41 | - `--hz`用来设定终端打印刷新的频率 42 | - `--req_flag`用来设定是否在执行脚本时给机械臂发送请求查询指令来获取机械臂的一些静态参数，比如固件版本、关节最大速度等 43 | 44 | 正常情况下执行下述指令即可 45 | 46 | 注意`PIPER_SDK_PATH`是由上述指令获取 47 | 48 | ```bash 49 | python3 $PIPER_SDK_PATH/piper_sdk/demo/detect_arm.py --can_port can0 --hz 10 --req_flag 1 50 | ``` 51 | -------------------------------------------------------------------------------- /piper_sdk/demo/manage_vcan.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import subprocess 3 | import argparse 4 | 5 | def interface_exists(name: str) -> bool: 6 | result = subprocess.run(['ip', 'link', 'show', name], 7 | stdout=subprocess.DEVNULL, 8 | stderr=subprocess.DEVNULL) 9 | return result.returncode == 0 10 | 11 | def is_physical_can(name: str) -> bool: 12 | try: 13 | output = subprocess.check_output(['ethtool', '-i', name], text=True) 14 | return "driver" in output.lower() 15 | except subprocess.CalledProcessError: 16 | return False 17 | 18 | def is_vcan(name: str) -> bool: 19 | try: 20 | output = subprocess.check_output(['ip', '-d', 'link', 'show', name], text=True) 21 | return 'vcan' in output.lower() 22 | except subprocess.CalledProcessError: 23 | return False 24 | 25 | def get_can_bitrate(name: str) -> str: 26 | try: 27 | output = subprocess.check_output(['ip', '-details', 'link', 'show', name], text=True) 28 | for line in output.splitlines(): 29 | line = line.strip() 30 | if line.startswith("bitrate"): 31 | return line # e.g., "bitrate 500000 sample-point 0.875" 32 | return "(未设置波特率)" 33 | except subprocess.CalledProcessError: 34 | return "(获取失败)" 35 | 36 | def create_vcan(name: str, bitrate: int): 37 | if interface_exists(name): 38 | print(f"[✓] 接口 {name} 已存在，跳过创建") 39 | if is_physical_can(name): 40 | bitrate_info = get_can_bitrate(name) 41 | print(f"[i] 当前波特率信息: {bitrate_info}") 42 | elif is_vcan(name): 43 | print(f"[i] {name} 是虚拟 CAN 接口，无波特率设置") 44 | else: 45 | print(f"[❓] {name} 类型无法识别") 46 | return 47 | 48 | try: 49 | subprocess.run(['modprobe', 'vcan'], check=True) 50 | subprocess.run(['ip', 'link', 'add', 'dev', name, 'type', 'vcan'], check=True) 51 | subprocess.run(['ip', 'link', 'set', name, 'up'], check=True) 52 | print(f"[+] 成功创建虚拟 CAN 接口: {name}") 53 | except subprocess.CalledProcessError as e: 54 | print(f"[×] 创建失败: {e}") 55 | 56 | def delete_vcan(name: str): 57 | if not interface_exists(name): 58 | print(f"[×] 接口 {name} 不存在，无法删除") 59 | return 60 | 61 | if is_physical_can(name): 62 | bitrate_info = get_can_bitrate(name) 63 | print(f"[⚠️] {name} 是实体 CAN 接口，跳过删除") 64 | print(f"[i] 当前波特率信息: {bitrate_info}") 65 | return 66 | elif not is_vcan(name): 67 | print(f"[❓] {name} 类型未知，为安全起见跳过删除") 68 | return 69 | 70 | try: 71 | subprocess.run(['ip', 'link', 'delete', name], check=True) 72 | print(f"[-] 已删除虚拟 CAN 接口: {name}") 73 | except subprocess.CalledProcessError as e: 74 | print(f"[×] 删除失败: {e}") 75 | 76 | def main(): 77 | parser = argparse.ArgumentParser(description="虚拟CAN接口管理工具") 78 | subparsers = parser.add_subparsers(dest='command', required=True) 79 | 80 | parser_create = subparsers.add_parser('create', help='创建虚拟 CAN 接口') 81 | parser_create.add_argument('--can_name', required=True) 82 | parser_create.add_argument('--bitrate', type=int, default=500000, help='波特率（用于显示，不影响虚拟 CAN）') 83 | 84 | parser_delete = subparsers.add_parser('delete', help='删除虚拟 CAN 接口') 85 | parser_delete.add_argument('--can_name', required=True) 86 | 87 | args = parser.parse_args() 88 | 89 | if args.command == 'create': 90 | create_vcan(args.can_name, args.bitrate) 91 | elif args.command == 'delete': 92 | delete_vcan(args.can_name) 93 | 94 | if __name__ == '__main__': 95 | main() 96 | -------------------------------------------------------------------------------- /piper_sdk/find_all_can_port.sh: -------------------------------------------------------------------------------- 1 | # Check if ethtool is installed 2 | if ! dpkg -l | grep -q "ethtool"; then 3 | echo "Error: ethtool not detected in the system." 4 | echo "Please install ethtool using the following command:" 5 | echo "sudo apt update && sudo apt install ethtool" 6 | exit 1 7 | fi 8 | 9 | # Check if can-utils is installed 10 | if ! dpkg -l | grep -q "can-utils"; then 11 | echo "Error: can-utils not detected in the system." 12 | echo "Please install can-utils using the following command:" 13 | echo "sudo apt update && sudo apt install can-utils" 14 | exit 1 15 | fi 16 | 17 | echo "Both ethtool and can-utils are installed." 18 | 19 | # Iterate through all CAN interfaces 20 | for iface in $(ip -br link show type can | awk '{print $1}'); do 21 | # Use ethtool to get bus-info 22 | BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}') 23 | 24 | if [ -z "$BUS_INFO" ];then 25 | echo "Error: Unable to get bus-info for interface $iface." 26 | continue 27 | fi 28 | 29 | echo "Interface $iface is connected to USB port $BUS_INFO" 30 | done -------------------------------------------------------------------------------- /piper_sdk/hardware_port/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | from .can_encapsulation_v0_4_0 import C_STD_CAN 3 | 4 | __all__ = [ 5 | 'C_STD_CAN' 6 | ] 7 | 8 | -------------------------------------------------------------------------------- /piper_sdk/interface/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | from .piper_interface import * 3 | from .piper_interface_v2 import C_PiperInterface_V2 4 | __all__ = [ 5 | 'C_PiperInterface', 6 | 'C_PiperInterface_V2' 7 | ] 8 | 9 | -------------------------------------------------------------------------------- /piper_sdk/interface/interface_version.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | 4 | from enum import Enum, auto 5 | 6 | class InterfaceVersion(Enum): 7 | ''' 8 | Interface版本枚举 9 | ''' 10 | ''' 11 | Interface Version Enumeration. 12 | ''' 13 | INTERFACE_V1 = auto() 14 | INTERFACE_V2 = auto() 15 | INTERFACE_UNKNOWN = auto() 16 | def __str__(self): 17 | return f"{self.name} (0x{self.value:X})" 18 | def __repr__(self): 19 | return f"{self.name}: 0x{self.value:X}" -------------------------------------------------------------------------------- /piper_sdk/kinematics/__init__.py: -------------------------------------------------------------------------------- 1 | from .piper_fk import C_PiperForwardKinematics 2 | 3 | __all__ = [ 4 | "C_PiperForwardKinematics" 5 | ] -------------------------------------------------------------------------------- /piper_sdk/log/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/piper_sdk/081e7c588e5b79eeaefa67a0469bcc701c81014f/piper_sdk/log/__init__.py -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/piper_sdk/081e7c588e5b79eeaefa67a0469bcc701c81014f/piper_sdk/piper_msgs/__init__.py -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/__init__.py: -------------------------------------------------------------------------------- 1 | # piper_msgs/msg_v2/__init__.py 2 | 3 | # msg_v2/__init__.py 4 | 5 | from .arm_messages import PiperMessage 6 | from .can_id import CanIDPiper 7 | from .arm_msg_type import ArmMsgType 8 | from .arm_id_type_map import ArmMessageMapping 9 | # 导入 feedback 子模块的类 10 | from .feedback import * 11 | # 导入 transmit 子模块的类 12 | from .transmit import * 13 | 14 | __all__ = [ 15 | # 反馈 16 | 'PiperMessage', 17 | 'CanIDPiper', 18 | 'ArmMsgType', 19 | 'ArmMsgFeedBackEndPose', 20 | 'ArmMsgFeedBackJointStates', 21 | 'ArmMsgFeedbackStatus', 22 | 'ArmMsgFeedbackStatusEnum', 23 | 'ArmMsgFeedBackGripper', 24 | 'ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd', 25 | 'ArmMsgFeedbackCurrentEndVelAccParam', 26 | 'ArmMsgFeedbackCurrentMotorMaxAccLimit', 27 | 'ArmMsgFeedbackJointVelAcc', 28 | 'ArmMsgFeedbackAllCurrentMotorAngleLimitMaxSpd', 29 | 'ArmMsgFeedbackAllCurrentMotorMaxAccLimit', 30 | 'ArmMsgFeedbackAllJointVelAcc', 31 | 'ArmMsgFeedbackCrashProtectionRating', 32 | 'ArmMsgFeedbackHighSpd', 33 | 'ArmMsgFeedbackLowSpd', 34 | 'ArmMsgFeedbackGripperTeachingPendantParam', 35 | 'ArmMsgFeedbackRespSetInstruction', 36 | # 发送 37 | 'ArmMsgMotionCtrl_1', 38 | 'ArmMsgMotionCtrl_2', 39 | 'ArmMsgMotionCtrlCartesian', 40 | 'ArmMsgJointCtrl', 41 | 'ArmMsgCircularPatternCoordNumUpdateCtrl', 42 | 'ArmMsgGripperCtrl', 43 | 'ArmMsgMasterSlaveModeConfig', 44 | 'ArmMsgMotorEnableDisableConfig', 45 | 'ArmMsgSearchMotorMaxAngleSpdAccLimit', 46 | 'ArmMsgMotorAngleLimitMaxSpdSet', 47 | 'ArmMsgJointConfig', 48 | 'ArmMsgInstructionResponseConfig', 49 | 'ArmMsgParamEnquiryAndConfig', 50 | 'ArmMsgEndVelAccParamConfig', 51 | 'ArmMsgCrashProtectionRatingConfig', 52 | 'ArmMsgGripperTeachingPendantParamConfig', 53 | 'ArmMsgJointMitCtrl', 54 | 'ArmMsgAllJointMitCtrl' 55 | ] 56 | 57 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/arm_msg_type.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | 4 | from enum import Enum, auto 5 | 6 | class ArmMsgType(Enum): 7 | ''' 8 | msg_v2 9 | 10 | 机械臂消息类型,枚举类型 11 | ''' 12 | ''' 13 | msg_v2 14 | 15 | Robotic Arm Message Types (Enumeration) 16 | ''' 17 | # feedback 18 | PiperMsgUnkonwn = 0x00 #未知类型 19 | PiperMsgStatusFeedback = auto() #机械臂状态消息反馈 20 | PiperMsgEndPoseFeedback_1 = auto() #机械臂末端位姿反馈1 21 | PiperMsgEndPoseFeedback_2 = auto() #机械臂末端位姿反馈2 22 | PiperMsgEndPoseFeedback_3 = auto() #机械臂末端位姿反馈3 23 | PiperMsgJointFeedBack_12 = auto() #机械臂臂部关节反馈12 24 | PiperMsgJointFeedBack_34 = auto() #机械臂臂部关节反馈34 25 | PiperMsgJointFeedBack_56 = auto() #机械臂臂部关节反馈56 26 | PiperMsgGripperFeedBack = auto() #夹爪反馈指令 27 | PiperMsgHighSpdFeed_1 = auto() 28 | PiperMsgHighSpdFeed_2 = auto() 29 | PiperMsgHighSpdFeed_3 = auto() 30 | PiperMsgHighSpdFeed_4 = auto() 31 | PiperMsgHighSpdFeed_5 = auto() 32 | PiperMsgHighSpdFeed_6 = auto() 33 | PiperMsgLowSpdFeed_1 = auto() 34 | PiperMsgLowSpdFeed_2 = auto() 35 | PiperMsgLowSpdFeed_3 = auto() 36 | PiperMsgLowSpdFeed_4 = auto() 37 | PiperMsgLowSpdFeed_5 = auto() 38 | PiperMsgLowSpdFeed_6 = auto() 39 | # transmit 40 | PiperMsgMotionCtrl_1=auto() 41 | # PiperMsgStopCtrl = auto() 42 | # PiperMsgTrackCtrl = auto() 43 | # PiperMsgGragTeachCtrl = auto() 44 | PiperMsgMotionCtrl_2=auto() 45 | # PiperMsgModeCtrl = auto() 46 | # PiperMsgMoveModeCtrl = auto() 47 | # PiperMsgMoveSpdRateCtrl = auto() 48 | PiperMsgMotionCtrlCartesian_1 = auto() 49 | PiperMsgMotionCtrlCartesian_2 = auto() 50 | PiperMsgMotionCtrlCartesian_3 = auto() 51 | PiperMsgJointCtrl_12 = auto() 52 | PiperMsgJointCtrl_34 = auto() 53 | PiperMsgJointCtrl_56 = auto() 54 | PiperMsgCircularPatternCoordNumUpdateCtrl=auto() 55 | PiperMsgGripperCtrl = auto() 56 | #----------------------------------基于V1.5-2版本后---------------------------------------------# 57 | PiperMsgJointMitCtrl_1 = auto() 58 | PiperMsgJointMitCtrl_2 = auto() 59 | PiperMsgJointMitCtrl_3 = auto() 60 | PiperMsgJointMitCtrl_4 = auto() 61 | PiperMsgJointMitCtrl_5 = auto() 62 | PiperMsgJointMitCtrl_6 = auto() 63 | #---------------------------------------------------------------------------------------------# 64 | PiperMsgMasterSlaveModeConfig = auto() 65 | # PiperMsgMSLinkageConfig = auto() 66 | # PiperMsgMSFeedbackInstructionOffsetConfig=auto() 67 | # PiperMsgMSCtrlInstructionOffsetConfig=auto() 68 | # PiperMsgMSLinkageCtrlOffsetConfig=auto() 69 | PiperMsgMotorEnableDisableConfig=auto() # 电机使能/失能设置指令 70 | # PiperMsgMotorDisableConfig=auto() 71 | # PiperMsgSearchMotorAngleConfig=auto() 72 | PiperMsgSearchMotorMaxAngleSpdAccLimit=auto() 73 | # PiperMsgSearchMotorMaxAccConfig=auto() 74 | PiperMsgFeedbackCurrentMotorAngleLimitMaxSpd=auto() 75 | PiperMsgMotorAngleLimitMaxSpdSet=auto()#电机角度限制/最大速度设置指令 76 | PiperMsgJointConfig=auto() 77 | PiperMsgInstructionResponseConfig=auto() 78 | PiperMsgFeedbackRespSetInstruction=auto() 79 | PiperMsgParamEnquiryAndConfig=auto() 80 | PiperMsgFeedbackCurrentEndVelAccParam=auto() 81 | PiperMsgEndVelAccParamConfig=auto() 82 | PiperMsgCrashProtectionRatingConfig=auto() 83 | PiperMsgCrashProtectionRatingFeedback=auto() 84 | PiperMsgFeedbackCurrentMotorMaxAccLimit=auto() 85 | #----------------------------------基于V1.5-2版本后---------------------------------------------# 86 | PiperMsgGripperTeachingPendantParamConfig = auto() 87 | PiperMsgGripperTeachingPendantParamFeedback = auto() 88 | #---------------------------------------------------------------------------------------------# 89 | PiperMsgFeedbackJointVelAcc_1=auto() 90 | PiperMsgFeedbackJointVelAcc_2=auto() 91 | PiperMsgFeedbackJointVelAcc_3=auto() 92 | PiperMsgFeedbackJointVelAcc_4=auto() 93 | PiperMsgFeedbackJointVelAcc_5=auto() 94 | PiperMsgFeedbackJointVelAcc_6=auto() 95 | PiperMsgLightCtrl=auto() 96 | PiperMsgCanUpdateSilentModeConfig=auto() 97 | PiperMsgFirmwareRead=auto() 98 | def __str__(self): 99 | return f"{self.name} (0x{self.value:X})" 100 | def __repr__(self): 101 | return f"{self.name}: 0x{self.value:X}" -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/can_id.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | 4 | from enum import Enum, auto 5 | 6 | class CanIDPiper(Enum): 7 | ''' 8 | msg_v2 9 | 10 | 机械臂can id 11 | ''' 12 | ''' 13 | msg_v2 14 | 15 | piper's can_ids 16 | ''' 17 | # 主动反馈指令，可设置整体偏移为 0x2B1~0x2B8或 0x2C1~0x2C8，详见指令0x470 18 | ARM_STATUS_FEEDBACK = 0x2A1 #机械臂状态反馈ID 19 | ARM_END_POSE_FEEDBACK_1 = 0x2A2 #机械臂末端姿态反馈 20 | ARM_END_POSE_FEEDBACK_2 = 0x2A3 21 | ARM_END_POSE_FEEDBACK_3 = 0x2A4 22 | ARM_JOINT_FEEDBACK_12 = 0x2A5 #机械臂关节反馈 23 | ARM_JOINT_FEEDBACK_34 = 0x2A6 24 | ARM_JOINT_FEEDBACK_56 = 0x2A7 25 | ARM_GRIPPER_FEEDBACK = 0x2A8 #机械臂夹爪反馈 26 | # 运动控制指令，可设置整体偏移为 0x160~0x169或 0x170~0x179，详见指令0x470 27 | ARM_MOTION_CTRL_1 = 0x150 28 | # ARM_STOP_CTRL = 0x150 #机械臂快速急停 29 | # ARM_TRACK_CTRL = 0x150 #机械臂轨迹指令 30 | # ARM_GRAG_TEACH_CTRL = 0x150 #机械臂拖动示教指令 31 | ARM_MOTION_CTRL_2 = 0x151 32 | # ARM_MODE_CTRL = 0x151 #机械臂控制模式 33 | # ARM_MOVE_MODE_CTRL = 0x151 #机械臂Mode模式 34 | # ARM_MOVE_SPD_RATE_CTRL = 0x151 #机械臂运动速度百分比 35 | ARM_MOTION_CTRL_CARTESIAN_1=0x152#机械臂运动控制直角坐标系指令1,X&Y 36 | ARM_MOTION_CTRL_CARTESIAN_2=0x153#机械臂运动控制直角坐标系指令1,Z&RX 37 | ARM_MOTION_CTRL_CARTESIAN_3=0x154#机械臂运动控制直角坐标系指令1,RY&RZ 38 | ARM_JOINT_CTRL_12=0x155 #机械臂臂部关节控制指令12,J1&J2 39 | ARM_JOINT_CTRL_34=0x156 #机械臂臂部关节控制指令12,J3&J4 40 | ARM_JOINT_CTRL_56=0x157 #机械臂臂部关节控制指令12,J5&J6 41 | ARM_CIRCULAR_PATTERN_COORD_NUM_UPDATE_CTRL=0x158#圆弧模式坐标序号更新指令数据 42 | ARM_GRIPPER_CTRL = 0x159 #夹爪控制指令 43 | #----------------------------------基于V1.5-2版本后---------------------------------------------# 44 | ARM_JOINT_MIT_CTRL_1 = 0x15A 45 | ARM_JOINT_MIT_CTRL_2 = 0x15B 46 | ARM_JOINT_MIT_CTRL_3 = 0x15C 47 | ARM_JOINT_MIT_CTRL_4 = 0x15D 48 | ARM_JOINT_MIT_CTRL_5 = 0x15E 49 | ARM_JOINT_MIT_CTRL_6 = 0x15F 50 | #---------------------------------------------------------------------------------------------# 51 | # 机械臂参数配置与设定指令 52 | # 若指令名称带有反馈、应答; 决策控制单元->机械臂主控 53 | # 若指令名称带有查询、设置; 机械臂主控->决策控制单元 54 | ARM_MASTER_SLAVE_MODE_CONFIG = 0x470 55 | # ARM_MS_LINKAGE_CONFIG = 0x470 #随动主从模式设置指令-联动设置指令 56 | # ARM_MS_FEEDBACK_INSTRUCTION_OFFSET_CONFIG = 0x470#随动主从模式设置指令-反馈指令偏移值 57 | # ARM_MS_CTRL_INSTRUCTION_OFFSET_CONFIG = 0x470#随动主从模式设置指令-控制指令偏移 58 | # ARM_MS_LINKAGE_CTRL_OFFSET_CONFIG = 0x470#随动主从模式设置指令-联动模式控制目标地址偏移值 59 | # 设置为示教输入臂后，主动周期反馈报文 ID 增加偏移（偏移量可设置），模式切换为联动示教输入模式，不响应控制指令，且会主动发送关节模式控制指令； 60 | # 设置为运动输出臂后恢复为常规状态（退出联动示教输入模式，进入待机模式）；未收到此条指令的机械臂默认为此状态 61 | ARM_MOTOR_ENABLE_DISABLE_CONFIG = 0x471 #电机使能指令 62 | # ARM_MOTOR_DISABLE_CONFIG = 0x471 #电机失能指令 63 | # ARM_SEARCH_MOTOR_ANGLE_CONFIG = 0x472 #查询电机角度 64 | ARM_SEARCH_MOTOR_MAX_SPD_ACC_LIMIT = 0x472 #查询电机角度/最大速度/加速度限制 65 | # ARM_SEARCH_MOTOR_MAX_ACC_CONFIG = 0x472 #查询电机最大加速度限制 66 | ARM_FEEDBACK_CURRENT_MOTOR_ANGLE_LIMIT_MAX_SPD = 0x473 #反馈当前电机最大角度限制,最小角度限制,最大关节速度 67 | ARM_MOTOR_ANGLE_LIMIT_MAX_SPD_SET = 0x474 #电机角度限制/最大速度设置指令 68 | ARM_JOINT_CONFIG = 0x475 #关节设置指令 69 | ARM_INSTRUCTION_RESPONSE_CONFIG=0x476#设置指令应答 70 | ARM_FEEDBACK_RESP_SET_INSTRUCTION = 0x476 #设置指令应答反馈 71 | ARM_PARAM_ENQUIRY_AND_CONFIG = 0x477#机械臂参数查询与设置指令 72 | ARM_FEEDBACK_CURRENT_END_VEL_ACC_PARAM = 0x478 #反馈当前末端速度/加速度参数 73 | ARM_END_VEL_ACC_PARAM_CONFIG = 0x479 #末端速度/加速度参数设置指令 74 | ARM_CRASH_PROTECTION_RATING_CONFIG=0x47A#碰撞防护等级设置指令 75 | ARM_CRASH_PROTECTION_RATING_FEEDBACK=0x47B#碰撞防护等级反馈指令 76 | ARM_FEEDBACK_CURRENT_MOTOR_MAX_ACC_LIMIT=0x47C#反馈当前电机最大加速度限制 77 | #----------------------------------基于V1.5-2版本后---------------------------------------------# 78 | ARM_GRIPPER_TEACHING_PENDANT_PARAM_CONFIG = 0x47D 79 | ARM_GRIPPER_TEACHING_PENDANT_PARAM_FEEDBACK = 0x47E 80 | #---------------------------------------------------------------------------------------------# 81 | ARM_FEEDBACK_JOINT_VEL_ACC_1 = 0x481 #反馈当前关节的末端速度/加速度 82 | ARM_FEEDBACK_JOINT_VEL_ACC_2 = 0x482 83 | ARM_FEEDBACK_JOINT_VEL_ACC_3 = 0x483 84 | ARM_FEEDBACK_JOINT_VEL_ACC_4 = 0x484 85 | ARM_FEEDBACK_JOINT_VEL_ACC_5 = 0x485 86 | ARM_FEEDBACK_JOINT_VEL_ACC_6 = 0x486 87 | #灯光控制0x1节点ID，帧ID 0x121 88 | ARM_LIGHT_CTRL = 0x121 #灯光控制指令 89 | #驱动器信息高速反馈，节点ID 0x1~0x6 90 | ARM_INFO_HIGH_SPD_FEEDBACK_1 = 0x251 91 | ARM_INFO_HIGH_SPD_FEEDBACK_2 = 0x252 92 | ARM_INFO_HIGH_SPD_FEEDBACK_3 = 0x253 93 | ARM_INFO_HIGH_SPD_FEEDBACK_4 = 0x254 94 | ARM_INFO_HIGH_SPD_FEEDBACK_5 = 0x255 95 | ARM_INFO_HIGH_SPD_FEEDBACK_6 = 0x256 96 | #驱动器信息低速反馈，节点ID 0x1~0x6 97 | ARM_INFO_LOW_SPD_FEEDBACK_1 = 0x261 98 | ARM_INFO_LOW_SPD_FEEDBACK_2 = 0x262 99 | ARM_INFO_LOW_SPD_FEEDBACK_3 = 0x263 100 | ARM_INFO_LOW_SPD_FEEDBACK_4 = 0x264 101 | ARM_INFO_LOW_SPD_FEEDBACK_5 = 0x265 102 | ARM_INFO_LOW_SPD_FEEDBACK_6 = 0x266 103 | #CAN 升级总线静默模式设定指令 104 | ARM_CAN_UPDATE_SILENT_MODE_CONFIG=0x422 105 | # 固件读取指令 106 | ARM_FIRMWARE_READ = 0x4AF 107 | def __str__(self): 108 | return f"{self.name} (0x{self.value:X})" 109 | def __repr__(self): 110 | return f"{self.name}: 0x{self.value:X}" -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/feedback/__init__.py: -------------------------------------------------------------------------------- 1 | # 导入 feedback 子模块的类 2 | from .arm_feedback_crash_protection_rating import ArmMsgFeedbackCrashProtectionRating 3 | from .arm_feedback_end_pose import ArmMsgFeedBackEndPose 4 | from .arm_feedback_current_motor_angle_limit_max_spd import ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd, ArmMsgFeedbackAllCurrentMotorAngleLimitMaxSpd 5 | from .arm_feedback_current_end_vel_acc_param import ArmMsgFeedbackCurrentEndVelAccParam 6 | from .arm_feedback_current_motor_max_acc_limit import ArmMsgFeedbackCurrentMotorMaxAccLimit, ArmMsgFeedbackAllCurrentMotorMaxAccLimit 7 | from .arm_feedback_joint_vel_acc import ArmMsgFeedbackJointVelAcc, ArmMsgFeedbackAllJointVelAcc 8 | #----------------------------------基于V1.5-2版本后---------------------------------------------# 9 | from .arm_feedback_gripper_teaching_param import ArmMsgFeedbackGripperTeachingPendantParam 10 | #---------------------------------------------------------------------------------------------# 11 | from .arm_feedback_high_spd import ArmMsgFeedbackHighSpd 12 | from .arm_feedback_joint_states import ArmMsgFeedBackJointStates 13 | from .arm_feedback_low_spd import ArmMsgFeedbackLowSpd 14 | from .arm_feedback_status import ArmMsgFeedbackStatus, ArmMsgFeedbackStatusEnum 15 | from .arm_feedback_gripper import ArmMsgFeedBackGripper 16 | from .arm_feedback_set_instruction_response import ArmMsgFeedbackRespSetInstruction 17 | 18 | __all__ = [ 19 | # 反馈 20 | 'ArmMsgFeedBackEndPose', 21 | 'ArmMsgFeedBackJointStates', 22 | 'ArmMsgFeedbackStatus', 23 | 'ArmMsgFeedbackStatusEnum', 24 | 'ArmMsgFeedBackGripper', 25 | 'ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd', 26 | 'ArmMsgFeedbackCurrentEndVelAccParam', 27 | 'ArmMsgFeedbackCurrentMotorMaxAccLimit', 28 | 'ArmMsgFeedbackJointVelAcc', 29 | 'ArmMsgFeedbackAllCurrentMotorAngleLimitMaxSpd', 30 | 'ArmMsgFeedbackAllCurrentMotorMaxAccLimit', 31 | 'ArmMsgFeedbackAllJointVelAcc', 32 | 'ArmMsgFeedbackCrashProtectionRating', 33 | 'ArmMsgFeedbackHighSpd', 34 | 'ArmMsgFeedbackLowSpd', 35 | 'ArmMsgFeedbackGripperTeachingPendantParam', 36 | 'ArmMsgFeedbackRespSetInstruction', 37 | ] 38 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/feedback/arm_feedback_crash_protection_rating.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | 4 | class ArmMsgFeedbackCrashProtectionRating: 5 | ''' 6 | msg_v2_feedback 7 | 8 | 碰撞防护等级反馈指令 9 | 10 | 0x477 Byte 0 = 0x02 feedback 11 | 12 | CAN ID: 13 | 0x47B 14 | 15 | Args: 16 | joint_1_protection_level: 1号关节碰撞防护等级 17 | joint_2_protection_level: 2号关节碰撞防护等级 18 | joint_3_protection_level: 3号关节碰撞防护等级 19 | joint_4_protection_level: 4号关节碰撞防护等级 20 | joint_5_protection_level: 5号关节碰撞防护等级 21 | joint_6_protection_level: 6号关节碰撞防护等级 22 | 23 | 设定值 : 0~8 24 | 25 | 等级 0 代表不检测碰撞； 6个关节可以独立设置 26 | 27 | 位描述: 28 | 29 | Byte 0: 1 号关节碰撞防护等级, uint8 30 | Byte 1: 2 号关节碰撞防护等级, uint8 31 | Byte 2: 3 号关节碰撞防护等级, uint8 32 | Byte 3: 4 号关节碰撞防护等级, uint8 33 | Byte 4: 5 号关节碰撞防护等级, uint8 34 | Byte 5: 6 号关节碰撞防护等级, uint8 35 | Byte 6: 保留 36 | Byte 7: 保留 37 | ''' 38 | 39 | ''' 40 | msg_v2_feedback 41 | 42 | Get the collision protection level feedback for each joint. 43 | 44 | 0x477 Byte 0 = 0x02 feedback 45 | 46 | CAN ID: 47 | 0x47B 48 | 49 | Args: 50 | joint_1_protection_level (int): Collision protection level for joint 1 (0-8) 51 | joint_2_protection_level (int): Collision protection level for joint 2 (0-8) 52 | joint_3_protection_level (int): Collision protection level for joint 3 (0-8) 53 | joint_4_protection_level (int): Collision protection level for joint 4 (0-8) 54 | joint_5_protection_level (int): Collision protection level for joint 5 (0-8) 55 | joint_6_protection_level (int): Collision protection level for joint 6 (0-8) 56 | 57 | Level Description: 58 | 0: No collision detection 59 | 1-8: Collision detection thresholds increase (higher values represent more sensitive thresholds) 60 | 61 | Byte Description: 62 | 63 | Byte 0: Collision protection level for joint 1, uint8 64 | Byte 1: Collision protection level for joint 2, uint8 65 | Byte 2: Collision protection level for joint 3, uint8 66 | Byte 3: Collision protection level for joint 4, uint8 67 | Byte 4: Collision protection level for joint 5, uint8 68 | Byte 5: Collision protection level for joint 6, uint8 69 | Byte 6: Reserved 70 | Byte 7: Reserved 71 | ''' 72 | def __init__(self, 73 | joint_1_protection_level: int = 0, 74 | joint_2_protection_level: int = 0, 75 | joint_3_protection_level: int = 0, 76 | joint_4_protection_level: int = 0, 77 | joint_5_protection_level: int = 0, 78 | joint_6_protection_level: int = 0 79 | ): 80 | self.joint_1_protection_level = joint_1_protection_level 81 | self.joint_2_protection_level = joint_2_protection_level 82 | self.joint_3_protection_level = joint_3_protection_level 83 | self.joint_4_protection_level = joint_4_protection_level 84 | self.joint_5_protection_level = joint_5_protection_level 85 | self.joint_6_protection_level = joint_6_protection_level 86 | 87 | def __str__(self): 88 | ''' 89 | 返回对象的字符串表示，用于打印。 90 | ''' 91 | ''' 92 | Return the string representation of the object for printing. 93 | ''' 94 | return (f"ArmMsgFeedbackCrashProtectionRating(\n" 95 | f" joint_1_protection_level: {self.joint_1_protection_level}\n" 96 | f" joint_2_protection_level: {self.joint_2_protection_level}\n" 97 | f" joint_3_protection_level: {self.joint_3_protection_level}\n" 98 | f" joint_4_protection_level: {self.joint_4_protection_level}\n" 99 | f" joint_5_protection_level: {self.joint_5_protection_level}\n" 100 | f" joint_6_protection_level: {self.joint_6_protection_level}\n" 101 | f")") 102 | 103 | def __repr__(self): 104 | ''' 105 | 返回对象的正式字符串表示，通常用于调试。 106 | 107 | :return: 对象的字符串表示，与 __str__ 相同。 108 | ''' 109 | ''' 110 | Return the formal string representation of the object, typically used for debugging. 111 | 112 | :return: The string representation of the object, identical to `__str__`. 113 | ''' 114 | return self.__str__() 115 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/feedback/arm_feedback_current_end_vel_acc_param.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | import math 4 | class ArmMsgFeedbackCurrentEndVelAccParam: 5 | ''' 6 | msg_v2_feedback 7 | 8 | 反馈当前末端速度/加速度参数 9 | 10 | 0x477 Byte 0 = 0x01 feedback 11 | 12 | CAN ID: 13 | 0x478 14 | 15 | Args: 16 | end_max_linear_vel: 末端最大线速度 17 | end_max_angular_vel: 末端最大角速度 18 | end_max_linear_acc: 末端最大线加速度 19 | end_max_angular_acc: 末端最大角加速度 20 | 21 | 位描述: 22 | 23 | Byte 0: 末端最大线速度 H, uint16, 单位 0.001m/s 24 | Byte 1: 末端最大线速度 L, 25 | Byte 2: 末端最大角速度 H, uint16, 单位 0.001rad/s 26 | Byte 3: 末端最大角速度 L, 27 | Byte 4: 末端最大线加速度 H, uint16, 单位 0.001m/s^2 28 | Byte 5: 末端最大线加速度 L 29 | Byte 6: 末端最大角加速度 H, uint16, 单位 0.001rad/s^2 30 | Byte 7: 末端最大角加速度 L 31 | ''' 32 | ''' 33 | msg_v2_feedback 34 | 35 | Feedback of Current End-Effector Speed/Acceleration Parameters 36 | 37 | 0x477 Byte 0 = 0x01 feedback 38 | 39 | CAN ID: 40 | 0x478 41 | 42 | Args: 43 | end_max_linear_vel: Maximum linear velocity of the end-effector. 44 | end_max_angular_vel: Maximum angular velocity of the end-effector. 45 | end_max_linear_acc: Maximum linear acceleration of the end-effector. 46 | end_max_angular_acc: Maximum angular acceleration of the end-effector. 47 | 48 | Bit Description: 49 | 50 | Byte 0: Maximum Linear Velocity (High Byte), uint16, unit: 0.001 m/s 51 | Byte 1: Maximum Linear Velocity (Low Byte) 52 | Byte 2: Maximum Angular Velocity (High Byte), uint16, unit: 0.001 rad/s 53 | Byte 3: Maximum Angular Velocity (Low Byte) 54 | Byte 4: Maximum Linear Acceleration (High Byte), uint16, unit: 0.001 m/s² 55 | Byte 5: Maximum Linear Acceleration (Low Byte) 56 | Byte 6: Maximum Angular Acceleration (High Byte), uint16, unit: 0.001 rad/s² 57 | Byte 7: Maximum Angular Acceleration (Low Byte) 58 | ''' 59 | def __init__(self, 60 | end_max_linear_vel: int = 0, 61 | end_max_angular_vel: int = 0, 62 | end_max_linear_acc: int = 0, 63 | end_max_angular_acc: int = 0 64 | ): 65 | self.end_max_linear_vel = end_max_linear_vel 66 | self.end_max_angular_vel = end_max_angular_vel 67 | self.end_max_linear_acc = end_max_linear_acc 68 | self.end_max_angular_acc = end_max_angular_acc 69 | 70 | def __str__(self): 71 | return (f"ArmMsgFeedbackCurrentEndVelAccParam(\n" 72 | f" end_max_linear_vel: {self.end_max_linear_vel}\n" 73 | f" end_max_angular_vel: {self.end_max_angular_vel}\n" 74 | f" end_max_linear_acc: {self.end_max_linear_acc }\n" 75 | f" end_max_angular_acc: {self.end_max_angular_acc}\n" 76 | f")") 77 | 78 | def __repr__(self): 79 | return self.__str__() 80 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/feedback/arm_feedback_current_motor_angle_limit_max_spd.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | class ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd: 7 | ''' 8 | msg_v2_feedback 9 | 10 | 反馈当前电机限制角度/最大速度 11 | 12 | CAN ID: 13 | 0x473 14 | 15 | Args: 16 | motor_num: 关节电机序号 17 | max_angle_limit: 最大角度限制 18 | min_angle_limit: 最小角度限制 19 | max_joint_spd: 最大关节速度 20 | 21 | 位描述: 22 | 23 | Byte 0: 关节电机序号, uint8 24 | Byte 1: 最大角度限制H,uint16, 单位 0.1度 25 | Byte 2: 最大角度限制L 26 | Byte 3: 最小角度限制H, uint16, 单位 0.1度 27 | Byte 4: 最小角度限制L 28 | Byte 5: 最大关节速度H, uint16, 单位 0.001rad/s 29 | Byte 6: 最大关节速度L 30 | Byte 7: 保留 31 | ''' 32 | ''' 33 | msg_v2_feedback 34 | 35 | Feedback on Current Motor Angle Limits/Maximum Speed 36 | 37 | CAN ID: 38 | 0x473 39 | 40 | Args: 41 | motor_num: Joint motor number. 42 | max_angle_limit: Maximum angle limit. 43 | min_angle_limit: Minimum angle limit. 44 | max_joint_spd: Maximum joint speed. 45 | 46 | Bit Description: 47 | 48 | Byte 0: Joint Motor Index, uint8 49 | Byte 1: Maximum Angle Limit (High Byte), uint16, unit: 0.1° 50 | Byte 2: Maximum Angle Limit (Low Byte) 51 | Byte 3: Minimum Angle Limit (High Byte), uint16, unit: 0.1° 52 | Byte 4: Minimum Angle Limit (Low Byte) 53 | Byte 5: Maximum Joint Speed (High Byte), uint16, unit: 0.001 rad/s 54 | Byte 6: Maximum Joint Speed (Low Byte) 55 | Byte 7: Reserved 56 | ''' 57 | def __init__(self, 58 | motor_num: Literal[0, 1, 2, 3, 4, 5, 6] = 0, 59 | max_angle_limit: int = 0, 60 | min_angle_limit: int = 0, 61 | max_joint_spd: int = 0): 62 | if motor_num not in [0, 1, 2, 3, 4, 5, 6]: 63 | raise ValueError(f"'motor_num' Value {motor_num} out of range [1, 2, 3, 4, 5, 6]") 64 | self.motor_num = motor_num 65 | self.max_angle_limit = max_angle_limit 66 | self.min_angle_limit = min_angle_limit 67 | self.max_joint_spd = max_joint_spd 68 | 69 | def __str__(self): 70 | return (f"ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd(\n" 71 | f" motor_num: {self.motor_num}\n" 72 | f" max_angle_limit: {self.max_angle_limit}\n" 73 | f" min_angle_limit: {self.min_angle_limit}\n" 74 | f" max_joint_spd: {self.max_joint_spd}\n" 75 | f")") 76 | 77 | def __repr__(self): 78 | return self.__str__() 79 | 80 | class ArmMsgFeedbackAllCurrentMotorAngleLimitMaxSpd: 81 | ''' 82 | 反馈所有电机限制角度/最大速度 83 | 84 | CAN ID: 85 | 0x473 86 | 87 | :param m1: 电机1的限制角度/最大速度 88 | :param m2: 电机2的限制角度/最大速度 89 | :param m3: 电机3的限制角度/最大速度 90 | :param m4: 电机4的限制角度/最大速度 91 | :param m5: 电机5的限制角度/最大速度 92 | :param m6: 电机6的限制角度/最大速度 93 | ''' 94 | ''' 95 | Feedback on Current Motor Angle Limits/Maximum Speed 96 | 97 | CAN ID: 98 | 0x473 99 | 100 | :param m1: Limit angle/maximum speed for motor 1. 101 | :param m2: Limit angle/maximum speed for motor 2. 102 | :param m3: Limit angle/maximum speed for motor 3. 103 | :param m4: Limit angle/maximum speed for motor 4. 104 | :param m5: Limit angle/maximum speed for motor 5. 105 | :param m6: Limit angle/maximum speed for motor 6. 106 | ''' 107 | def __init__(self, 108 | m1:ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd = ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd(0,0,0,0), 109 | m2:ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd = ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd(0,0,0,0), 110 | m3:ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd = ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd(0,0,0,0), 111 | m4:ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd = ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd(0,0,0,0), 112 | m5:ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd = ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd(0,0,0,0), 113 | m6:ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd = ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd(0,0,0,0)): 114 | self.__m = [ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd(0,0,0,0), m1, m2, m3, m4, m5, m6] 115 | self.motor = [ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd() for _ in range(7)] 116 | self.motor[0] = ArmMsgFeedbackCurrentMotorAngleLimitMaxSpd(0,0,0,0) 117 | 118 | def assign(self): 119 | for i in range(1,7): 120 | if(self.__m[i].motor_num != 0): 121 | self.motor[i] = self.__m[i] 122 | 123 | def __str__(self): 124 | return (f"{self.motor[1]}\n" 125 | f"{self.motor[2]}\n" 126 | f"{self.motor[3]}\n" 127 | f"{self.motor[4]}\n" 128 | f"{self.motor[5]}\n" 129 | f"{self.motor[6]}\n" 130 | f")") 131 | 132 | def __repr__(self): 133 | return self.__str__() -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/feedback/arm_feedback_current_motor_max_acc_limit.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | class ArmMsgFeedbackCurrentMotorMaxAccLimit: 7 | ''' 8 | msg_v2_feedback 9 | 10 | 反馈当前电机最大加速度限制 11 | 12 | CAN ID: 13 | 0x47C 14 | 15 | Args: 16 | joint_motor_num: 关节电机序号 17 | max_joint_acc: 最大关节加速度 18 | 19 | 位描述: 20 | 21 | Byte 0: 关节序号, uint8, 值域 1-6(1-6 代表关节驱动器序号) 22 | Byte 1: 最大关节加速度 H, uint16, 单位 0.001rad/^2 23 | Byte 2: 最大关节加速度 L 24 | ''' 25 | ''' 26 | msg_v2_feedback 27 | 28 | Feedback on Current Motor Maximum Acceleration Limit 29 | 30 | CAN ID: 31 | 0x47C 32 | 33 | Args: 34 | joint_motor_num: Joint motor number. 35 | max_joint_acc: Maximum joint acceleration. 36 | 37 | Bit Description: 38 | 39 | Byte 0: Joint Index, uint8, range 1-6(1-6 represent the joint motor index) 40 | Byte 1: Maximum Joint Acceleration (High Byte), uint16, unit: 0.001rad/^2 41 | Byte 2: Maximum Joint Acceleration (Low Byte) 42 | ''' 43 | def __init__(self, 44 | joint_motor_num: Literal[0, 1, 2, 3, 4, 5, 6] = 0, 45 | max_joint_acc: int = 0 46 | ): 47 | if joint_motor_num not in [0, 1, 2, 3, 4, 5, 6]: 48 | raise ValueError(f"'joint_motor_num' Value {joint_motor_num} out of range [1, 2, 3, 4, 5, 6]") 49 | self.joint_motor_num = joint_motor_num 50 | self.max_joint_acc = max_joint_acc 51 | 52 | def __str__(self): 53 | return (f"ArmMsgFeedbackCurrentMotorMaxAccLimit(\n" 54 | f" joint_motor_num: {self.joint_motor_num}\n" 55 | f" max_joint_acc: {self.max_joint_acc}\n" 56 | f")") 57 | 58 | def __repr__(self): 59 | return self.__str__() 60 | 61 | class ArmMsgFeedbackAllCurrentMotorMaxAccLimit: 62 | ''' 63 | 反馈全部电机最大加速度限制 64 | 65 | CAN ID: 66 | 0x47C 67 | 68 | :param m1: 电机1的最大加速度限制 69 | :param m2: 电机2的最大加速度限制 70 | :param m3: 电机3的最大加速度限制 71 | :param m4: 电机4的最大加速度限制 72 | :param m5: 电机5的最大加速度限制 73 | :param m6: 电机6的最大加速度限制 74 | ''' 75 | ''' 76 | Feedback on Current Motor Maximum Acceleration Limit 77 | 78 | CAN ID: 79 | 0x47C 80 | 81 | :param m1: Maximum acceleration limit for motor 1. 82 | :param m2: Maximum acceleration limit for motor 2. 83 | :param m3: Maximum acceleration limit for motor 3. 84 | :param m4: Maximum acceleration limit for motor 4. 85 | :param m5: Maximum acceleration limit for motor 5. 86 | :param m6: Maximum acceleration limit for motor 6. 87 | ''' 88 | def __init__(self, 89 | m1:ArmMsgFeedbackCurrentMotorMaxAccLimit=ArmMsgFeedbackCurrentMotorMaxAccLimit(0,0), 90 | m2:ArmMsgFeedbackCurrentMotorMaxAccLimit=ArmMsgFeedbackCurrentMotorMaxAccLimit(0,0), 91 | m3:ArmMsgFeedbackCurrentMotorMaxAccLimit=ArmMsgFeedbackCurrentMotorMaxAccLimit(0,0), 92 | m4:ArmMsgFeedbackCurrentMotorMaxAccLimit=ArmMsgFeedbackCurrentMotorMaxAccLimit(0,0), 93 | m5:ArmMsgFeedbackCurrentMotorMaxAccLimit=ArmMsgFeedbackCurrentMotorMaxAccLimit(0,0), 94 | m6:ArmMsgFeedbackCurrentMotorMaxAccLimit=ArmMsgFeedbackCurrentMotorMaxAccLimit(0,0) 95 | ): 96 | self.__m = [ArmMsgFeedbackCurrentMotorMaxAccLimit(0,0), m1, m2, m3, m4, m5, m6] 97 | self.motor = [ArmMsgFeedbackCurrentMotorMaxAccLimit(0,0) for _ in range(7)] 98 | 99 | def assign(self): 100 | for i in range(1,7): 101 | if(self.__m[i].joint_motor_num != 0): 102 | self.motor[i] = self.__m[i] 103 | 104 | def __str__(self): 105 | return (f"{self.motor[1]}\n" 106 | f"{self.motor[2]}\n" 107 | f"{self.motor[3]}\n" 108 | f"{self.motor[4]}\n" 109 | f"{self.motor[5]}\n" 110 | f"{self.motor[6]}\n" 111 | f")") 112 | 113 | def __repr__(self): 114 | return self.__str__() -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/feedback/arm_feedback_end_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | 4 | class ArmMsgFeedBackEndPose(): 5 | ''' 6 | msg_v2_feedback 7 | 8 | 机械臂末端姿态反馈,单位0.001mm 9 | 10 | CAN ID: 11 | 0x152、0x153、0x154 12 | 13 | Args: 14 | X_axis: X坐标 15 | Y_axis: Y坐标 16 | Z_axis: Z坐标 17 | RX_axis: RX角度 18 | RY_axis: RY角度 19 | RZ_axis: RZ角度 20 | ''' 21 | ''' 22 | msg_v2_feedback 23 | 24 | End-Effector Pose Feedback for the Robotic Arm, unit: 0.001 mm. 25 | 26 | CAN ID: 27 | 0x152、0x153、0x154 28 | 29 | Args: 30 | X_axis: X-coordinate. 31 | Y_axis: Y-coordinate. 32 | Z_axis: Z-coordinate. 33 | RX_axis: Rotation angle around the X-axis (RX). 34 | RY_axis: Rotation angle around the Y-axis (RY). 35 | RZ_axis: Rotation angle around the Z-axis (RZ). 36 | ''' 37 | def __init__(self, 38 | X_axis: int = 0, 39 | Y_axis: int = 0, 40 | Z_axis: int = 0, 41 | RX_axis: int = 0, 42 | RY_axis: int = 0, 43 | RZ_axis: int = 0): 44 | self.X_axis = X_axis 45 | self.Y_axis = Y_axis 46 | self.Z_axis = Z_axis 47 | self.RX_axis = RX_axis 48 | self.RY_axis = RY_axis 49 | self.RZ_axis = RZ_axis 50 | 51 | def __str__(self): 52 | # 将角度乘以0.001，并保留三位小数 53 | end_pose = [ 54 | (" X_axis ", self.X_axis), 55 | (" Y_axis ", self.Y_axis), 56 | (" Z_axis ", self.Z_axis), 57 | (" RX_axis ", self.RX_axis), 58 | (" RY_axis ", self.RY_axis), 59 | (" RZ_axis ", self.RZ_axis) 60 | ] 61 | 62 | # 生成格式化字符串，保留三位小数 63 | formatted_ = "\n".join([f"{name}: {pose}" for name, pose in end_pose]) 64 | 65 | return f"ArmMsgFeedBackEndPose:\n{formatted_}" 66 | 67 | def __repr__(self): 68 | return self.__str__() -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/feedback/arm_feedback_gripper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | 4 | class ArmMsgFeedBackGripper: 5 | ''' 6 | msg_v2_feedback 7 | 8 | 夹爪反馈消息 9 | 10 | CAN ID: 11 | 0x2A8 12 | 13 | Args: 14 | grippers_angle: 夹爪行程，以整数表示。 15 | grippers_effort: 夹爪扭矩，以整数表示。 16 | status_code: 夹爪状态码，以整数表示。 17 | 18 | 位描述: 19 | 20 | Byte 0: 夹爪行程最高位, int32, 单位 0.001mm 21 | Byte 1: 22 | Byte 2: 23 | Byte 3: 24 | Byte 4: 夹爪扭矩 H, int16, 单位 0.001N/m 25 | Byte 5: 夹爪扭矩 L 26 | Byte 6: 状态码, uint8 27 | bit[0] 电源电压是否过低(0:正常 1:过低) 28 | bit[1] 电机是否过温(0:正常 1:过温) 29 | bit[2] 驱动器是否过流(0:正常 1:过流) 30 | bit[3] 驱动器是否过温(0:正常 1:过温) 31 | bit[4] 传感器状态(0:正常 1:异常) 32 | bit[5] 驱动器错误状态(0:正常 1:错误) 33 | bit[6] 驱动器使能状态(1:使能 0:失能) 34 | bit[7] 回零状态(0:没有回零 1:已经回零,或已经回过零) 35 | Byte 7: 保留 36 | ''' 37 | ''' 38 | msg_v2_feedback 39 | 40 | Gripper Feedback Message 41 | 42 | CAN ID: 43 | 0x2A8 44 | 45 | Args: 46 | grippers_angle: The stroke of the gripper, represented as an integer. 47 | grippers_effort: The torque of the gripper, represented as an integer. 48 | status_code: The status code of the gripper, represented as an integer. 49 | 50 | Bit Definitions: 51 | 52 | Byte Definitions: 53 | Byte 0: Gripper Stroke (Most Significant Byte), int32, unit: 0.001 mm 54 | Byte 1: Gripper Stroke (Second Most Significant Byte) 55 | Byte 2: Gripper Stroke (Second Least Significant Byte) 56 | Byte 3: Gripper Stroke (Least Significant Byte) 57 | Byte 4: Gripper Torque (High Byte), int16, unit: 0.001 N·m 58 | Byte 5: Gripper Torque (Low Byte) 59 | Byte 6: Status Code, uint8: 60 | bit[0]: Power voltage low (0: Normal, 1: Low) 61 | bit[1]: Motor over-temperature (0: Normal, 1: Over-temperature) 62 | bit[2]: Driver over-current (0: Normal, 1: Over-current) 63 | bit[3]: Driver over-temperature (0: Normal, 1: Over-temperature) 64 | bit[4]: Sensor status (0: Normal, 1: Abnormal) 65 | bit[5]: Driver error status (0: Normal, 1: Error) 66 | bit[6]: Driver enable status (1: Enabled, 0: Disabled) 67 | bit[7]: Zeroing status (0: Not zeroed, 1: Zeroed or previously zeroed) 68 | Byte 7: Reserved 69 | ''' 70 | def __init__(self, 71 | grippers_angle: int = 0, 72 | grippers_effort: int = 0, 73 | status_code: int = 0): 74 | self.grippers_angle = grippers_angle 75 | self.grippers_effort = grippers_effort 76 | self._status_code = status_code 77 | self.foc_status = self.FOC_Status() 78 | 79 | class FOC_Status: 80 | def __init__(self): 81 | self.voltage_too_low = False 82 | self.motor_overheating = False 83 | self.driver_overcurrent = False 84 | self.driver_overheating = False 85 | self.sensor_status = False 86 | self.driver_error_status = False 87 | self.driver_enable_status = False 88 | self.homing_status = False 89 | def __str__(self): 90 | return (f" voltage_too_low : {self.voltage_too_low}\n" 91 | f" motor_overheating: {self.motor_overheating}\n" 92 | f" driver_overcurrent: {self.driver_overcurrent}\n" 93 | f" driver_overheating: {self.driver_overheating}\n" 94 | f" sensor_status: {self.sensor_status}\n" 95 | f" driver_error_status: {self.driver_error_status}\n" 96 | f" driver_enable_status: {self.driver_enable_status}\n" 97 | f" homing_status: {self.homing_status}\n" 98 | ) 99 | @property 100 | def status_code(self): 101 | return self._status_code 102 | 103 | @status_code.setter 104 | def status_code(self, value: int): 105 | if not (0 <= value < 2**8): 106 | raise ValueError("status_code must be an 8-bit integer between 0 and 255.") 107 | self._status_code = value 108 | # Update foc_status based on the status_code bits 109 | self.foc_status.voltage_too_low = bool(value & (1 << 0)) 110 | self.foc_status.motor_overheating = bool(value & (1 << 1)) 111 | self.foc_status.driver_overcurrent = bool(value & (1 << 2)) 112 | self.foc_status.driver_overheating = bool(value & (1 << 3)) 113 | self.foc_status.sensor_status = bool(value & (1 << 4)) 114 | self.foc_status.driver_error_status = bool(value & (1 << 5)) 115 | self.foc_status.driver_enable_status = bool(value & (1 << 6)) 116 | self.foc_status.homing_status = bool(value & (1 << 7)) 117 | 118 | def __str__(self): 119 | return (f"ArmMsgFeedBackGripper(\n" 120 | f" grippers_angle: {self.grippers_angle}, {self.grippers_angle * 0.001:.3f},\n" 121 | f" grippers_effort: {self.grippers_effort} \t {self.grippers_effort * 0.001:.3f},\n" 122 | f" status_code: \n{self.foc_status}\n" 123 | f")") 124 | 125 | def __repr__(self): 126 | return self.__str__() 127 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/feedback/arm_feedback_gripper_teaching_param.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | class ArmMsgFeedbackGripperTeachingPendantParam: 7 | ''' 8 | msg_v2_feedback 9 | 10 | 夹爪/示教器参数反馈指令(基于V1.5-2版本后) 11 | 12 | CAN ID: 13 | 0x47E 14 | 15 | Args: 16 | teaching_range_per: 示教器行程系数反馈,[100~200] 17 | max_range_config: 夹爪/示教器最大控制行程限制值反馈,[0,70,100] 18 | teaching_friction: 示教器摩擦系数设置,范围[1, 10] ----- (基于V1.5-8版本及以后) 19 | 20 | 位描述: 21 | 22 | Byte 0 示教器行程系数反馈, uint8 23 | 示教器行程系数反馈---100~200,单位(%)(默认100%) 24 | 仅适用于设置主从臂的主臂，用于放大控制行程给从臂 25 | Byte 1 夹爪/示教器最大控制行程限制值反馈, uint8, 单位(mm) 26 | 无效值---0 27 | 小夹爪为---70mm 28 | 大夹爪为---100mm 29 | Byte 2 示教器摩擦系数设置, uint8, 范围[1, 10] ----- (基于V1.5-8版本及以后) 30 | Byte 3 保留 31 | Byte 4 保留 32 | Byte 5 保留 33 | Byte 6 保留 34 | Byte 7 保留 35 | ''' 36 | ''' 37 | msg_v2_feedback 38 | 39 | Gripper/Teaching Pendant Parameter Feedback(Based on version V1.5-2 and later) 40 | 41 | CAN ID: 42 | 0x47E 43 | 44 | Args: 45 | teaching_range_per: Teaching pendant stroke coefficient setting.[100~200] 46 | max_range_config: Maximum control stroke limit setting for the gripper/teaching pendant.[0,70,100] 47 | teaching_friction: Teaching pendant friction coefficient setting,range [1, 10].(Based on version V1.5-8 and later) 48 | 49 | Bit Description: 50 | 51 | Byte Field Type Details 52 | Byte 0 Teaching range coefficient uint8 Stroke coefficient setting: 100% to 200% (default: 100%). 53 | Only applicable to the master arm in a master-slave setup to scale control range for the slave arm. 54 | Byte 1 Max control stroke limit uint8 Invalid value: 0 55 | Small gripper: 70 mm 56 | Large gripper: 100 mm 57 | Byte 2 Teaching pendant friction coefficient setting, `uint8`, range [1, 10].(Based on version V1.5-8 and later) 58 | Byte 3 Reserved - Reserved for future use. 59 | Byte 4 Reserved - Reserved for future use. 60 | Byte 5 Reserved - Reserved for future use. 61 | Byte 6 Reserved - Reserved for future use. 62 | Byte 7 Reserved - Reserved for future use. 63 | ''' 64 | def __init__(self, 65 | teaching_range_per: int = 0, 66 | max_range_config: int = 0, 67 | teaching_friction: int = 0): 68 | self.teaching_range_per = teaching_range_per 69 | self.max_range_config = max_range_config 70 | self.teaching_friction = teaching_friction 71 | 72 | def __str__(self): 73 | return (f"ArmMsgFeedbackGripperTeachingPendantParam(\n" 74 | f" teaching_range_per: {self.teaching_range_per}\n" 75 | f" max_range_config: {self.max_range_config}\n" 76 | f" teaching_friction: {self.teaching_friction}\n" 77 | f")") 78 | 79 | def __repr__(self): 80 | return self.__str__() 81 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/feedback/arm_feedback_high_spd.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | import math 4 | from typing_extensions import ( 5 | Literal, 6 | ) 7 | class ArmMsgFeedbackHighSpd: 8 | ''' 9 | msg_v2_feedback 10 | 11 | 驱动器信息高速反馈 0x5 12 | 13 | 节点 ID: 14 | 0x1~0x06 15 | 16 | CAN ID: 17 | 0X251~0x256 18 | 19 | Args: 20 | can_id: 当前canid,用来代表关节序号 21 | motor_speed: 电机当前转速 22 | current: 电机当前电流 23 | pos: 电机当前位置 24 | effort: 经过固定系数转换的力矩,单位0.001N/m 25 | 26 | 位描述: 27 | 28 | Byte 0: 转速高八位, int16, 电机当前转速 单位: 0.001rad/s 29 | Byte 1: 转速低八位 30 | Byte 2: 电流高八位, uint16, 电机当前电流 单位: 0.001A 31 | Byte 3: 电流低八位 32 | Byte 4: 位置最高位, int32, 电机当前位置 单位: rad 33 | Byte 5: 位置次高位 34 | Byte 6: 位置次低位 35 | Byte 7: 位置最低位 36 | ''' 37 | ''' 38 | msg_v2_feedback 39 | 40 | High-Speed Feedback of Drive Information 0x5 41 | 42 | Node ID: 43 | 0x1~0x06 44 | 45 | CAN ID: 46 | 0x251~0x256 47 | 48 | Args: 49 | can_id: Current CAN ID, used to represent the joint number. 50 | motor_speed: Motor Speed. 51 | current: Motor Current. 52 | pos: Motor Position. 53 | effort: Torque converted using a fixed coefficient, with a unit of 0.001 N/m. 54 | 55 | 56 | Bit Description: 57 | 58 | Byte 0: Motor Speed (High Byte), int16, unit: 0.001rad/s 59 | Byte 1: Motor Speed (Low Byte) 60 | Byte 2: Motor Current (High Byte), uint16, unit: 0.001A 61 | Byte 3: Motor Current (Low Byte) 62 | Byte 4: Motor Position (Most Significant Byte), int32, unit: rad 63 | Byte 5: Motor Position (Second Most Significant Byte) 64 | Byte 6: Motor Position (Second Least Significant Byte) 65 | Byte 7: Motor Position (Least Significant Byte) 66 | ''' 67 | def __init__(self, 68 | can_id: Literal[0x000, 0x251, 0x252, 0x253, 0x254, 0x255, 0x256] = 0, 69 | motor_speed: int = 0, 70 | current: int = 0, 71 | pos: int = 0, 72 | effort: float = 0 73 | ): 74 | if can_id not in [0x000, 0x251, 0x252, 0x253, 0x254, 0x255, 0x256]: 75 | raise ValueError(f"'can_id' Value {can_id} out of range [0x000, 0x251, 0x252, 0x253, 0x254, 0x255, 0x256]") 76 | self._COEFFICIENT_1_3 = 1.18125 77 | self._COEFFICIENT_4_6 = 0.95844 78 | self.can_id = can_id 79 | self.motor_speed = motor_speed 80 | self.current = current 81 | self.pos = pos 82 | self.effort = effort 83 | 84 | def cal_effort(self, current: int = None)-> float: 85 | current_ = 0 86 | if(current is None): 87 | current_ = self.current 88 | elif(isinstance(current, (int, float))): 89 | current_ = current 90 | else: 91 | raise TypeError(f"current {current} is not 'int' or 'float'.") 92 | if(self.can_id in [0x251, 0x252, 0x253]): 93 | self.effort = current_ * self._COEFFICIENT_1_3 94 | elif(self.can_id in [0x254, 0x255, 0x256]): 95 | self.effort = current_ * self._COEFFICIENT_4_6 96 | return self.effort 97 | 98 | def __str__(self): 99 | return (f"ArmMsgFeedbackHighSpd(\n" 100 | f" can_id: {hex(self.can_id)}\n" 101 | f" motor_speed: {self.motor_speed}\n" 102 | f" current: {self.current}\n" 103 | f" pos: {self.pos}\n" 104 | f" effort: {self.effort}\n" 105 | f")") 106 | 107 | def __repr__(self): 108 | return self.__str__() 109 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/feedback/arm_feedback_joint_states.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | 4 | class ArmMsgFeedBackJointStates(): 5 | ''' 6 | msg_v2_feedback 7 | 8 | 机械臂关节角度反馈,单位0.001度 9 | 10 | CAN ID: 11 | 0x2A5、0x2A6、0x2A7 12 | 13 | Args: 14 | joint_1: 关节1反馈角度 15 | joint_2: 关节2反馈角度 16 | joint_3: 关节3反馈角度 17 | joint_4: 关节4反馈角度 18 | joint_5: 关节5反馈角度 19 | joint_6: 关节6反馈角度 20 | ''' 21 | ''' 22 | msg_v2_feedback 23 | 24 | Joint Angle Feedback for Robotic Arm, in 0.001 Degrees 25 | 26 | CAN ID: 27 | 0x2A5、0x2A6、0x2A7 28 | 29 | Args: 30 | joint_1: Feedback angle of joint 1, in 0.001 degrees. 31 | joint_2: Feedback angle of joint 2, in 0.001 degrees. 32 | joint_3: Feedback angle of joint 3, in 0.001 degrees. 33 | joint_4: Feedback angle of joint 4, in 0.001 degrees. 34 | joint_5: Feedback angle of joint 5, in 0.001 degrees. 35 | joint_6: Feedback angle of joint 6, in 0.001 degrees. 36 | ''' 37 | def __init__(self, 38 | joint_1: int = 0, 39 | joint_2: int = 0, 40 | joint_3: int = 0, 41 | joint_4: int = 0, 42 | joint_5: int = 0, 43 | joint_6: int = 0): 44 | self.joint_1 = joint_1 45 | self.joint_2 = joint_2 46 | self.joint_3 = joint_3 47 | self.joint_4 = joint_4 48 | self.joint_5 = joint_5 49 | self.joint_6 = joint_6 50 | 51 | def __str__(self): 52 | # 将角度乘以0.001，并保留三位小数 53 | joint_angles = [ 54 | ("Joint 1", self.joint_1), 55 | ("Joint 2", self.joint_2), 56 | ("Joint 3", self.joint_3), 57 | ("Joint 4", self.joint_4), 58 | ("Joint 5", self.joint_5), 59 | ("Joint 6", self.joint_6) 60 | ] 61 | 62 | # 生成格式化字符串，保留三位小数 63 | formatted_angles = "\n".join([f"{name}:{angle}" for name, angle in joint_angles]) 64 | 65 | return f"ArmMsgFeedBackJointStates:\n{formatted_angles}" 66 | 67 | def __repr__(self): 68 | return self.__str__() -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/feedback/arm_feedback_joint_vel_acc.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | import math 4 | from typing_extensions import ( 5 | Literal, 6 | ) 7 | class ArmMsgFeedbackJointVelAcc: 8 | ''' 9 | msg_v2_feedback 10 | 11 | 反馈各个关节当前末端速度/加速度 12 | 13 | CAN ID: 14 | 0x481 ~ 0x486 15 | 代表 1~6 号关节 16 | 17 | Args: 18 | can_id: 当前canid,用来代表关节序号 19 | end_linear_vel: 末端线速度 20 | end_angular_vel: 末端角速度 21 | end_linear_acc: 末端线加速度 22 | end_angular_acc: 末端角加速度 23 | 24 | 位描述: 25 | 26 | Byte 0: 末端线速度 H, uint16, 单位 0.001m/s 27 | Byte 1: 末端线速度 L, 28 | Byte 2: 末端角速度 H, uint16, 单位 0.001rad/s 29 | Byte 3: 末端角速度 L, 30 | Byte 4: 末端线加速度 H, uint16, 单位 0.001m/s^2 31 | Byte 5: 末端线加速度 L 32 | Byte 6: 末端角加速度 H, uint16, 单位 0.001rad/s^2 33 | Byte 7: 末端角加速度 L 34 | ''' 35 | ''' 36 | msg_v2_feedback 37 | 38 | Feedback on Current End-Effector Speed/Acceleration for Each Joint 39 | 40 | CAN ID: 41 | 0x481 ~ 0x486 42 | Corresponds to Joints 1~6. 43 | 44 | Args: 45 | can_id: Current CAN ID, used to represent the joint number. 46 | end_linear_vel: End-effector linear velocity. 47 | end_angular_vel: End-effector angular velocity. 48 | end_linear_acc: End-effector linear acceleration. 49 | end_angular_acc: End-effector angular acceleration. 50 | 51 | Bit Description: 52 | 53 | Byte 0: End-Effector Linear Velocity (High Byte), uint16, unit: 0.001 m/s 54 | Byte 1: End-Effector Linear Velocity (Low Byte) 55 | Byte 2: End-Effector Angular Velocity (High Byte), uint16, unit: 0.001 rad/s 56 | Byte 3: End-Effector Angular Velocity (Low Byte) 57 | Byte 4: End-Effector Linear Acceleration (High Byte), uint16, unit: 0.001 m/s² 58 | Byte 5: End-Effector Linear Acceleration (Low Byte) 59 | Byte 6: End-Effector Angular Acceleration (High Byte), uint16, unit: 0.001 rad/s² 60 | Byte 7: End-Effector Angular Acceleration (Low Byte) 61 | ''' 62 | def __init__(self, 63 | can_id: Literal[0, 0x481, 0x482, 0x483, 0x484, 0x485, 0x486] = 0, 64 | end_linear_vel: int = 0, 65 | end_angular_vel: int = 0, 66 | end_linear_acc: int = 0, 67 | end_angular_acc: int = 0 68 | ): 69 | if can_id not in [0, 0x481, 0x482, 0x483, 0x484, 0x485, 0x486]: 70 | raise ValueError(f"'can_id' Value {can_id} out of range [0x481, 0x482, 0x483, 0x484, 0x485, 0x486]") 71 | self.can_id = can_id 72 | self.end_linear_vel = end_linear_vel 73 | self.end_angular_vel = end_angular_vel 74 | self.end_linear_acc = end_linear_acc 75 | self.end_angular_acc = end_angular_acc 76 | 77 | def __str__(self): 78 | return (f"ArmMsgFeedbackJointVelAcc(\n" 79 | f" can_id: {self.can_id}\n" 80 | f" end_linear_vel: {self.end_linear_vel}\n" 81 | f" end_angular_vel: {self.end_angular_vel }\n" 82 | f" end_linear_acc: {self.end_linear_acc }\n" 83 | f" end_angular_acc: {self.end_angular_acc}\n" 84 | f")") 85 | 86 | def __repr__(self): 87 | return self.__str__() 88 | 89 | class ArmMsgFeedbackAllJointVelAcc: 90 | ''' 91 | 反馈全部关节当前末端速度/加速度 92 | 93 | CAN ID: 94 | 0x481 ~ 0x486 95 | 代表 1~6 号关节 96 | 97 | Args: 98 | j1: 电机1的当前末端速度/加速度 99 | j2: 电机2的当前末端速度/加速度 100 | j3: 电机3的当前末端速度/加速度 101 | j4: 电机4的当前末端速度/加速度 102 | j5: 电机5的当前末端速度/加速度 103 | j6: 电机6的当前末端速度/加速度 104 | ''' 105 | ''' 106 | Feedback on Current End-Effector Speed/Acceleration for Each Joint 107 | 108 | CAN ID: 109 | 0x481 ~ 0x486 110 | Corresponds to Joints 1~6. 111 | 112 | Args: 113 | j1: Current end-effector velocity/acceleration for motor 1. 114 | j2: Current end-effector velocity/acceleration for motor 2. 115 | j3: Current end-effector velocity/acceleration for motor 3. 116 | j4: Current end-effector velocity/acceleration for motor 4. 117 | j5: Current end-effector velocity/acceleration for motor 5. 118 | j6: Current end-effector velocity/acceleration for motor 6. 119 | ''' 120 | def __init__(self, 121 | j1:ArmMsgFeedbackJointVelAcc = ArmMsgFeedbackJointVelAcc(0,0,0,0,0), 122 | j2:ArmMsgFeedbackJointVelAcc = ArmMsgFeedbackJointVelAcc(0,0,0,0,0), 123 | j3:ArmMsgFeedbackJointVelAcc = ArmMsgFeedbackJointVelAcc(0,0,0,0,0), 124 | j4:ArmMsgFeedbackJointVelAcc = ArmMsgFeedbackJointVelAcc(0,0,0,0,0), 125 | j5:ArmMsgFeedbackJointVelAcc = ArmMsgFeedbackJointVelAcc(0,0,0,0,0), 126 | j6:ArmMsgFeedbackJointVelAcc = ArmMsgFeedbackJointVelAcc(0,0,0,0,0)): 127 | self.j = [ArmMsgFeedbackJointVelAcc(0,0,0,0,0), j1, j2, j3, j4, j5, j6] 128 | self.joint = [ArmMsgFeedbackJointVelAcc() for _ in range(7)] 129 | self.joint[0] = ArmMsgFeedbackJointVelAcc(0,0,0,0,0) 130 | 131 | def assign(self): 132 | for i in range(1,7): 133 | if(self.j[i].can_id != 0): 134 | self.joint[i] = self.j[i] 135 | 136 | def __str__(self): 137 | return (f"{self.joint[1]}\n" 138 | f"{self.joint[2]}\n" 139 | f"{self.joint[3]}\n" 140 | f"{self.joint[4]}\n" 141 | f"{self.joint[5]}\n" 142 | f"{self.joint[6]}\n" 143 | f")") 144 | 145 | def __repr__(self): 146 | return self.__str__() -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/feedback/arm_feedback_set_instruction_response.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | 7 | class ArmMsgFeedbackRespSetInstruction: 8 | ''' 9 | msg_v2_feedback 10 | 11 | 发送设置指令后的应答帧, 0x4XX开头为设置指令id 12 | 13 | CAN ID: 14 | 0x476 15 | 16 | Args: 17 | instruction_index: 应答指令索引 18 | 取设置指令 id 最后一个字节 19 | 例如,应答 0x471 设置指令时此位填充0x71 20 | is_set_zero_successfully: 零点是否设置成功 21 | 零点成功设置-0x01 22 | 设置失败/未设置-0x00 23 | 仅在关节设置指令--成功设置 N 号电机当前位置为零点时应答-0x01 24 | 25 | 位描述: 26 | 27 | Byte 0: uint8, 应答指令索引 28 | 取设置指令 id 最后一个字节 29 | 例如：应答 0x471 设置指令时此位填充0x71 30 | Byte 1: uint8, 零点是否设置成功 31 | 零点成功设置 : 0x01 32 | 设置失败/未设置: 0x00 33 | 仅在关节设置指令--成功设置 N 号电机当前位置为零点时应答 0x01 34 | ''' 35 | ''' 36 | msg_v2_transmit 37 | 38 | Set Command ResponseThe response frame after sending the setting command, starting with 0x4XX is the setting command ID 39 | 40 | CAN ID: 41 | 0x476 42 | 43 | Args: 44 | instruction_index (int): The response instruction index. 45 | This is derived from the last byte of the set instruction ID. 46 | For example, when responding to the 0x471 set instruction, this would be 0x71. 47 | 48 | is_zero_config_success (int): Flag indicating whether the zero point was successfully set. 49 | 0x01: Zero point successfully set. 50 | 0x00: Zero point set failed/not set. 51 | This is only applicable when responding to a joint setting instruction that successfully sets 52 | motor N's current position as the zero point. 53 | 54 | Bit Description: 55 | 56 | Byte 0: uint8, response instruction index. 57 | Fill in the last byte of the set command ID. 58 | Example: Responding to the 0x471 set command, this byte will be 0x71. 59 | Byte 1: uint8, zero-point configuration success flag. 60 | 0x01: Zero-point successfully set. 61 | 0x00: Failed to set/Not set. 62 | ''' 63 | def __init__(self, 64 | instruction_index: int = -1, 65 | is_set_zero_successfully: int = -1): 66 | self.instruction_index = instruction_index 67 | self.is_set_zero_successfully = is_set_zero_successfully 68 | 69 | def __str__(self): 70 | return (f"ArmMsgFeedbackRespSetInstruction(\n" 71 | f" instruction_index: {hex(self.instruction_index) },\n" 72 | f" is_set_zero_successfully: {self.is_set_zero_successfully },\n" 73 | f")") 74 | 75 | def __repr__(self): 76 | return self.__str__() 77 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | # 导入 transmit 子模块 3 | from .arm_circular_pattern import ArmMsgCircularPatternCoordNumUpdateCtrl 4 | from .arm_crash_protection_rating_config import ArmMsgCrashProtectionRatingConfig 5 | from .arm_end_vel_acc_param_config import ArmMsgEndVelAccParamConfig 6 | from .arm_gripper_ctrl import ArmMsgGripperCtrl 7 | #----------------------------------基于V1.5-2版本后---------------------------------------------# 8 | from .arm_gripper_teaching_param_config import ArmMsgGripperTeachingPendantParamConfig 9 | #---------------------------------------------------------------------------------------------# 10 | from .arm_joint_config import ArmMsgJointConfig 11 | from .arm_joint_ctrl import ArmMsgJointCtrl 12 | #----------------------------------基于V1.5-2版本后---------------------------------------------# 13 | from .arm_joint_mit_ctrl import ArmMsgJointMitCtrl 14 | from .arm_joint_mit_ctrl import ArmMsgAllJointMitCtrl 15 | #---------------------------------------------------------------------------------------------# 16 | from .arm_master_slave_config import ArmMsgMasterSlaveModeConfig 17 | from .arm_motion_ctrl_1 import ArmMsgMotionCtrl_1 18 | from .arm_motion_ctrl_2 import ArmMsgMotionCtrl_2 19 | from .arm_motion_ctrl_cartesian import ArmMsgMotionCtrlCartesian 20 | from .arm_motor_angle_limit_max_spd_config import ArmMsgMotorAngleLimitMaxSpdSet 21 | from .arm_motor_enable_disable import ArmMsgMotorEnableDisableConfig 22 | from .arm_param_enquiry_and_config import ArmMsgParamEnquiryAndConfig 23 | from .arm_search_motor_max_angle_spd_acc_limit import ArmMsgSearchMotorMaxAngleSpdAccLimit 24 | from .arm_set_instruction_response import ArmMsgInstructionResponseConfig 25 | 26 | __all__ = [ 27 | # 发送 28 | 'ArmMsgMotionCtrl_1', 29 | 'ArmMsgMotionCtrl_2', 30 | 'ArmMsgMotionCtrlCartesian', 31 | 'ArmMsgJointCtrl', 32 | 'ArmMsgCircularPatternCoordNumUpdateCtrl', 33 | 'ArmMsgGripperCtrl', 34 | 'ArmMsgMasterSlaveModeConfig', 35 | 'ArmMsgMotorEnableDisableConfig', 36 | 'ArmMsgSearchMotorMaxAngleSpdAccLimit', 37 | 'ArmMsgMotorAngleLimitMaxSpdSet', 38 | 'ArmMsgJointConfig', 39 | 'ArmMsgInstructionResponseConfig', 40 | 'ArmMsgParamEnquiryAndConfig', 41 | 'ArmMsgEndVelAccParamConfig', 42 | 'ArmMsgCrashProtectionRatingConfig', 43 | 'ArmMsgGripperTeachingPendantParamConfig', 44 | 'ArmMsgJointMitCtrl', 45 | 'ArmMsgAllJointMitCtrl' 46 | ] 47 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_circular_pattern.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | 7 | class ArmMsgCircularPatternCoordNumUpdateCtrl: 8 | ''' 9 | msg_v2_transmit 10 | 11 | 圆弧模式坐标序号更新指令 12 | 13 | CAN ID: 14 | 0x158 15 | 16 | Args: 17 | instruction_num: 指令点序号 18 | 19 | 位描述: 20 | 21 | Byte 0: uint8, 指令点序号,以整数表示。 22 | 0x00 无效 23 | 0x01 起点 24 | 0x02 中点 25 | 0x03 终点 26 | ''' 27 | ''' 28 | msg_v2_transmit 29 | 30 | Arc Mode Coordinate Index Update Command 31 | 32 | CAN ID: 33 | 0x158 34 | 35 | Args: 36 | instruction_num: Instruction point index. 37 | 38 | Bit Description: 39 | 40 | Byte 0 instruction_num: uint8, Instruction point index, represented as an integer. 41 | 0x00 Invalid 42 | 0x01 Start point 43 | 0x02 Midpoint 44 | 0x03 Endpoint 45 | ''' 46 | def __init__(self, instruction_num: Literal[0x00, 0x01, 0x02, 0x03] = 0x00): 47 | if instruction_num not in [0x00, 0x01, 0x02, 0x03]: 48 | raise ValueError(f"'instruction_num' Value {instruction_num} out of range [0x00, 0x01, 0x02, 0x03]") 49 | self.instruction_num = instruction_num 50 | 51 | def __str__(self): 52 | return (f"ArmMsgCircularPatternCoordNumUpdateCtrl(\n" 53 | f" instruction_num: {self.instruction_num},\n" 54 | f")") 55 | 56 | def __repr__(self): 57 | return self.__str__() 58 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_crash_protection_rating_config.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | 4 | class ArmMsgCrashProtectionRatingConfig: 5 | ''' 6 | msg_v2_transmit 7 | 8 | 碰撞防护等级设置指令 9 | 10 | CAN ID: 11 | 0x47A 12 | 13 | 有效值 : 0~8 14 | 15 | 等级 0 代表不检测碰撞； 6个关节可以独立设置 16 | 17 | Args: 18 | joint_1_protection_level: 关节1的碰撞等级设定 19 | joint_2_protection_level: 关节2的碰撞等级设定 20 | joint_3_protection_level: 关节3的碰撞等级设定 21 | joint_4_protection_level: 关节4的碰撞等级设定 22 | joint_5_protection_level: 关节5的碰撞等级设定 23 | joint_6_protection_level: 关节6的碰撞等级设定 24 | 25 | 位描述: 26 | 27 | Byte 0: 1 号关节碰撞防护等级, uint8 28 | Byte 1: 2 号关节碰撞防护等级, uint8 29 | Byte 2: 3 号关节碰撞防护等级, uint8 30 | Byte 3: 4 号关节碰撞防护等级, uint8 31 | Byte 4: 5 号关节碰撞防护等级, uint8 32 | Byte 5: 6 号关节碰撞防护等级, uint8 33 | Byte 6: 保留 34 | Byte 7: 保留 35 | ''' 36 | ''' 37 | msg_v2_transmit 38 | 39 | End Effector Speed/Acceleration Parameter Setting Command 40 | 41 | CAN ID: 42 | 0x47A 43 | 44 | Valid Values: 0~8 45 | Level 0 indicates no collision detection. 46 | Collision protection levels can be set independently for the six joints. 47 | 48 | Args: 49 | joint_1_protection_level: Collision protection level for Joint 1. 50 | joint_2_protection_level: Collision protection level for Joint 2. 51 | joint_3_protection_level: Collision protection level for Joint 3. 52 | joint_4_protection_level: Collision protection level for Joint 4. 53 | joint_5_protection_level: Collision protection level for Joint 5. 54 | joint_6_protection_level: Collision protection level for Joint 6. 55 | 56 | Bit Description: 57 | 58 | Byte 0: Collision protection level for Joint 1, uint8. 59 | Byte 1: Collision protection level for Joint 2, uint8. 60 | Byte 2: Collision protection level for Joint 3, uint8. 61 | Byte 3: Collision protection level for Joint 4, uint8. 62 | Byte 4: Collision protection level for Joint 5, uint8. 63 | Byte 5: Collision protection level for Joint 6, uint8. 64 | Byte 6: Reserved. 65 | Byte 7: Reserved. 66 | ''' 67 | def __init__(self, 68 | joint_1_protection_level: int = 0xFF, 69 | joint_2_protection_level: int = 0xFF, 70 | joint_3_protection_level: int = 0xFF, 71 | joint_4_protection_level: int = 0xFF, 72 | joint_5_protection_level: int = 0xFF, 73 | joint_6_protection_level: int = 0xFF 74 | ): 75 | self.joint_1_protection_level = joint_1_protection_level 76 | self.joint_2_protection_level = joint_2_protection_level 77 | self.joint_3_protection_level = joint_3_protection_level 78 | self.joint_4_protection_level = joint_4_protection_level 79 | self.joint_5_protection_level = joint_5_protection_level 80 | self.joint_6_protection_level = joint_6_protection_level 81 | 82 | def __str__(self): 83 | return (f"ArmMsgCrashProtectionRatingConfig(\n" 84 | f" joint_1_protection_level: {self.joint_1_protection_level},\n" 85 | f" joint_2_protection_level: {self.joint_2_protection_level},\n" 86 | f" joint_3_protection_level: {self.joint_3_protection_level},\n" 87 | f" joint_4_protection_level: {self.joint_4_protection_level},\n" 88 | f" joint_5_protection_level: {self.joint_5_protection_level},\n" 89 | f" joint_6_protection_level: {self.joint_6_protection_level}\n" 90 | f")") 91 | 92 | def __repr__(self): 93 | return self.__str__() 94 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_end_vel_acc_param_config.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | 4 | class ArmMsgEndVelAccParamConfig: 5 | ''' 6 | msg_v2_transmit 7 | 8 | 末端速度/加速度参数设置指令 9 | 10 | CAN ID: 11 | 0x479 12 | 13 | Args: 14 | end_max_linear_vel: 末端最大线速度,0x7FFF为设定无效数值 15 | end_max_angular_vel: 末端最大角速度,0x7FFF为设定无效数值 16 | end_max_linear_acc: 末端最大线加速度,0x7FFF为设定无效数值 17 | end_max_angular_acc: 末端最大角加速度,0x7FFF为设定无效数值 18 | 19 | 位描述: 20 | 21 | Byte 0: 末端最大线速度 H, uint16, 单位 0.001m/s, (基于V1.5-2版本后增加无效数值0x7FFF) 22 | Byte 1: 末端最大线速度 L, 23 | Byte 2: 末端最大角速度 H, uint16, 单位 0.001rad/s, (基于V1.5-2版本后增加无效数值0x7FFF) 24 | Byte 3: 末端最大角速度 L, 25 | Byte 4: 末端最大线加速度 H, uint16, 单位 0.001m/s^2, (基于V1.5-2版本后增加无效数值0x7FFF) 26 | Byte 5: 末端最大线加速度 L 27 | Byte 6: 末端最大角加速度 H, uint16, 单位 0.001rad/s^2, (基于V1.5-2版本后增加无效数值0x7FFF) 28 | Byte 7: 末端最大角加速度 L 29 | ''' 30 | ''' 31 | msg_v2_transmit 32 | 33 | End Effector Speed/Acceleration Parameter Setting Command 34 | 35 | CAN ID: 36 | 0x479 37 | 38 | Args: 39 | end_max_linear_vel: Maximum linear velocity of the end effector.0x7FFF is defined as the invalid value. 40 | end_max_angular_vel: Maximum angular velocity of the end effector.0x7FFF is defined as the invalid value. 41 | end_max_linear_acc: Maximum linear acceleration of the end effector.0x7FFF is defined as the invalid value. 42 | end_max_angular_acc: Maximum angular acceleration of the end effector.0x7FFF is defined as the invalid value. 43 | 44 | Bit Description: 45 | 46 | Byte 0: High byte of maximum linear velocity, uint16, unit: 0.001m/s.(Based on version V1.5-2 and later, the invalid value 0x7FFF is added.) 47 | Byte 1: Low byte of maximum linear velocity. 48 | Byte 2: High byte of maximum angular velocity, uint16, unit: 0.001rad/s.(Based on version V1.5-2 and later, the invalid value 0x7FFF is added.) 49 | Byte 3: Low byte of maximum angular velocity. 50 | Byte 4: High byte of maximum linear acceleration, uint16, unit: 0.001m/s².(Based on version V1.5-2 and later, the invalid value 0x7FFF is added.) 51 | Byte 5: Low byte of maximum linear acceleration. 52 | Byte 6: High byte of maximum angular acceleration, uint16, unit: 0.001rad/s².(Based on version V1.5-2 and later, the invalid value 0x7FFF is added.) 53 | Byte 7: Low byte of maximum angular acceleration. 54 | ''' 55 | def __init__(self, 56 | end_max_linear_vel: int = 0, 57 | end_max_angular_vel: int = 0, 58 | end_max_linear_acc: int = 0, 59 | end_max_angular_acc: int = 0 60 | ): 61 | self.end_max_linear_vel = end_max_linear_vel 62 | self.end_max_angular_vel = end_max_angular_vel 63 | self.end_max_linear_acc = end_max_linear_acc 64 | self.end_max_angular_acc = end_max_angular_acc 65 | 66 | def __str__(self): 67 | return (f"ArmMsgEndVelAccParamConfig(\n" 68 | f" end_max_linear_vel: {self.end_max_linear_vel * 0.001:.3f},\n" 69 | f" end_max_angular_vel: {self.end_max_angular_vel * 0.001:.3f},\n" 70 | f" end_max_linear_acc: {self.end_max_linear_acc * 0.001:.3f},\n" 71 | f" end_max_angular_acc: {self.end_max_angular_acc * 0.001:.3f}\n" 72 | f")") 73 | 74 | def __repr__(self): 75 | return self.__str__() 76 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_gripper_ctrl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | class ArmMsgGripperCtrl: 7 | ''' 8 | msg_v2_transmit 9 | 10 | 夹爪控制指令 11 | 12 | CAN ID: 13 | 0x159 14 | 15 | Args: 16 | grippers_angle: 夹爪行程 17 | grippers_effort: 夹爪扭矩,范围0-5000,对应0-5N/m 18 | status_code: 19 | 0x00失能; 20 | 0x01使能; 21 | 0x02失能清除错误; 22 | 0x03使能清除错误. 23 | set_zero: 夹爪零点设置 24 | 0x00无效值 25 | 0xAE设置零点 26 | 27 | 位描述: 28 | 29 | Byte 0 grippers_angle: int32, 单位 0.001mm, 夹爪行程,以整数表示。 30 | Byte 1 31 | Byte 2 32 | Byte 3 33 | Byte 4 grippers_effort: uint16, 单位 0.001N/m, 夹爪扭矩,以整数表示。 34 | Byte 5 35 | Byte 6 status_code: uint8, 夹爪状态码, 使能/失能/清除错误; 36 | 0x00失能; 37 | 0x01使能; 38 | 0x02失能清除错误; 39 | 0x03使能清除错误. 40 | Byte 7 set_zero: uint8, 设定当前位置为0点 41 | 0x00无效值 42 | 0xAE设置零点 43 | ''' 44 | ''' 45 | msg_v2_transmit 46 | 47 | Gripper Control Command 48 | 49 | CAN ID: 50 | 0x159 51 | 52 | Args: 53 | grippers_angle: Gripper stroke, represented as an integer, unit: 0.001mm. 54 | grippers_effort: Gripper torque, represented as an integer, unit: 0.001N·m.Range 0-5000,corresponse 0-5N/m 55 | status_code: 56 | 0x00: Disable; 57 | 0x01: Enable; 58 | 0x03: Enable with clear error; 59 | 0x02: Disable with clear error. 60 | set_zero: Set the current position as the zero point. 61 | 0x00: Invalid; 62 | 0xAE: Set zero. 63 | 64 | Bit Description: 65 | 66 | Byte 0-3 grippers_angle: int32, unit: 0.001°, represents the gripper stroke. 67 | Byte 4-5 grippers_effort: uint16, unit: 0.001N·m, represents the gripper torque. 68 | Byte 6 status_code: uint8, gripper status code for enable/disable/clear error. 69 | 0x00: Disable; 70 | 0x01: Enable; 71 | 0x03: Enable with clear error; 72 | 0x02: Disable with clear error. 73 | Byte 7 set_zero: uint8, flag to set the current position as the zero point. 74 | 0x00: Invalid; 75 | 0xAE: Set zero. 76 | ''' 77 | def __init__(self, 78 | grippers_angle: int = 0, 79 | grippers_effort: int = 0, 80 | status_code: Literal[0x00, 0x01, 0x02, 0x03] = 0, 81 | set_zero: Literal[0x00, 0xAE] = 0): 82 | if status_code not in [0x00, 0x01, 0x02, 0x03]: 83 | raise ValueError(f"'status_code' Value {status_code} out of range [0x00, 0x01, 0x02, 0x03]") 84 | if not (0 <= grippers_effort <= 5000): 85 | raise ValueError(f"'grippers_effort' Value {grippers_effort} out of range 0-5000") 86 | if set_zero not in [0x00, 0xAE]: 87 | raise ValueError(f"'set_zero' Value {set_zero} out of range [0x00,0xAE]") 88 | self.grippers_angle = grippers_angle 89 | self.grippers_effort = grippers_effort 90 | self.status_code = status_code 91 | self.set_zero = set_zero 92 | 93 | def __str__(self): 94 | return (f"ArmMsgGripperCtrl(\n" 95 | f" grippers_angle: {self.grippers_angle}, {self.grippers_angle * 0.001:.3f},\n" 96 | f" grippers_effort: {self.grippers_effort} \t {self.grippers_effort * 0.001:.3f},\n" 97 | f" status_code: {self.status_code},\n" 98 | f" set_zero: {self.set_zero}\n" 99 | f")") 100 | 101 | def __repr__(self): 102 | return self.__str__() 103 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_gripper_teaching_param_config.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | class ArmMsgGripperTeachingPendantParamConfig: 7 | ''' 8 | msg_v2_transmit 9 | 10 | 夹爪/示教器参数设置指令(基于V1.5-2版本后) 11 | 12 | CAN ID: 13 | 0x47D 14 | 15 | Args: 16 | teaching_range_per: 示教器行程系数设置,[100~200] 17 | max_range_config: 夹爪/示教器最大控制行程限制值设置,[0,70,100] 18 | 19 | 位描述: 20 | 21 | Byte 0: 示教器行程系数设置, uint8 22 | 示教器行程系数设置---100~200,单位(%)(默认100%) 23 | 仅适用于设置主从臂的主臂，用于放大控制行程给从臂 24 | Byte 1: 夹爪/示教器最大控制行程限制值设置, uint8, 单位(mm) 25 | 无效值---0 26 | 小夹爪为---70mm 27 | 大夹爪为---100mm 28 | Byte 2: 示教器摩擦系数设置, uint8, 范围[1, 10] ----- (基于V1.5-8版本及以后) 29 | Byte 3: 保留 30 | Byte 4: 保留 31 | Byte 5: 保留 32 | Byte 6: 保留 33 | Byte 7: 保留 34 | ''' 35 | ''' 36 | msg_v2_transmit 37 | 38 | Gripper/Teaching Pendant Parameter Configuration Command(Based on version V1.5-2 and later) 39 | 40 | CAN ID: 41 | 0x47D 42 | 43 | Args: 44 | teaching_range_per: Teaching pendant stroke coefficient setting.[100~200] 45 | max_range_config: Maximum control stroke limit setting for the gripper/teaching pendant.[0,70,100] 46 | 47 | Bit Description: 48 | 49 | Byte Field Type Details 50 | Byte 0 Teaching range coefficient uint8 - Stroke coefficient setting: 100% to 200% (default: 100%). 51 | - Only applicable to the master arm in a master-slave setup to scale control range for the slave arm. 52 | Byte 1 Max control stroke limit uint8 - Invalid value: 0 53 | - Small gripper: 70 mm 54 | - Large gripper: 100 mm 55 | Byte 2 Teaching pendant friction coefficient setting, `uint8`, range [1, 10].(Based on version V1.5-8 and later) 56 | Byte 3 Reserved - Reserved for future use. 57 | Byte 4 Reserved - Reserved for future use. 58 | Byte 5 Reserved - Reserved for future use. 59 | Byte 6 Reserved - Reserved for future use. 60 | Byte 7 Reserved - Reserved for future use. 61 | ''' 62 | def __init__(self, 63 | teaching_range_per: int = 100, 64 | max_range_config: Literal[0, 70, 100] = 0, 65 | teaching_friction: Literal[1, 2, 3, 4, 5, 6, 7, 8, 9, 10] = 1): 66 | if not (100 <= teaching_range_per <= 200): 67 | raise ValueError(f"'teaching_range_per' Value {teaching_range_per} out of range [100, 200]") 68 | if max_range_config not in [0, 70, 100]: 69 | raise ValueError(f"'max_range_config' Value {max_range_config} out of range [0,70,100]") 70 | if teaching_friction not in [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]: 71 | raise ValueError(f"'teaching_friction' Value {teaching_friction} out of range [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]") 72 | self.teaching_range_per = teaching_range_per 73 | self.max_range_config = max_range_config 74 | self.teaching_friction = teaching_friction 75 | 76 | def __str__(self): 77 | return (f"ArmMsgGripperTeachingPendantParamConfig(\n" 78 | f" teaching_range_per: {self.teaching_range_per},\n" 79 | f" max_range_config: {self.max_range_config},\n" 80 | f" teaching_friction: {self.teaching_friction},\n" 81 | f")") 82 | 83 | def __repr__(self): 84 | return self.__str__() 85 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_joint_config.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | class ArmMsgJointConfig: 7 | ''' 8 | msg_v2_transmit 9 | 10 | 关节设置指令 11 | 12 | CAN ID: 13 | 0x475 14 | 15 | Args: 16 | joint_motor_num: 关节电机序号,[1, 7] 17 | set_motor_current_pos_as_zero: 设置当前位置为零点, 有效值-0xAE 18 | acc_param_config_is_effective_or_not: 加速度参数设置是否生效, 有效值-0xAE 19 | max_joint_acc: 最大关节加速度,单位0.01rad/s^2(0x7FFF为设定无效数值),输入范围\[0, 500\]-->[0 rad/s^2, 5.0 rad/s^2] 20 | clear_joint_err: 清除关节错误代码, 有效值-0xAE 21 | 22 | 位描述: 23 | 24 | Byte 0: 关节电机序号 uint8, 值域 1-7 25 | 1-6 代表关节驱动器序号； 26 | 7 代表全部关节电机 27 | Byte 1: 设置N号电机当前位置为零点: uint8, 有效值-0xAE 28 | Byte 2: 加速度参数设置是否生效: uint8, 有效值-0xAE 29 | Byte 3: 最大关节加速度 H: uint16, 单位 0.01rad/s^2.(基于V1.5-2版本后增加无效数值0x7FFF) 30 | Byte 4: 最大关节加速度 L 31 | Byte 5: 清除关节错误代码: uint8, 有效值-0xAE 32 | Byte 6: 保留 33 | Byte 7: 保留 34 | ''' 35 | ''' 36 | msg_v2_transmit 37 | 38 | Joint Configuration Command 39 | 40 | CAN ID: 41 | 0x475 42 | 43 | Args: 44 | joint_motor_num: Joint motor number. 45 | Value range: 1-6 represents individual joint motor numbers. 46 | Value 7 applies to all joint motors. 47 | set_motor_current_pos_as_zero: Command to set the current position of the specified joint motor as zero, with a valid value of 0xAE. 48 | acc_param_config_is_effective_or_not: Indicates whether the acceleration parameter configuration is effective, with a valid value of 0xAE. 49 | max_joint_acc: Maximum joint acceleration, unit: 0.01rad/s^2, 0x7FFF is defined as the invalid value.Range is \[0, 500\]-->[0 rad/s^2, 5.0 rad/s^2] 50 | clear_joint_err: Command to clear joint error codes, with a valid value of 0xAE. 51 | 52 | Bit Description: 53 | 54 | Byte 0: Joint motor number (uint8). 55 | - 1-6: Corresponds to individual joint motor numbers. 56 | - 7: Represents all joint motors. 57 | Byte 1: Set the current position of the specified joint motor as zero (uint8). 58 | - Valid value: 0xAE. 59 | Byte 2: Determines if the acceleration parameter configuration is effective (uint8). 60 | - Valid value: 0xAE. 61 | Byte 3-4: Maximum joint acceleration (uint16).(Based on version V1.5-2 and later, the invalid value 0x7FFF is added.) 62 | - Unit: 0.01rad/s^2. 63 | - Byte 3: High byte, Byte 4: Low byte. 64 | Byte 5: Clear joint error code (uint8). 65 | - Valid value: 0xAE. 66 | Byte 6: Reserved 67 | Byte 7: Reserved 68 | ''' 69 | def __init__(self, 70 | joint_motor_num: Literal[1, 2, 3, 4, 5, 6, 7] = 7, 71 | set_motor_current_pos_as_zero: Literal[0x00, 0xAE] = 0, 72 | acc_param_config_is_effective_or_not: Literal[0x00, 0xAE] = 0, 73 | max_joint_acc: int = 500, 74 | clear_joint_err: Literal[0x00, 0xAE] = 0): 75 | if joint_motor_num not in [1, 2, 3, 4, 5, 6, 7]: 76 | raise ValueError(f"'joint_motor_num Value' {joint_motor_num} out of range [1, 2, 3, 4, 5, 6, 7]") 77 | if set_motor_current_pos_as_zero not in [0x00, 0xAE]: 78 | raise ValueError(f"'set_motor_current_pos_as_zero' Value {set_motor_current_pos_as_zero} out of range [0x00, 0xAE]") 79 | if acc_param_config_is_effective_or_not not in [0x00, 0xAE]: 80 | raise ValueError(f"'acc_param_config_is_effective_or_not' Value {acc_param_config_is_effective_or_not} out of range [0x00, 0xAE]") 81 | if not (0 <= max_joint_acc <= 500 or max_joint_acc == 0x7FFF): 82 | raise ValueError(f"'max_joint_acc' Value {max_joint_acc} out of range 0-500 or not equal to 0x7FFF") 83 | if clear_joint_err not in [0x00, 0xAE]: 84 | raise ValueError(f"clear_joint_err' Value {clear_joint_err} out of range [0x00, 0xAE]") 85 | self.joint_motor_num = joint_motor_num 86 | self.set_motor_current_pos_as_zero = set_motor_current_pos_as_zero 87 | self.acc_param_config_is_effective_or_not = acc_param_config_is_effective_or_not 88 | self.max_joint_acc = max_joint_acc 89 | self.clear_joint_err = clear_joint_err 90 | 91 | def __str__(self): 92 | return (f"ArmMsgJointConfig(\n" 93 | f" joint_motor_num: {self.joint_motor_num},\n" 94 | f" set_motor_current_pos_as_zero: {self.set_motor_current_pos_as_zero},\n" 95 | f" acc_param_config_is_effective_or_not: {self.acc_param_config_is_effective_or_not},\n" 96 | f" max_joint_acc: {self.max_joint_acc}, {self.max_joint_acc*0.01:.2f}\n" 97 | f" clear_joint_err: {self.clear_joint_err}\n" 98 | f")") 99 | 100 | def __repr__(self): 101 | return self.__str__() 102 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_joint_ctrl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | 4 | class ArmMsgJointCtrl(): 5 | ''' 6 | msg_v2_transmit 7 | 8 | 机械臂关节控制,单位0.001度 9 | 10 | CAN ID: 11 | 0x155,0x156,0x157 12 | 13 | Args: 14 | joint_1: joint_1角度 15 | joint_2: joint_2角度 16 | joint_3: joint_3角度 17 | joint_4: joint_4角度 18 | joint_5: joint_5角度 19 | joint_6: joint_6角度 20 | ''' 21 | ''' 22 | msg_v2_transmit 23 | 24 | Robotic Arm Joint Control (Unit: 0.001°) 25 | 26 | CAN IDs: 27 | 0x155, 0x156, 0x157 28 | 29 | Args: 30 | joint_1: The target angle of joint 1 in 0.001°. 31 | joint_2: The target angle of joint 2 in 0.001°. 32 | joint_3: The target angle of joint 3 in 0.001°. 33 | joint_4: The target angle of joint 4 in 0.001°. 34 | joint_5: The target angle of joint 5 in 0.001°. 35 | joint_6: The target angle of joint 6 in 0.001°. 36 | ''' 37 | def __init__(self, 38 | joint_1: int = 0, 39 | joint_2: int = 0, 40 | joint_3: int = 0, 41 | joint_4: int = 0, 42 | joint_5: int = 0, 43 | joint_6: int = 0): 44 | self.joint_1 = joint_1 45 | self.joint_2 = joint_2 46 | self.joint_3 = joint_3 47 | self.joint_4 = joint_4 48 | self.joint_5 = joint_5 49 | self.joint_6 = joint_6 50 | 51 | def __str__(self): 52 | # 将角度乘以0.001，并保留三位小数 53 | joint_angles = [ 54 | ("Joint_1", self.joint_1 * 0.001), 55 | ("Joint_2", self.joint_2 * 0.001), 56 | ("Joint_3", self.joint_3 * 0.001), 57 | ("Joint_4", self.joint_4 * 0.001), 58 | ("Joint_5", self.joint_5 * 0.001), 59 | ("Joint_6", self.joint_6 * 0.001) 60 | ] 61 | 62 | # 生成格式化字符串，保留三位小数 63 | formatted_angles = "\n".join([f"{name}: {angle:.3f}" for name, angle in joint_angles]) 64 | 65 | return f"ArmMsgJointCtrl:\n{formatted_angles}" 66 | 67 | def __repr__(self): 68 | return self.__str__() -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_light_ctrl.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/agilexrobotics/piper_sdk/081e7c588e5b79eeaefa67a0469bcc701c81014f/piper_sdk/piper_msgs/msg_v2/transmit/arm_light_ctrl.py -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_master_slave_config.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | 7 | class ArmMsgMasterSlaveModeConfig: 8 | ''' 9 | msg_v2_transmit 10 | 11 | 随动主从模式设置指令 12 | 13 | CAN ID: 14 | 0x470 15 | 16 | Args: 17 | linkage_config: 联动设置指令 18 | feedback_offset: 反馈指令偏移值 19 | ctrl_offset: 控制指令偏移值 20 | linkage_offset: 联动模式控制目标地址偏移值 21 | 22 | 位描述: 23 | 24 | Byte 0 linkage_config: uint8, 联动设置指令。 25 | 0x00 无效 26 | 0xFA 设置为示教输入臂 27 | 0xFC 设置为运动输出臂 28 | Byte 1 feedback_offset: uint8, 反馈指令偏移值。 29 | 0x00 : 不偏移/恢复默认 30 | 0x10 ：反馈指令基 ID 由 2Ax偏移为 2Bx 31 | 0x20 ：反馈指令基 ID 由 2Ax偏移为 2Cx 32 | Byte 2 ctrl_offset: uint8, 控制指令偏移值。 33 | 0x00 : 不偏移/恢复默认 34 | 0x10 ：控制指令基 ID 由 15x偏移为 16x 35 | 0x20 ：控制指令基 ID 由 15x偏移为 17x 36 | Byte 3 linkage_offset: uint8, 联动模式控制目标地址偏移值。 37 | 0x00 : 不偏移/恢复默认 38 | 0x10 : 控制目标地址基 ID由 15x 偏移为 16x 39 | 0x20 : 控制目标地址基 ID由 15x 偏移为 17x 40 | ''' 41 | ''' 42 | msg_v2_transmit 43 | 44 | Follow Master-Slave Mode Setting Command 45 | 46 | CAN ID: 47 | 0x470 48 | 49 | Args: 50 | linkage_config: Linkage setting command. 51 | feedback_offset: Offset value for feedback instructions. 52 | ctrl_offset: Offset value for control instructions. 53 | linkage_offset: Offset value for linkage mode control target address. 54 | 55 | Bit Descriptions: 56 | 57 | Byte 0 linkage_config: uint8, linkage setting command. 58 | 0x00: Invalid. 59 | 0xFA: Set as teaching input arm. 60 | 0xFC: Set as motion output arm. 61 | 62 | Byte 1 feedback_offset: uint8, feedback instruction offset value. 63 | 0x00: No offset/restore default. 64 | 0x10: Feedback instruction base ID shifted from 2Ax to 2Bx. 65 | 0x20: Feedback instruction base ID shifted from 2Ax to 2Cx. 66 | 67 | Byte 2 ctrl_offset: uint8, control instruction offset value. 68 | 0x00: No offset/restore default. 69 | 0x10: Control instruction base ID shifted from 15x to 16x. 70 | 0x20: Control instruction base ID shifted from 15x to 17x. 71 | 72 | Byte 3 linkage_offset: uint8, offset value for the linkage mode control target address. 73 | 0x00: No offset/restore default. 74 | 0x10: Control target address base ID shifted from 15x to 16x. 75 | 0x20: Control target address base ID shifted from 15x to 17x. 76 | ''' 77 | def __init__(self, 78 | linkage_config: Literal[0x00, 0xFA, 0xFC] = 0x00, 79 | feedback_offset: Literal[0x00, 0x10, 0x20] = 0x00, 80 | ctrl_offset: Literal[0x00, 0x10, 0x20] = 0x00, 81 | linkage_offset: Literal[0x00, 0x10, 0x20] = 0x00): 82 | if linkage_config not in [0x00, 0xFA, 0xFC]: 83 | raise ValueError(f"'linkage_config' Value {linkage_config} out of range [0x00, 0xFA, 0xFC]") 84 | if feedback_offset not in [0x00, 0x10, 0x20]: 85 | raise ValueError(f"'feedback_offset' Value {feedback_offset} out of range [0x00, 0x10, 0x20]") 86 | if ctrl_offset not in [0x00, 0x10, 0x20]: 87 | raise ValueError(f"'ctrl_offset' Value {ctrl_offset} out of range [0x00, 0x10, 0x20]") 88 | if linkage_offset not in [0x00, 0x10, 0x20]: 89 | raise ValueError(f"'linkage_offset' Value {linkage_offset} out of range [0x00, 0x10, 0x20]") 90 | self.linkage_config = linkage_config 91 | self.feedback_offset = feedback_offset 92 | self.ctrl_offset = ctrl_offset 93 | self.linkage_offset = linkage_offset 94 | 95 | def __str__(self): 96 | return (f"ArmMsgMasterSlaveModeConfig(\n" 97 | f" linkage_config: {self.linkage_config },\n" 98 | f" feedback_offset: {self.feedback_offset },\n" 99 | f" ctrl_offset: {self.ctrl_offset },\n" 100 | f" linkage_offset: {self.linkage_offset}\n" 101 | f")") 102 | 103 | def __repr__(self): 104 | return self.__str__() 105 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_motion_ctrl_1.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | class ArmMsgMotionCtrl_1(): 7 | ''' 8 | msg_v2_transmit 9 | 10 | 机械臂运动控制指令1 11 | 12 | CAN ID: 13 | 0x150 14 | 15 | Args: 16 | emergency_stop: 快速急停 17 | track_ctrl: 轨迹指令 18 | grag_teach_ctrl: 拖动示教指令 19 | 20 | 位描述: 21 | 22 | Byte 0: 快速急停 uint8 0x00 无效 23 | 0x01 快速急停 24 | 0x02 恢复 25 | Byte 1: 轨迹指令 uint8 0x00 关闭 26 | 0x01 暂停当前规划 27 | 0x02 继续当前轨迹 28 | 0x03 清除当前轨迹 29 | 0x04 清除所有轨迹 30 | 0x05 获取当前规划轨迹 31 | 0x06 终止执行 32 | 0x07 轨迹传输 33 | 0x08 轨迹传输结束 34 | Byte 2: 拖动示教指令 uint8 0x00 关闭 35 | 0x01 开始示教记录（进入拖动示教模式） 36 | 0x02 结束示教记录（退出拖动示教模式） 37 | 0x03 执行示教轨迹（拖动示教轨迹复现） 38 | 0x04 暂停执行 39 | 0x05 继续执行（轨迹复现继续） 40 | 0x06 终止执行 41 | 0x07 运动到轨迹起点 42 | Byte 3: 轨迹索引 uint8 标记刚才传输的轨迹点为第N个轨迹点 43 | N=0~255 44 | 主控收到后会应答0x476 byte0 = 0x50 ;byte 2=N(详见0x476 )未收到应答需要重传 45 | Byte 4: NameIndex_H uint16 当前轨迹包名称索引,由NameIndex和crc组成(应答0x477 byte0=03) 46 | Byte 5: NameIndex_L 47 | Byte 6: crc16_H uint16 48 | Byte 7: crc16_L 49 | ''' 50 | ''' 51 | msg_v2_transmit 52 | 53 | Robotic Arm Motion Control Command 1 54 | 55 | CAN ID: 56 | 0x150 57 | 58 | Args: 59 | emergency_stop: Emergency stop command. 60 | track_ctrl: Trajectory control command. 61 | grag_teach_ctrl: Drag teach command. 62 | 63 | Bit Descriptions: 64 | 65 | Byte 0: emergency_stop: uint8, emergency stop control. 66 | 0x00: Invalid. 67 | 0x01: Activate emergency stop. 68 | 0x02: Resume from emergency stop. 69 | 70 | Byte 1: track_ctrl: uint8, trajectory control instructions. 71 | 0x00: Disable. 72 | 0x01: Pause current planning. 73 | 0x02: Resume current trajectory. 74 | 0x03: Clear current trajectory. 75 | 0x04: Clear all trajectories. 76 | 0x05: Get the current planned trajectory. 77 | 0x06: Terminate execution. 78 | 0x07: Trajectory transfer. 79 | 0x08: End trajectory transfer. 80 | 81 | Byte 2: grag_teach_ctrl: uint8, drag teach control. 82 | 0x00: Disable. 83 | 0x01: Start teaching record (enter drag teach mode). 84 | 0x02: End teaching record (exit drag teach mode). 85 | 0x03: Execute the teaching trajectory (reproduce drag teaching trajectory). 86 | 0x04: Pause execution. 87 | 0x05: Continue execution (resume trajectory reproduction). 88 | 0x06: Terminate execution. 89 | 0x07: Move to the starting point of the trajectory. 90 | 91 | Byte 3: trajectory_index: uint8, mark the transmitted trajectory point as the Nth trajectory point. 92 | N = 0~255: 93 | The controller responds with CAN ID: 0x476, Byte 0 = 0x50, Byte 2 = N. If no response is received, retransmission is required. 94 | 95 | Byte 4-5: NameIndex_H/L: uint16, current trajectory packet name index, composed of NameIndex and CRC. 96 | Response on CAN ID: 0x477, Byte 0 = 0x03. 97 | 98 | Byte 6-7: crc16_H/L: uint16, CRC checksum for validation. 99 | ''' 100 | def __init__(self, 101 | emergency_stop: Literal[0x00, 0x01, 0x02] = 0, 102 | track_ctrl: Literal[0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08] = 0, 103 | grag_teach_ctrl: Literal[0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07] = 0): 104 | # 检查 emergency_stop 是否在有效范围内 105 | if emergency_stop not in [0x00, 0x01, 0x02]: 106 | raise ValueError(f"'emergency_stop' Value {emergency_stop} out of range [0x00, 0x01, 0x02]") 107 | 108 | # 检查 track_ctrl 是否在有效范围内 109 | if track_ctrl not in [0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08]: 110 | raise ValueError(f"'track_ctrl' Value {track_ctrl} out of range [0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08]") 111 | 112 | # 检查 grag_teach_ctrl 是否在有效范围内 113 | if grag_teach_ctrl not in [0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07]: 114 | raise ValueError(f"'grag_teach_ctrl' Value {grag_teach_ctrl} out of range [0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07]") 115 | self.emergency_stop = emergency_stop 116 | self.track_ctrl = track_ctrl 117 | self.grag_teach_ctrl = grag_teach_ctrl 118 | 119 | def __str__(self): 120 | dict_ = [ 121 | (" emergency_stop ", self.emergency_stop), 122 | (" track_ctrl ", self.track_ctrl), 123 | (" grag_teach_ctrl ", self.grag_teach_ctrl) 124 | ] 125 | 126 | # 生成格式化字符串，保留三位小数 127 | formatted_ = "\n".join([f"{name}: {value}" for name, value in dict_]) 128 | 129 | return f"ArmMsgMotionCtrl_1:\n{formatted_}" 130 | 131 | def __repr__(self): 132 | return self.__str__() -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_motion_ctrl_2.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | class ArmMsgMotionCtrl_2(): 7 | ''' 8 | msg_v2_transmit 9 | 10 | 机械臂运动控制指令2 11 | 12 | CAN ID: 13 | 0x151 14 | 15 | Args: 16 | ctrl_mode: 控制模式 17 | move_mode: MOVE模式 18 | move_spd_rate_ctrl: 运动速度百分比 19 | mit_mode: mit模式 20 | residence_time: 离线轨迹点停留时间 21 | installation_pos: 安装位置 22 | 23 | 位描述: 24 | 25 | Byte 0: 控制模式 uint8 26 | 0x00 待机模式 27 | 0x01 CAN 指令控制模式 28 | 0x03 以太网控制模式 29 | 0x04 wifi 控制模式 30 | 0x07 离线轨迹模式 31 | Byte 1: MOVE模式 uint8 32 | 0x00 MOVE P 33 | 0x01 MOVE J 34 | 0x02 MOVE L 35 | 0x03 MOVE C 36 | 0x04 MOVE M ---基于V1.5-2版本后 37 | 0x05 MOVE CPV ---基于V1.8-1版本后 38 | Byte 2: 运动速度百分比 uint8 0~100 39 | Byte 3: mit模式 uint8 40 | 0x00 位置速度模式 41 | 0xAD MIT模式 42 | 0xFF 无效 43 | Byte 4: 离线轨迹点停留时间 uint8 0~254 ,单位: s;255:轨迹终止 44 | Byte 5: 安装位置 uint8 注意接线朝后 ---基于V1.5-2版本后 45 | 0x00 无效值 46 | 0x01 水平正装 47 | 0x02 侧装左 48 | 0x03 侧装右 49 | ''' 50 | ''' 51 | msg_v2_transmit 52 | 53 | Robotic Arm Motion Control Command 2 54 | 55 | CAN ID: 56 | 0x151 57 | 58 | Args: 59 | ctrl_mode: Control mode. 60 | move_mode: MOVE mode. 61 | move_spd_rate_ctrl: Movement speed as a percentage. 62 | mit_mode: MIT mode. 63 | residence_time: Hold time at offline trajectory points. 64 | 65 | Bit Descriptions: 66 | 67 | Byte 0 control_mode: uint8, control mode selection. 68 | 0x00: Standby mode. 69 | 0x01: CAN command control mode. 70 | 0x03: Ethernet control mode. 71 | 0x04: Wi-Fi control mode. 72 | 0x07: Offline trajectory mode. 73 | 74 | Byte 1 move_mode: uint8, movement mode selection. 75 | 0x00: MOVE P (Position). 76 | 0x01: MOVE J (Joint). 77 | 0x02: MOVE L (Linear). 78 | 0x03: MOVE C (Circular). 79 | 0x04: MOVE M (MIT) ---- Based on version V1.5-2 and later 80 | 0x05: MOVE CPV ---- Based on version V1.8-1 and later 81 | 82 | Byte 2 speed_percentage: uint8, movement speed as a percentage. 83 | Range: 0~100. 84 | 85 | Byte 3 mit_mode: uint8, motion control mode. 86 | 0x00: Position-speed mode. 87 | 0xAD: MIT mode. 88 | 0xFF: Invalid. 89 | 90 | Byte 4 offline_trajectory_hold_time: uint8, duration to hold at offline trajectory points. 91 | Range: 0~255, unit: seconds. 92 | 93 | Byte 5: Installation Position (uint8) - Note: Wiring should face backward ---- Based on version V1.5-2 and later 94 | 0x00: Invalid value 95 | 0x01: Horizontal upright 96 | 0x02: Left-side mount 97 | 0x03: Right-side mount 98 | ''' 99 | def __init__(self, 100 | ctrl_mode: Literal[0x00, 0x01, 0x03, 0x04, 0x07] = 0x01, 101 | move_mode: Literal[0x00, 0x01, 0x02, 0x03, 0x04, 0x05] = 0x01, 102 | move_spd_rate_ctrl: int = 50, 103 | mit_mode: Literal[0x00, 0xAD, 0xFF] = 0x00, 104 | residence_time: int = 0, 105 | installation_pos: Literal[0x00, 0x01, 0x02, 0x03] = 0x00): 106 | # 检查是否在有效范围内 107 | if ctrl_mode not in [0x00, 0x01, 0x03, 0x04, 0x07]: 108 | raise ValueError(f"'ctrl_mode' Value {ctrl_mode} out of range [0x00, 0x01, 0x02, 0x03, 0x04, 0x07]") 109 | if move_mode not in [0x00, 0x01, 0x02, 0x03, 0x04, 0x05]: 110 | raise ValueError(f"'move_mode' Value {move_mode} out of range [0x00, 0x01, 0x02, 0x03, 0x04, 0x05]") 111 | if not (0 <= move_spd_rate_ctrl <= 100): 112 | raise ValueError(f"'move_spd_rate_ctrl' Value {move_spd_rate_ctrl} out of range 0-100") 113 | if mit_mode not in [0x00, 0xAD, 0xFF]: 114 | raise ValueError(f"'mit_mode' Value {mit_mode} out of range [0x00, 0xAD, 0xFF]") 115 | if not (0 <= residence_time <= 255): 116 | raise ValueError(f"'residence_time' Value {residence_time} out of range 0-255") 117 | if installation_pos not in [0x00, 0x01, 0x02, 0x03]: 118 | raise ValueError(f"'installation_pos' Value {installation_pos} out of range [0x00, 0x01, 0x02, 0x03]") 119 | self.ctrl_mode = ctrl_mode 120 | self.move_mode = move_mode 121 | self.move_spd_rate_ctrl = move_spd_rate_ctrl 122 | self.mit_mode = mit_mode 123 | self.residence_time = residence_time 124 | self.installation_pos = installation_pos 125 | 126 | def __str__(self): 127 | dict_ = [ 128 | (" ctrl_mode ", self.ctrl_mode), 129 | (" move_mode ", self.move_mode), 130 | (" move_spd_rate_ctrl ", self.move_spd_rate_ctrl), 131 | (" mit_mode ", self.mit_mode), 132 | (" residence_time ", self.residence_time), 133 | (" installation_pos ", self.installation_pos), 134 | ] 135 | 136 | # 生成格式化字符串，保留三位小数 137 | formatted_ = "\n".join([f"{name}: {value}" for name, value in dict_]) 138 | 139 | return f"ArmMsgMotionCtrl_2:\n{formatted_}" 140 | 141 | def __repr__(self): 142 | return self.__str__() -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_motion_ctrl_cartesian.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | 4 | class ArmMsgMotionCtrlCartesian(): 5 | ''' 6 | msg_v2_transmit 7 | 8 | 机械臂运动控制直角坐标系指令 9 | 10 | CAN ID: 11 | 0x152,0x153,0x154 12 | 13 | Args: 14 | X_axis: X坐标,单位0.001mm 15 | Y_axis: Y坐标,单位0.001mm 16 | Z_axis: Z坐标,单位0.001mm 17 | RX_axis: RX角度,单位0.001度 18 | RY_axis: RY角度,单位0.001度 19 | RZ_axis: RZ角度,单位0.001度 20 | ''' 21 | ''' 22 | msg_v2_transmit 23 | 24 | Robotic Arm Motion Control Command in Cartesian Coordinate System 25 | 26 | CAN ID: 27 | 0x152, 0x153, 0x154 28 | 29 | Args: 30 | X_axis: X-axis coordinate, in 0.001 mm. 31 | Y_axis: Y-axis coordinate, in 0.001 mm. 32 | Z_axis: Z-axis coordinate, in 0.001 mm. 33 | RX_axis: Rotation about X-axis, in 0.001 degrees. 34 | RY_axis: Rotation about Y-axis, in 0.001 degrees. 35 | RZ_axis: Rotation about Z-axis, in 0.001 degrees. 36 | ''' 37 | def __init__(self, 38 | X_axis: int = 0, 39 | Y_axis: int = 0, 40 | Z_axis: int = 0, 41 | RX_axis: int = 0, 42 | RY_axis: int = 0, 43 | RZ_axis: int = 0): 44 | self.X_axis = X_axis 45 | self.Y_axis = Y_axis 46 | self.Z_axis = Z_axis 47 | self.RX_axis = RX_axis 48 | self.RY_axis = RY_axis 49 | self.RZ_axis = RZ_axis 50 | 51 | def __str__(self): 52 | dict_ = [ 53 | (" X_axis ", self.X_axis), 54 | (" Y_axis ", self.Y_axis), 55 | (" Z_axis ", self.Z_axis), 56 | (" RX_axis ", self.RX_axis), 57 | (" RY_axis ", self.RY_axis), 58 | (" RZ_axis ", self.RZ_axis) 59 | ] 60 | 61 | # 生成格式化字符串，保留三位小数 62 | formatted_ = "\n".join([f"{name}: {value}" for name, value in dict_]) 63 | 64 | return f"ArmMsgMotionCtrlCartesian:\n{formatted_}" 65 | 66 | def __repr__(self): 67 | return self.__str__() -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_motor_angle_limit_max_spd_config.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | class ArmMsgMotorAngleLimitMaxSpdSet: 7 | ''' 8 | msg_v2_transmit 9 | 10 | 电机角度限制/最大速度设置指令-V2(基于V1.5-2版本后增加无效数值0x7FFF) 11 | 12 | CAN ID: 13 | 0x474 14 | 15 | Args: 16 | motor_num: 关节电机序号 17 | max_angle_limit: 最大角度限制,单位 0.1°,0x7FFF为设定无效数值 18 | min_angle_limit: 最小角度限制,单位 0.1°,0x7FFF为设定无效数值 19 | max_joint_spd: 最大关节速度,单位 0.001rad/s,范围[0,3000],0x7FFF为设定无效数值 20 | 21 | |joint_name| limit(rad) | limit(angle) | limit(rad/s) | 22 | |----------| ---------- | ---------- | ---------- | 23 | |joint1 | [-2.6179, 2.6179] | [-150.0, 150.0] | [0, 3.0] | 24 | |joint2 | [0, 3.14] | [0, 180.0] | [0, 3.0] | 25 | |joint3 | [-2.967, 0] | [-170, 0] | [0, 3.0] | 26 | |joint4 | [-1.745, 1.745] | [-100.0, 100.0] | [0, 3.0] | 27 | |joint5 | [-1.22, 1.22] | [-70.0, 70.0] | [0, 3.0] | 28 | |joint6 | [-2.09439, 2.09439]| [-120.0, 120.0] | [0, 3.0] | 29 | 30 | 位描述: 31 | 32 | Byte 0: 关节电机序号 uint8, 值域 1-6:1-6 代表关节驱动器序号 33 | Byte 1: 最大角度限制 H: int16, 单位 0.1°(基于V1.5-2版本后增加无效数值0x7FFF) 34 | Byte 2: 最大角度限制 L 35 | Byte 3: 最小角度限制 H: int16, 单位 0.1°(基于V1.5-2版本后增加无效数值0x7FFF) 36 | Byte 4: 最小角度限制 L 37 | Byte 5: 最大关节速度 H: uint16, 单位 0.001rad/s(基于V1.5-2版本后增加无效数值0x7FFF) 38 | Byte 6: 最大关节速度 L 39 | ''' 40 | ''' 41 | msg_v2_transmit 42 | 43 | Motor Angle Limits/Maximum Speed Setting Command-V2(Based on version V1.5-2 and later, the invalid value 0x7FFF is added.) 44 | 45 | CAN ID: 46 | 0x474 47 | 48 | Args: 49 | motor_num: Joint motor index. 50 | max_angle_limit: Maximum angle limit, unit 0.1°,0x7FFF is defined as the invalid value. 51 | min_angle_limit: Minimum angle limit, unit 0.1°,0x7FFF is defined as the invalid value. 52 | max_joint_spd: Maximum joint speed, unit 0.001 rad/s,Range [0, 3000],0x7FFF is defined as the invalid value. 53 | 54 | |joint_name| limit(rad) | limit(angle) | limit(rad/s) | 55 | |----------| ---------- | ---------- | ---------- | 56 | |joint1 | [-2.6179, 2.6179] | [-150.0, 150.0] | [0, 3.0] | 57 | |joint2 | [0, 3.14] | [0, 180.0] | [0, 3.0] | 58 | |joint3 | [-2.967, 0] | [-170, 0] | [0, 3.0] | 59 | |joint4 | [-1.745, 1.745] | [-100.0, 100.0] | [0, 3.0] | 60 | |joint5 | [-1.22, 1.22] | [-70.0, 70.0] | [0, 3.0] | 61 | |joint6 | [-2.09439, 2.09439]| [-120.0, 120.0] | [0, 3.0] | 62 | 63 | Bit Description: 64 | 65 | Byte 0: Joint motor index, uint8, range 1-6. 66 | Byte 1: Maximum angle limit (high byte), int16, unit 0.1°.(Based on version V1.5-2 and later, the invalid value 0x7FFF is added.) 67 | Byte 2: Maximum angle limit (low byte). 68 | Byte 3: Minimum angle limit (high byte), int16, unit 0.1°.(Based on version V1.5-2 and later, the invalid value 0x7FFF is added.) 69 | Byte 4: Minimum angle limit (low byte). 70 | Byte 5: Maximum joint speed (high byte), uint16, unit 0.001 rad/s.(Based on version V1.5-2 and later, the invalid value 0x7FFF is added.) 71 | Byte 6: Maximum joint speed (low byte). 72 | ''' 73 | def __init__(self, 74 | motor_num: Literal[1, 2, 3, 4, 5, 6] = 1, 75 | max_angle_limit: int = 0x7FFF, 76 | min_angle_limit: int = 0x7FFF, 77 | max_joint_spd: int = 0x7FFF): 78 | if motor_num not in [1, 2, 3, 4, 5, 6]: 79 | raise ValueError(f"'motor_num' Value {motor_num} out of range [1, 2, 3, 4, 5, 6]") 80 | if not (0 <= max_joint_spd <= 3000 or max_joint_spd == 0x7FFF): 81 | raise ValueError(f"'max_joint_spd' Value {max_joint_spd} out of range 0-3000 or not equal to 0x7FFF") 82 | self.motor_num = motor_num 83 | self.max_angle_limit = max_angle_limit 84 | self.min_angle_limit = min_angle_limit 85 | self.max_joint_spd = max_joint_spd 86 | 87 | def __str__(self): 88 | return (f"ArmMsgMotorAngleSpdLimitConfig(\n" 89 | f" motor_num: {self.motor_num},\n" 90 | f" max_angle_limit: {self.max_angle_limit}, {self.max_angle_limit * 0.1:.1f},\n" 91 | f" min_angle_limit: {self.min_angle_limit}, {self.min_angle_limit * 0.1:.1f},\n" 92 | f" max_joint_spd: {self.max_joint_spd}, {self.max_joint_spd * 0.3:.3f}\n" 93 | f")") 94 | 95 | def __repr__(self): 96 | return self.__str__() 97 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_motor_enable_disable.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | 7 | class ArmMsgMotorEnableDisableConfig: 8 | ''' 9 | msg_v2_transmit 10 | 11 | 电机使能/失能设置指令 12 | 13 | CAN ID: 14 | 0x471 15 | 16 | Args: 17 | motor_num: 电机序号[1,7],7代表所有电机 18 | enable_flag: 使能标志位,0x01-失能;0x02-使能 19 | 20 | 位描述: 21 | 22 | Byte 0: uint8, 关节电机序号。 23 | 值域 1-7:1-6 代表关节驱动器序号,7代表夹爪电机,FF代表全部关节电机(包含夹爪) 24 | Byte 1: uint8, 使能/失能。 25 | 0x01 : 失能 26 | 0x02 : 使能 27 | ''' 28 | ''' 29 | msg_v2_transmit 30 | 31 | Motor Enable/Disable Command 32 | 33 | CAN ID: 34 | 0x471 35 | 36 | Args: 37 | motor_num: Motor index [1, 7], where 7 represents all motors. 38 | enable_flag: Enable flag, 0x01 for disable, 0x02 for enable. 39 | 40 | Bit Description: 41 | 42 | Byte 0: 43 | motor_num, uint8, motor index. 44 | Range 1-7: 45 | 1-6: Represents joint motor index. 46 | 7: Represents gripper motor. 47 | 0xFF: Represents all joint motors (including gripper). 48 | Byte 1: 49 | enable_flag, uint8, enable/disable. 50 | 0x01: Disable. 51 | 0x02: Enable. 52 | ''' 53 | def __init__(self, 54 | motor_num: Literal[1, 2, 3, 4, 5, 6, 7, 0xFF] = 0xFF, 55 | enable_flag: Literal[0x01, 0x02] = 0x01): 56 | if motor_num not in [1, 2, 3, 4, 5, 6, 7, 0xFF]: 57 | raise ValueError(f"'motor_num' Value {motor_num} out of range [1, 2, 3, 4, 5, 6, 7, 0xFF]") 58 | if enable_flag not in [0x01, 0x02]: 59 | raise ValueError(f"'enable_flag' Value {enable_flag} out of range [0x01, 0x02]") 60 | self.motor_num = motor_num 61 | self.enable_flag = enable_flag 62 | 63 | def __str__(self): 64 | return (f"ArmMsgMotorEnableDisableConfig(\n" 65 | f" motor_num: {self.motor_num },\n" 66 | f" enable_flag: {self.enable_flag },\n" 67 | f")") 68 | 69 | def __repr__(self): 70 | return self.__str__() 71 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_param_enquiry_and_config.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | class ArmMsgParamEnquiryAndConfig: 7 | ''' 8 | msg_v2_transmit 9 | 10 | 机械臂参数查询与设置指令 11 | 12 | CAN ID: 13 | 0x477 14 | 15 | Args: 16 | param_enquiry: 参数查询 17 | param_setting: 参数设置 18 | data_feedback_0x48x: 0x48X报文反馈设置 19 | end_load_param_setting_effective: 末端负载参数设置是否生效 20 | set_end_load: 设置末端负载 21 | 22 | 位描述: 23 | 24 | Byte 0: uint8,参数查询,查询末端 V/acc 25 | 0x01,查询末端 V/acc 26 | 0x02,查询碰撞防护等级 27 | 0x03,查询当前轨迹索引 28 | 0x04,查询夹爪/示教器参数索引 ---- 基于V1.5-2版本后 29 | Byte 1: uint8,参数设置, 30 | 设置末端 V/acc 参数为初始值: 0x01 31 | 设置全部关节限位、关节最大速度、关节加速度为默认值: 0x02 32 | Byte 2: uint8,0x48X 报文反馈设置, 33 | 无效:0x00 34 | 关闭周期反馈: 0x01; 35 | 开启周期反馈: 0x02; 36 | 开启后周期上报 1~6 号关节当前末端速度/加速度 37 | Byte 3: uint8,末端负载参数设置是否生效,有效值 : 0xAE 38 | Byte 4: uint8,设置末端负载, 39 | 0x00 : 空载; 40 | 0x01 : 半载; 41 | 0x02 : 满载; 42 | 0x03 : 无效 43 | ''' 44 | ''' 45 | msg_v2_transmit 46 | 47 | Robot Arm Parameter Query and Setting Command 48 | 49 | CAN ID: 50 | 0x477 51 | 52 | Args: 53 | param_enquiry: Parameter query. 54 | param_setting: Parameter setting. 55 | data_feedback_0x48x: 0x48X message feedback setting. 56 | end_load_param_setting_effective: Whether the end-load parameter setting is effective. 57 | set_end_load: Set the end load. 58 | 59 | Bit Description: 60 | 61 | Byte 0: uint8, parameter query, used to query end velocity/acceleration or collision protection level: 62 | 0x01: Query end velocity/acceleration. 63 | 0x02: Query collision protection level. 64 | 0x03: Query the current trajectory index. 65 | 0x04: Query gripper/teaching pendant parameter index (Based on version V1.5-2 and later) 66 | Byte 1: uint8, parameter setting, 67 | set end velocity/acceleration parameters to initial values(0x01) 68 | Sets all joint limits, joint maximum speed, and joint acceleration to default values (0x02). 69 | Byte 2: uint8, 0x48X message feedback setting: 70 | 0x00: Invalid. 71 | 0x01: Disable periodic feedback. 72 | 0x02: Enable periodic feedback (reports current end velocity/acceleration of joints 1~6). 73 | Byte 3: uint8, whether the end-load parameter setting is effective: 74 | Valid value: 0xAE. 75 | Byte 4: uint8, set the end load: 76 | 0x00: No load. 77 | 0x01: Half load. 78 | 0x02: Full load. 79 | 0x03: Invalid. 80 | ''' 81 | def __init__(self, 82 | param_enquiry:Literal[0x00, 0x01, 0x02, 0x03, 0x04] = 0x00, 83 | param_setting: Literal[0x00, 0x01, 0x02] = 0, 84 | data_feedback_0x48x: Literal[0x00, 0x01, 0x02] = 0x00, 85 | end_load_param_setting_effective: Literal[0x00, 0xAE] = 0, 86 | set_end_load: Literal[0x00, 0x01, 0x02, 0x03] = 0x03 87 | ): 88 | if param_enquiry not in [0x00, 0x01, 0x02, 0x03, 0x04]: 89 | raise ValueError(f"'param_enquiry' Value {param_enquiry} out of range [0x00, 0x01, 0x02, 0x03, 0x04]") 90 | if param_setting not in [0x00, 0x01, 0x02]: 91 | raise ValueError(f"'param_setting' Value {param_setting} out of range [0x00, 0x01, 0x02]") 92 | if data_feedback_0x48x not in [0x00, 0x01, 0x02]: 93 | raise ValueError(f"'data_feedback_0x48x' Value {data_feedback_0x48x} out of range [0x00, 0x01, 0x02]") 94 | if end_load_param_setting_effective not in [0x00, 0xAE]: 95 | raise ValueError(f"'end_load_param_setting_effective' Value {end_load_param_setting_effective} out of range [0x00, 0xAE]") 96 | if set_end_load not in [0x00, 0x01, 0x02, 0x03]: 97 | raise ValueError(f"'set_end_load' Value {set_end_load} out of range [0x00, 0x01, 0x02, 0x03]") 98 | self.param_enquiry = param_enquiry 99 | self.param_setting = param_setting 100 | self.data_feedback_0x48x = data_feedback_0x48x 101 | self.end_load_param_setting_effective = end_load_param_setting_effective 102 | self.set_end_load = set_end_load 103 | 104 | def __str__(self): 105 | return (f"ArmMsgParamEnquiryAndConfig(\n" 106 | f" param_enquiry: {self.param_enquiry},\n" 107 | f" param_setting: {self.param_setting},\n" 108 | f" data_feedback_0x48x: {self.data_feedback_0x48x},\n" 109 | f" end_load_param_setting_effective: {self.end_load_param_setting_effective},\n" 110 | f" set_end_load: {self.set_end_load}\n" 111 | f")") 112 | 113 | def __repr__(self): 114 | return self.__str__() 115 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_search_motor_max_angle_spd_acc_limit.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | 7 | class ArmMsgSearchMotorMaxAngleSpdAccLimit: 8 | ''' 9 | msg_v2_transmit 10 | 11 | 查询电机角度/最大速度/最大加速度限制指令 12 | 13 | CAN ID: 14 | 0x472 15 | 16 | Args: 17 | motor_num: 关节电机序号,1-6 18 | search_content: 查询内容,0x01-查询电机角度/最大速度,0x02-查询电机最大加速度限制 19 | 20 | 位描述: 21 | 22 | :Byte 0 motor_num: uint8, 关节电机序号。 23 | 值域 1-6,1-6 代表关节驱动器序号 24 | :Byte 1 search_content: uint8, 查询内容。 25 | 0x01 : 查询电机角度/最大速度 26 | 0x02 : 查询电机最大加速度限制 27 | ''' 28 | ''' 29 | msg_v2_transmit 30 | 31 | Motor Angle/Max Speed/Max Acceleration Limit Query Command 32 | 33 | CAN ID: 34 | 0x472 35 | 36 | Args: 37 | motor_num: Motor joint number. 38 | search_content: Query content. 39 | 40 | Bit Description: 41 | 42 | Byte 0: uint8, motor joint number. 43 | Value range: 1-6. 44 | 1-6: Represent joint driver numbers. 45 | Byte 1: uint8, query content. 46 | 0x01: Query motor angle/max speed. 47 | 0x02: Query motor max acceleration limit. 48 | ''' 49 | def __init__(self, 50 | motor_num: Literal[1, 2, 3, 4, 5, 6] = 1, 51 | search_content: Literal[0x01, 0x02] = 0x01): 52 | if motor_num not in [1, 2, 3, 4, 5, 6, 7]: 53 | raise ValueError(f"'motor_num' Value {motor_num} out of range [1, 2, 3, 4, 5, 6, 7]") 54 | if search_content not in [0x01, 0x02]: 55 | raise ValueError(f"'search_content' Value {search_content} out of range [0x01, 0x02]") 56 | self.motor_num = motor_num 57 | self.search_content = search_content 58 | 59 | def __str__(self): 60 | return (f"ArmMsgSearchMotorMaxAngleSpdAccConfig(\n" 61 | f" motor_num: {self.motor_num },\n" 62 | f" search_content: {self.search_content },\n" 63 | f")") 64 | 65 | def __repr__(self): 66 | return self.__str__() 67 | -------------------------------------------------------------------------------- /piper_sdk/piper_msgs/msg_v2/transmit/arm_set_instruction_response.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | 7 | class ArmMsgInstructionResponseConfig: 8 | ''' 9 | msg_v2_transmit 10 | 11 | 设置指令应答 12 | 13 | CAN ID: 14 | 0x476 15 | 16 | Args: 17 | instruction_index: 应答指令索引 18 | zero_config_success_flag: 零点是否设置成功 19 | 20 | 位描述: 21 | 22 | Byte 0: uint8, 应答指令索引 23 | 取设置指令 id 最后一个字节 24 | 例如：应答 0x471 设置指令时此位填充0x71 25 | Byte 1: uint8, 零点是否设置成功 26 | 零点成功设置 : 0x01 27 | 设置失败/未设置: 0x00 28 | 仅在关节设置指令--成功设置 N 号电机当前位置为零点时应答 0x01 29 | ''' 30 | ''' 31 | msg_v2_transmit 32 | 33 | Set Command Response 34 | 35 | CAN ID: 36 | 0x476 37 | 38 | Args: 39 | instruction_index: Response instruction index. 40 | zero_config_success_flag: Whether the zero-point configuration was successful. 41 | 42 | Bit Description: 43 | 44 | Byte 0: uint8, response instruction index. 45 | Fill in the last byte of the set command ID. 46 | Example: Responding to the 0x471 set command, this byte will be 0x71. 47 | Byte 1: uint8, zero-point configuration success flag. 48 | 0x01: Zero-point successfully set. 49 | 0x00: Failed to set/Not set. 50 | ''' 51 | def __init__(self, 52 | instruction_index: int = 0, 53 | zero_config_success_flag: Literal[0x00, 0x01] =0 ): 54 | if zero_config_success_flag not in [0x00, 0x01]: 55 | raise ValueError(f"'zero_config_success_flag' Value {zero_config_success_flag} out of range [0x01, 0x02]") 56 | self.instruction_index = instruction_index 57 | self.zero_config_success_flag = zero_config_success_flag 58 | 59 | def __str__(self): 60 | return (f"ArmMsgInstructionResponseConfig(\n" 61 | f" instruction_index: {self.instruction_index },\n" 62 | f" zero_config_success_flag: {self.zero_config_success_flag },\n" 63 | f")") 64 | 65 | def __repr__(self): 66 | return self.__str__() 67 | -------------------------------------------------------------------------------- /piper_sdk/piper_param/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | from .piper_param_manager import C_PiperParamManager 3 | 4 | __all__ = [ 5 | 'C_PiperParamManager' 6 | ] 7 | 8 | -------------------------------------------------------------------------------- /piper_sdk/piper_param/piper_param_manager.py: -------------------------------------------------------------------------------- 1 | import copy 2 | from typing_extensions import ( 3 | Literal, 4 | ) 5 | from ..version import PiperSDKVersion 6 | 7 | class C_PiperParamManager(): 8 | _instance = None 9 | 10 | def __new__(cls, *args, **kwargs): 11 | if cls._instance is None: 12 | cls._instance = super().__new__(cls) 13 | return cls._instance 14 | 15 | def __init__(self) -> None: 16 | ''' 17 | |joint_name| limit(rad) | limit(angle) | 18 | |----------| ---------- | ---------- | 19 | |joint1 | [-2.6179, 2.6179] | [-150.0, 150.0] | 20 | |joint2 | [0, 3.14] | [0, 180.0] | 21 | |joint3 | [-2.967, 0] | [-170, 0] | 22 | |joint4 | [-1.745, 1.745] | [-100.0, 100.0] | 23 | |joint5 | [-1.22, 1.22] | [-70.0, 70.0] | 24 | |joint6 | [-2.09439, 2.09439]| [-120.0, 120.0] | 25 | ''' 26 | if not hasattr(self, "PIPER_PARAM"): 27 | self.__PIPER_PARAM_ORIGIN = { 28 | "joint_limit":{ 29 | "j1": [-2.6179, 2.6179], 30 | "j2": [0, 3.14], 31 | "j3": [-2.967, 0], 32 | "j4": [-1.745, 1.745], 33 | "j5": [-1.22, 1.22], 34 | "j6": [-2.09439, 2.09439], 35 | }, 36 | "gripper_range": [0.0, 0.07], 37 | "piper_sdk_version": PiperSDKVersion.PIPER_SDK_CURRENT_VERSION 38 | } 39 | self.PIPER_PARAM = copy.deepcopy(self.__PIPER_PARAM_ORIGIN) 40 | 41 | def ResetDefaultParam(self): 42 | self.PIPER_PARAM.update(copy.deepcopy(self.__PIPER_PARAM_ORIGIN)) 43 | 44 | def GetPiperParamOrigin(self): 45 | return copy.deepcopy(self.__PIPER_PARAM_ORIGIN) 46 | 47 | def GetCurrentPiperParam(self): 48 | return copy.deepcopy(self.PIPER_PARAM) 49 | 50 | def GetCurrentPiperSDKVersion(self): 51 | return self.PIPER_PARAM["piper_sdk_version"] 52 | 53 | def GetJointLimitParam(self, 54 | joint_name: Literal["j1", "j2", "j3", "j4", "j5", "j6"]): 55 | if joint_name not in ["j1", "j2", "j3", "j4", "j5", "j6"]: 56 | raise ValueError(f'"joint_name" Value {joint_name} is not in ["j1", "j2", "j3", "j4", "j5", "j6"]') 57 | return self.PIPER_PARAM["joint_limit"][joint_name][0], self.PIPER_PARAM["joint_limit"][joint_name][1] 58 | 59 | def GetGripperRangeParam(self): 60 | return self.PIPER_PARAM["gripper_range"][0], self.PIPER_PARAM["gripper_range"][1] 61 | 62 | def SetJointLimitParam(self, 63 | joint_name: Literal["j1", "j2", "j3", "j4", "j5", "j6"], 64 | min_val: float, 65 | max_val: float): 66 | if joint_name not in ["j1", "j2", "j3", "j4", "j5", "j6"]: 67 | raise ValueError(f'"joint_name" Value {joint_name} is not in ["j1", "j2", "j3", "j4", "j5", "j6"]') 68 | if max_val - min_val < 0: 69 | raise ValueError(f'max_val should be greater than min_val.') 70 | self.PIPER_PARAM["joint_limit"][joint_name] = [min_val, max_val] 71 | 72 | def SetGripperRangeParam(self, 73 | min_val: float, 74 | max_val: float): 75 | if max_val - min_val < 0: 76 | raise ValueError(f'max_val should be greater than min_val.') 77 | self.PIPER_PARAM["gripper_range"] = [min_val, max_val] 78 | 79 | # a = C_PiperParamManager() 80 | # print( a.GetCurrentPiperParam()) 81 | # a.SetGripperRangeParam(-20000,30000) 82 | # a.SetJointLimitParam("j1",-20000,30000) 83 | # print( a.GetCurrentPiperParam()) 84 | # a.ResetDefaultParam() 85 | # print( a.GetCurrentPiperParam()) 86 | 87 | # # ✅ 测试单例模式 88 | # manager1 = C_PiperParamManager() 89 | # manager2 = C_PiperParamManager() 90 | 91 | # print(manager1 is manager2) # ✅ True，确保是同一个实例 92 | 93 | # # 修改 manager1 的参数 94 | # manager1.SetGripperRangeParam(-500, 500) 95 | # print(manager2.GetCurrentPiperParam()) # ✅ manager2 也受影响，说明是同一个实例 96 | 97 | # # 复位参数 98 | # manager2.ResetDefaultParam() 99 | # print(manager1.GetCurrentPiperParam()) # ✅ manager1 也被复位，单例模式生效！ -------------------------------------------------------------------------------- /piper_sdk/protocol/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | from .piper_protocol_base import C_PiperParserBase 3 | from .protocol_v2 import * 4 | 5 | __all__ = [ 6 | 'C_PiperParserBase', 7 | 'C_PiperParserV2' 8 | ] 9 | 10 | -------------------------------------------------------------------------------- /piper_sdk/protocol/protocol_v2/__init__.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | from .piper_protocol_v2 import C_PiperParserV2 3 | 4 | __all__ = [ 5 | "C_PiperParserV2" 6 | ] 7 | -------------------------------------------------------------------------------- /piper_sdk/pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | # requires = ["setuptools >= 40.0", "setuptools_scm>=8"] 3 | requires = ["setuptools >= 40.0"] 4 | build-backend = "setuptools.build_meta" 5 | 6 | [project] 7 | name = "piper_sdk" 8 | dynamic = ["readme", "version"] 9 | description = "A sdk to control piper" 10 | authors = [ 11 | {name = "RosenYin", email = "yinruocheng321@gmail.com"}, 12 | ] 13 | license = {text = "MIT License"} 14 | requires-python = ">=3.6" 15 | dependencies = [ 16 | "python-can>=3.3.4", 17 | ] 18 | classifiers = [ 19 | "Operating System :: POSIX :: Linux", 20 | ] 21 | 22 | [project.urls] 23 | Homepage = "https://github.com/agilexrobotics/piper_sdk" 24 | Repository = "https://github.com/agilexrobotics/piper_sdk" 25 | ChangeLog = "https://github.com/agilexrobotics/piper_sdk/blob/master/CHANGELOG.MD" 26 | 27 | [tool.setuptools.dynamic] 28 | readme = { file = "README.MD" } -------------------------------------------------------------------------------- /piper_sdk/utils/__init__.py: -------------------------------------------------------------------------------- 1 | from .fps import C_FPSCounter 2 | from .tf import ( 3 | quat_convert_euler, 4 | euler_convert_quat, 5 | ) 6 | 7 | from .logger_mag import LogManager, LogLevel 8 | import logging 9 | global_area = "PIPER" 10 | LogManager.init_logger(global_area=global_area, 11 | level=LogLevel.SILENT, 12 | log_to_file=False, 13 | log_file_name=None, 14 | log_file_path=None, 15 | file_mode='w') 16 | logger = LogManager.get_logger(global_area=global_area) 17 | 18 | __all__ = [ 19 | 'C_FPSCounter', 20 | 'quat_convert_euler', 21 | 'euler_convert_quat', 22 | 'logging', 23 | 'LogManager', 24 | 'LogLevel', 25 | 'global_area', 26 | 'logger', 27 | ] 28 | 29 | -------------------------------------------------------------------------------- /piper_sdk/utils/fps.py: -------------------------------------------------------------------------------- 1 | import threading 2 | import time 3 | from collections import defaultdict 4 | from collections import deque 5 | 6 | class C_FPSCounter: 7 | def __init__(self, 8 | start_realtime_fps:bool = False): 9 | """ 初始化 FPS 统计器 """ 10 | self.start_realtime_fps = start_realtime_fps 11 | self.fps_data = defaultdict(int) # 记录帧计数 12 | self.fps_results = defaultdict(float) # 计算出的 FPS 结果 13 | self.prev_data = defaultdict(int) # 上一次的计数值 14 | if(self.start_realtime_fps): 15 | self.time_stamps = defaultdict(deque) # 存储时间戳 16 | self.last_time = defaultdict(float) # 记录上次帧的时间 17 | self.lock = threading.Lock() # 确保线程安全 18 | self.running = False # 控制线程状态 19 | self.thread = None # 线程对象 20 | self.stop_event = threading.Event() # 用于控制线程退出 21 | self.__interval = 0.1 22 | 23 | def set_cal_fps_time_interval(self, interval:float=0.1): 24 | self.__interval = interval 25 | return self.__interval 26 | 27 | def get_cal_fps_time_interval(self): 28 | return self.__interval 29 | 30 | def add_variable(self, name, window_size=5000): 31 | """ 添加新的 FPS 变量，并限制时间窗口大小 """ 32 | with self.lock: 33 | if name not in self.fps_data: 34 | self.fps_data[name] = 0 35 | self.fps_results[name] = 0.0 36 | if(self.start_realtime_fps): 37 | self.time_stamps[name] = deque(maxlen=window_size) # 限制最大存储窗口 38 | self.last_time[name] = time.perf_counter() 39 | 40 | def increment(self, name): 41 | """ 递增帧计数，并记录时间戳 """ 42 | current_time = time.perf_counter() 43 | with self.lock: 44 | if name in self.fps_data: 45 | self.fps_data[name] += 1 46 | if(self.start_realtime_fps): 47 | self.time_stamps[name].append(current_time) # `deque` 自动管理过期数据 48 | self.last_time[name] = current_time 49 | 50 | def get_fps(self, name): 51 | """ 获取 1 秒内的 FPS 计算结果 """ 52 | multiple = 1 / self.__interval 53 | with self.lock: 54 | return self.fps_results.get(name, 0.0) * multiple 55 | 56 | def get_real_time_fps(self, name, window=1.0): 57 | """ 计算过去 window 秒的实时 FPS """ 58 | now = time.perf_counter() 59 | with self.lock: 60 | if(self.start_realtime_fps): 61 | while self.time_stamps[name] and now - self.time_stamps[name][0] > window: 62 | self.time_stamps[name].popleft() # 直接丢弃最早的时间戳 63 | 64 | return len(self.time_stamps[name]) / window if self.time_stamps[name] else 0.0 65 | else: return -1 66 | 67 | def start(self): 68 | """ 启动 FPS 计算线程，防止重复启动 """ 69 | with self.lock: 70 | if self.running: 71 | return # 已经在运行，避免重复启动 72 | self.running = True 73 | self.stop_event.clear() 74 | 75 | self.thread = threading.Thread(target=self._calculate_fps, daemon=True) 76 | self.thread.start() 77 | 78 | def stop(self): 79 | """ 停止 FPS 计算线程 """ 80 | with self.lock: 81 | if not self.running: 82 | return # 已经停止 83 | self.running = False 84 | self.stop_event.set() # 设置事件，确保线程退出 85 | 86 | if self.thread and self.thread.is_alive(): 87 | self.thread.join() 88 | 89 | def cal_average(self, *args): 90 | """ 计算一组数的平均值，但是只在所有数都不是0的情况下才算；如果有0，就直接返回0 """ 91 | return round(sum(args) / len(args) if args and all(args) else 0,3) 92 | 93 | def _calculate_fps(self): 94 | """ 定期计算 FPS """ 95 | while not self.stop_event.is_set(): 96 | with self.lock: 97 | for name in self.fps_data: 98 | self.fps_results[name] = self.fps_data[name] - self.prev_data[name] 99 | self.prev_data[name] = self.fps_data[name] 100 | self.stop_event.wait(self.__interval) # 用 wait() 代替 sleep()，更易控制 101 | 102 | # def camera_simulation(fps_counter, name, interval, stop_after=None): 103 | # """ 104 | # 模拟相机帧生成并调用 increment 105 | # :param fps_counter: FPSCounter 实例 106 | # :param name: 相机变量名称 107 | # :param interval: 模拟帧生成的时间间隔 108 | # :param stop_after: 停止生成数据的时间（秒），为 None 时不停止 109 | # """ 110 | # start_time = time.perf_counter() 111 | # while True: 112 | # if stop_after is not None and time.perf_counter() - start_time > stop_after: 113 | # print(f"{name} stopped generating data.") 114 | # break 115 | # fps_counter.increment(name) 116 | # time.sleep(interval) 117 | 118 | 119 | # if __name__ == "__main__": 120 | # fps_counter = C_FPSCounter() 121 | # fps_counter.add_variable("camera1") 122 | # fps_counter.add_variable("camera2") 123 | 124 | # fps_counter.start() 125 | 126 | # try: 127 | # # 创建线程模拟相机1和相机2的帧生成 128 | # camera1_thread = threading.Thread(target=camera_simulation, args=(fps_counter, "camera1", 0.01, 5), daemon=True) 129 | # camera2_thread = threading.Thread(target=camera_simulation, args=(fps_counter, "camera2", 0.01, None), daemon=True) 130 | 131 | # camera1_thread.start() 132 | # camera2_thread.start() 133 | 134 | # # 打印 FPS 135 | # while True: 136 | # print(f"FPS for camera1 (1s avg): {fps_counter.get_fps('camera1')}") 137 | # print(f"FPS for camera2 (1s avg): {fps_counter.get_fps('camera2')}") 138 | # print(f"Real-time FPS for camera1: {fps_counter.get_real_time_fps('camera1', window=1.0)}") 139 | # print(f"Real-time FPS for camera2: {fps_counter.get_real_time_fps('camera2', window=1.0)}") 140 | # print(f"Instant FPS for camera1: {fps_counter.get_instant_fps('camera1')}") 141 | # print(f"Instant FPS for camera2: {fps_counter.get_instant_fps('camera2')}") 142 | # time.sleep(0.01) 143 | 144 | # finally: 145 | # fps_counter.stop() 146 | -------------------------------------------------------------------------------- /piper_sdk/utils/tf.py: -------------------------------------------------------------------------------- 1 | from typing import Tuple 2 | import math 3 | from typing_extensions import ( 4 | Literal, 5 | ) 6 | 7 | # 定义欧拉角顺序编码表 8 | _AXES2TUPLE = { 9 | 'sxyz': (0, 0, 0, 0), 10 | 'rzyx': (0, 0, 0, 1), 11 | } 12 | axes = 'sxyz' 13 | # 用于轴索引计算 14 | _NEXT_AXIS = [1, 2, 0, 1] 15 | # 设置浮点比较误差阈值 16 | _EPS = 1e-10 17 | 18 | def normalize_quat(qx, qy, qz, qw): 19 | norm = math.sqrt(qx**2 + qy**2 + qz**2 + qw**2) 20 | return qx / norm, qy / norm, qz / norm, qw / norm 21 | 22 | def quat_convert_euler(qx: float, qy: float, qz: float, qw: float) -> Tuple[float, float, float]: 23 | # axes: Literal['sxyz', 'rzyx'] = 'sxyz') -> Tuple[float, float, float]: 24 | """ 25 | 将四元数 [x, y, z, w] 转换为欧拉角 (roll, pitch, yaw)。 26 | 27 | 参数: 28 | x, y, z, w - 四元数各分量 29 | axes - 欧拉角轴顺序，支持 'sxyz' 或 'rzyx' 30 | 31 | 返回: 32 | tuple(float, float, float): 欧拉角 (roll, pitch, yaw)，单位为弧度 33 | """ 34 | 35 | # 轴顺序设置 36 | try: 37 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 38 | except (KeyError, AttributeError): 39 | raise ValueError(f"Unsupported axes specification: '{axes}'. " 40 | f"Only 'sxyz' and 'rzyx' are currently supported.") 41 | # 轴索引对应 x=0, y=1, z=2 42 | # 获取轴顺序索引 43 | i = firstaxis 44 | j = _NEXT_AXIS[i + parity] 45 | k = _NEXT_AXIS[i - parity + 1] 46 | qx, qy, qz, qw = normalize_quat(qx, qy, qz, qw) 47 | # 矩阵 M[i][j] 表达方式：预先展开 3x3 旋转矩阵 48 | M = [[0.0] * 3 for _ in range(3)] 49 | M[0][0] = 1 - 2*(qy**2 + qz**2) 50 | M[0][1] = 2*(qx*qy - qz*qw) 51 | M[0][2] = 2*(qx*qz + qy*qw) 52 | M[1][0] = 2*(qx*qy + qz*qw) 53 | M[1][1] = 1 - 2*(qx**2 + qz**2) 54 | M[1][2] = 2*(qy*qz - qx*qw) 55 | M[2][0] = 2*(qx*qz - qy*qw) 56 | M[2][1] = 2*(qy*qz + qx*qw) 57 | M[2][2] = 1 - 2*(qx**2 + qy**2) 58 | 59 | # 计算欧拉角 60 | if repetition: 61 | sy = math.sqrt(M[i][j] ** 2 + M[i][k] ** 2) 62 | if sy > _EPS: 63 | ax = math.atan2(M[i][j], M[i][k]) 64 | ay = math.atan2(sy, M[i][i]) 65 | az = math.atan2(M[j][i], -M[k][i]) 66 | else: 67 | ax = math.atan2(-M[j][k], M[j][j]) 68 | ay = math.atan2(sy, M[i][i]) 69 | az = 0.0 70 | else: 71 | cy = math.sqrt(M[i][i] ** 2 + M[j][i] ** 2) 72 | if cy > _EPS: 73 | ax = math.atan2(M[k][j], M[k][k]) 74 | ay = math.atan2(-M[k][i], cy) 75 | az = math.atan2(M[j][i], M[i][i]) 76 | else: 77 | ax = math.atan2(-M[j][k], M[j][j]) 78 | ay = math.atan2(-M[k][i], cy) 79 | az = 0.0 80 | 81 | # 调整角度方向 82 | if parity: 83 | ax, ay, az = -ax, -ay, -az 84 | if frame: 85 | ax, az = az, ax 86 | 87 | return ax, ay, az 88 | 89 | def euler_convert_quat(roll:float, pitch:float, yaw:float)->Tuple[float, float, float, float]: 90 | """ 91 | 将欧拉角（roll, pitch, yaw）转换为四元数。 92 | 93 | 参数: 94 | roll - 绕X轴旋转角（单位：弧度） 95 | pitch - 绕Y轴旋转角（单位：弧度） 96 | yaw - 绕Z轴旋转角（单位：弧度） 97 | 98 | 返回: 99 | list: 四元数 [x, y, z, w] 100 | """ 101 | 102 | try: 103 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 104 | except (KeyError, AttributeError): 105 | raise ValueError(f"Unsupported axes specification: '{axes}'. " 106 | f"Only 'sxyz' and 'rzyx' are currently supported.") 107 | # 轴索引对应 x=0, y=1, z=2 108 | # 获取轴顺序索引 109 | i = firstaxis 110 | j = _NEXT_AXIS[i + parity] 111 | k = _NEXT_AXIS[i - parity + 1] 112 | 113 | # 坐标系调整 114 | if frame: 115 | roll, yaw = yaw, roll 116 | if parity: 117 | pitch = -pitch 118 | 119 | # 角度减半 120 | roll *= 0.5 121 | pitch *= 0.5 122 | yaw *= 0.5 123 | 124 | # 三角函数 125 | c_roll = math.cos(roll) 126 | s_roll = math.sin(roll) 127 | c_pitch = math.cos(pitch) 128 | s_pitch = math.sin(pitch) 129 | c_yaw = math.cos(yaw) 130 | s_yaw = math.sin(yaw) 131 | 132 | cc = c_roll * c_yaw 133 | cs = c_roll * s_yaw 134 | sc = s_roll * c_yaw 135 | ss = s_roll * s_yaw 136 | 137 | # 初始化四元数 [x, y, z, w] 138 | q = [0.0, 0.0, 0.0, 0.0] 139 | 140 | if repetition: 141 | q[i] = c_pitch * (cs + sc) 142 | q[j] = s_pitch * (cc + ss) 143 | q[k] = s_pitch * (cs - sc) 144 | q[3] = c_pitch * (cc - ss) 145 | else: 146 | q[i] = c_pitch * sc - s_pitch * cs 147 | q[j] = c_pitch * ss + s_pitch * cc 148 | q[k] = c_pitch * cs - s_pitch * sc 149 | q[3] = c_pitch * cc + s_pitch * ss 150 | 151 | if parity: 152 | q[j] *= -1 153 | 154 | return q[0], q[1], q[2], q[3] # [qx, qy, qz, qw] -------------------------------------------------------------------------------- /piper_sdk/version.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*-coding:utf8-*- 3 | 4 | from enum import Enum, auto 5 | 6 | class PiperSDKVersion(Enum): 7 | PIPER_SDK_VERSION_0_2_19 = '0.2.19' 8 | PIPER_SDK_VERSION_0_2_20 = '0.2.20' 9 | PIPER_SDK_VERSION_0_3_0 = '0.3.0' 10 | PIPER_SDK_VERSION_0_3_2 = '0.3.2' 11 | PIPER_SDK_VERSION_0_3_3 = '0.3.3' 12 | PIPER_SDK_VERSION_0_4_0 = '0.4.0' 13 | PIPER_SDK_VERSION_0_4_1 = '0.4.1' 14 | PIPER_SDK_VERSION_0_4_2 = '0.4.2' 15 | PIPER_SDK_VERSION_0_4_3 = '0.4.3' 16 | PIPER_SDK_VERSION_0_5_0 = '0.5.0' 17 | PIPER_SDK_VERSION_0_6_0 = '0.6.0' 18 | PIPER_SDK_VERSION_0_6_1 = '0.6.1' 19 | PIPER_SDK_CURRENT_VERSION = PIPER_SDK_VERSION_0_6_1 20 | PIPER_SDK_VERSION_UNKNOWN = 'unknown' 21 | def __str__(self): 22 | return f"{self.name} ({self.value})" 23 | def __repr__(self): 24 | return f"{self.name}: {self.value}" -------------------------------------------------------------------------------- /rm_tmp.sh: -------------------------------------------------------------------------------- 1 | rm -r ./build 2 | rm -r ./dist 3 | rm -r ./piper_sdk.egg-info -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | from setuptools import setup, find_packages 3 | 4 | # 获取 setup.py 所在目录 5 | here = os.path.abspath(os.path.dirname(__file__)) 6 | 7 | setup( 8 | name='piper_sdk', 9 | version='0.6.1', 10 | setup_requires=['setuptools>=40.0'], 11 | long_description=open(os.path.join(here, 'DESCRIPTION.MD'), encoding='utf-8').read(), 12 | long_description_content_type='text/markdown', 13 | url='https://github.com/agilexrobotics/piper_sdk', 14 | license='MIT License', 15 | packages=find_packages(include=['piper_sdk', 'piper_sdk.*']), 16 | include_package_data=True, 17 | package_data={ 18 | '': ['LICENSE', '*.sh', '*.MD'], 19 | 'piper_sdk/asserts': ['*'], 20 | }, 21 | install_requires=[ 22 | 'python-can>=3.3.4', 23 | ], 24 | entry_points={}, 25 | author='Agilex Robotice Co., Ltd.', 26 | author_email='', 27 | description='A sdk to control Agilex piper arm', 28 | platforms=['Linux'], 29 | classifiers=[ 30 | 'License :: OSI Approved :: MIT License', 31 | 'Programming Language :: Python :: 3.6', 32 | 'Operating System :: OS Independent', 33 | ], 34 | python_requires='>=3.6', 35 | project_urls={ 36 | 'Repository': 'https://github.com/agilexrobotics/piper_sdk', 37 | 'ChangeLog': 'https://github.com/agilexrobotics/piper_sdk/blob/master/CHANGELOG.MD', 38 | }, 39 | ) 40 | --------------------------------------------------------------------------------