integrate agilex piper arm into lerobot
This commit is contained in:
brucecai-2001
138
piper_scripts/can_activate.sh
Normal file
138
piper_scripts/can_activate.sh
Normal file
@@ -0,0 +1,138 @@
|
||||
#!/bin/bash
|
||||
|
||||
# The default CAN name can be set by the user via command-line parameters.
|
||||
DEFAULT_CAN_NAME="${1:-can0}"
|
||||
|
||||
# The default bitrate for a single CAN module can be set by the user via command-line parameters.
|
||||
DEFAULT_BITRATE="${2:-1000000}"
|
||||
|
||||
# USB hardware address (optional parameter)
|
||||
USB_ADDRESS="${3}"
|
||||
echo "-------------------START-----------------------"
|
||||
# Check if ethtool is installed.
|
||||
if ! dpkg -l | grep -q "ethtool"; then
|
||||
echo "\e[31mError: ethtool not detected in the system.\e[0m"
|
||||
echo "Please use the following command to install ethtool:"
|
||||
echo "sudo apt update && sudo apt install ethtool"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# Check if can-utils is installed.
|
||||
if ! dpkg -l | grep -q "can-utils"; then
|
||||
echo "\e[31mError: can-utils not detected in the system.\e[0m"
|
||||
echo "Please use the following command to install ethtool:"
|
||||
echo "sudo apt update && sudo apt install can-utils"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
echo "Both ethtool and can-utils are installed."
|
||||
|
||||
# Retrieve the number of CAN modules in the current system.
|
||||
CURRENT_CAN_COUNT=$(ip link show type can | grep -c "link/can")
|
||||
|
||||
# Verify if the number of CAN modules in the current system matches the expected value.
|
||||
if [ "$CURRENT_CAN_COUNT" -ne "1" ]; then
|
||||
if [ -z "$USB_ADDRESS" ]; then
|
||||
# Iterate through all CAN interfaces.
|
||||
for iface in $(ip -br link show type can | awk '{print $1}'); do
|
||||
# Use ethtool to retrieve bus-info.
|
||||
BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}')
|
||||
|
||||
if [ -z "$BUS_INFO" ];then
|
||||
echo "Error: Unable to retrieve bus-info for interface $iface."
|
||||
continue
|
||||
fi
|
||||
|
||||
echo "Interface $iface is inserted into USB port $BUS_INFO"
|
||||
done
|
||||
echo -e " \e[31m Error: The number of CAN modules detected by the system ($CURRENT_CAN_COUNT) does not match the expected number (1). \e[0m"
|
||||
echo -e " \e[31m Please add the USB hardware address parameter, such as: \e[0m"
|
||||
echo -e " bash can_activate.sh can0 1000000 1-2:1.0"
|
||||
echo "-------------------ERROR-----------------------"
|
||||
exit 1
|
||||
fi
|
||||
fi
|
||||
|
||||
# Load the gs_usb module.
|
||||
# sudo modprobe gs_usb
|
||||
# if [ $? -ne 0 ]; then
|
||||
# echo "Error: Unable to load the gs_usb module."
|
||||
# exit 1
|
||||
# fi
|
||||
|
||||
if [ -n "$USB_ADDRESS" ]; then
|
||||
echo "Detected USB hardware address parameter: $USB_ADDRESS"
|
||||
|
||||
# Use ethtool to find the CAN interface corresponding to the USB hardware address.
|
||||
INTERFACE_NAME=""
|
||||
for iface in $(ip -br link show type can | awk '{print $1}'); do
|
||||
BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}')
|
||||
if [ "$BUS_INFO" == "$USB_ADDRESS" ]; then
|
||||
INTERFACE_NAME="$iface"
|
||||
break
|
||||
fi
|
||||
done
|
||||
|
||||
if [ -z "$INTERFACE_NAME" ]; then
|
||||
echo "Error: Unable to find CAN interface corresponding to USB hardware address $USB_ADDRESS."
|
||||
exit 1
|
||||
else
|
||||
echo "Found the interface corresponding to USB hardware address $USB_ADDRESS: $INTERFACE_NAME."
|
||||
fi
|
||||
else
|
||||
# Retrieve the unique CAN interface.
|
||||
INTERFACE_NAME=$(ip -br link show type can | awk '{print $1}')
|
||||
|
||||
# Check if the interface name has been retrieved.
|
||||
if [ -z "$INTERFACE_NAME" ]; then
|
||||
echo "Error: Unable to detect CAN interface."
|
||||
exit 1
|
||||
fi
|
||||
BUS_INFO=$(sudo ethtool -i "$INTERFACE_NAME" | grep "bus-info" | awk '{print $2}')
|
||||
echo "Expected to configure a single CAN module, detected interface $INTERFACE_NAME with corresponding USB address $BUS_INFO."
|
||||
fi
|
||||
|
||||
# Check if the current interface is already activated.
|
||||
IS_LINK_UP=$(ip link show "$INTERFACE_NAME" | grep -q "UP" && echo "yes" || echo "no")
|
||||
|
||||
# Retrieve the bitrate of the current interface.
|
||||
CURRENT_BITRATE=$(ip -details link show "$INTERFACE_NAME" | grep -oP 'bitrate \K\d+')
|
||||
|
||||
if [ "$IS_LINK_UP" == "yes" ] && [ "$CURRENT_BITRATE" -eq "$DEFAULT_BITRATE" ]; then
|
||||
echo "Interface $INTERFACE_NAME is already activated with a bitrate of $DEFAULT_BITRATE."
|
||||
|
||||
# Check if the interface name matches the default name.
|
||||
if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then
|
||||
echo "Rename interface $INTERFACE_NAME to $DEFAULT_CAN_NAME."
|
||||
sudo ip link set "$INTERFACE_NAME" down
|
||||
sudo ip link set "$INTERFACE_NAME" name "$DEFAULT_CAN_NAME"
|
||||
sudo ip link set "$DEFAULT_CAN_NAME" up
|
||||
echo "The interface has been renamed to $DEFAULT_CAN_NAME and reactivated."
|
||||
else
|
||||
echo "The interface name is already $DEFAULT_CAN_NAME."
|
||||
fi
|
||||
else
|
||||
# If the interface is not activated or the bitrate is different, configure it.
|
||||
if [ "$IS_LINK_UP" == "yes" ]; then
|
||||
echo "Interface $INTERFACE_NAME is already activated, but the bitrate is $CURRENT_BITRATE, which does not match the set value of $DEFAULT_BITRATE."
|
||||
else
|
||||
echo "Interface $INTERFACE_NAME is not activated or bitrate is not set."
|
||||
fi
|
||||
|
||||
# Set the interface bitrate and activate it.
|
||||
sudo ip link set "$INTERFACE_NAME" down
|
||||
sudo ip link set "$INTERFACE_NAME" type can bitrate $DEFAULT_BITRATE
|
||||
sudo ip link set "$INTERFACE_NAME" up
|
||||
echo "Interface $INTERFACE_NAME has been reset to bitrate $DEFAULT_BITRATE and activated."
|
||||
|
||||
# Rename the interface to the default name.
|
||||
if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then
|
||||
echo "Rename interface $INTERFACE_NAME to $DEFAULT_CAN_NAME."
|
||||
sudo ip link set "$INTERFACE_NAME" down
|
||||
sudo ip link set "$INTERFACE_NAME" name "$DEFAULT_CAN_NAME"
|
||||
sudo ip link set "$DEFAULT_CAN_NAME" up
|
||||
echo "The interface has been renamed to $DEFAULT_CAN_NAME and reactivated."
|
||||
fi
|
||||
fi
|
||||
|
||||
echo "-------------------OVER------------------------"
|
||||
69
piper_scripts/piper_disable.py
Normal file
69
piper_scripts/piper_disable.py
Normal file
@@ -0,0 +1,69 @@
|
||||
#!/usr/bin/env python3
|
||||
# -*-coding:utf8-*-
|
||||
# 注意demo无法直接运行，需要pip安装sdk后才能运行
|
||||
# 使能机械臂
|
||||
from typing import (
|
||||
Optional,
|
||||
)
|
||||
import time
|
||||
from piper_sdk import *
|
||||
|
||||
def enable_fun(piper:C_PiperInterface_V2, enable:bool):
|
||||
'''
|
||||
使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序
|
||||
'''
|
||||
enable_flag = False
|
||||
loop_flag = False
|
||||
# 设置超时时间（秒）
|
||||
timeout = 5
|
||||
# 记录进入循环前的时间
|
||||
start_time = time.time()
|
||||
elapsed_time_flag = False
|
||||
while not (loop_flag):
|
||||
elapsed_time = time.time() - start_time
|
||||
print(f"--------------------")
|
||||
enable_list = []
|
||||
enable_list.append(piper.GetArmLowSpdInfoMsgs().motor_1.foc_status.driver_enable_status)
|
||||
enable_list.append(piper.GetArmLowSpdInfoMsgs().motor_2.foc_status.driver_enable_status)
|
||||
enable_list.append(piper.GetArmLowSpdInfoMsgs().motor_3.foc_status.driver_enable_status)
|
||||
enable_list.append(piper.GetArmLowSpdInfoMsgs().motor_4.foc_status.driver_enable_status)
|
||||
enable_list.append(piper.GetArmLowSpdInfoMsgs().motor_5.foc_status.driver_enable_status)
|
||||
enable_list.append(piper.GetArmLowSpdInfoMsgs().motor_6.foc_status.driver_enable_status)
|
||||
if(enable):
|
||||
enable_flag = all(enable_list)
|
||||
piper.EnableArm(7)
|
||||
piper.GripperCtrl(0,1000,0x01, 0)
|
||||
else:
|
||||
enable_flag = any(enable_list)
|
||||
piper.DisableArm(7)
|
||||
piper.GripperCtrl(0,1000,0x02, 0)
|
||||
print(f"使能状态: {enable_flag}")
|
||||
print(f"--------------------")
|
||||
if(enable_flag == enable):
|
||||
loop_flag = True
|
||||
enable_flag = True
|
||||
else:
|
||||
loop_flag = False
|
||||
enable_flag = False
|
||||
# 检查是否超过超时时间
|
||||
if elapsed_time > timeout:
|
||||
print(f"超时....")
|
||||
elapsed_time_flag = True
|
||||
enable_flag = False
|
||||
loop_flag = True
|
||||
break
|
||||
time.sleep(0.5)
|
||||
resp = enable_flag
|
||||
print(f"Returning response: {resp}")
|
||||
return resp
|
||||
|
||||
# 测试代码
|
||||
if __name__ == "__main__":
|
||||
piper = C_PiperInterface_V2()
|
||||
piper.ConnectPort()
|
||||
import time
|
||||
flag = enable_fun(piper=piper, enable=False)
|
||||
if(flag == True):
|
||||
print("失能成功!!!!")
|
||||
exit(0)
|
||||
pass
|
||||
Reference in New Issue
Block a user