Panthera-HT 机械臂 ROS 2 Humble 工作空间

source /opt/ros/humble/setup.bash

cd ~
git clone https://github.com/HighTorque-Robotics/Panthera_HT_ROS2.git
cd Panthera_HT_ROS2

# ROS 2、MoveIt、ros2_control 和 Gazebo Sim
sudo apt install -y \
  ros-humble-desktop \
  ros-humble-moveit \
  ros-humble-ros2-control \
  ros-humble-ros2-controllers \
  ros-humble-gazebo-ros-pkgs \
  ros-humble-pinocchio \
  ros-humble-xacro
  
# 底层 SDK 和标定 GUI 依赖
sudo apt install -y \
  libyaml-cpp-dev \
  libserialport-dev \
  qt6-base-dev

sudo rosdep init  # 仅首次执行；若已初始化可跳过
rosdep update
rosdep install --from-paths src --ignore-src -r -y

colcon build --symlink-install \
  --packages-skip moveit_calibration_gui moveit_calibration_plugins \
  --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF \
  -DPython3_EXECUTABLE=/usr/bin/python3

cd ~/Panthera_HT_ROS2
source /opt/ros/humble/setup.bash
colcon build --symlink-install --cmake-args \
  -DCMAKE_BUILD_TYPE=Release \
  -DPython3_EXECUTABLE=/usr/bin/python3
source install/setup.bash

echo "source ~/Panthera_HT_ROS2/install/setup.bash" >> ~/.bashrc

# 查看串口设备
ls /dev/ttyACM*

# 添加当前用户到 dialout 组（需重新登录生效）
sudo usermod -aG dialout $USER

cd ~/Panthera-HT_ROS2
source /opt/ros/humble/setup.bash
source install/setup.bash

ros2 pkg prefix rviz2                   
ros2 pkg prefix moveit_ros_move_group    
ros2 pkg prefix moveit_ros_visualization
# 均为/opt/ros/humble

# 确认串口已连接
ls /dev/ttyACM*

ros2 launch panthera_bringup hardware_moveit_rviz.launch.py

ros2 launch panthera_bringup hardware_moveit_rviz_mit.launch.py

ros2 launch panthera_bringup hardware_moveit_rviz_mit.launch.py --show-args

ros2 launch panthera_bringup hardware_moveit_rviz_mit.launch.py \
  mit_kp:="10.0,25.0,25.0,5.0,5.0,2.5" \
  mit_kd:="1.5,2.0,2.0,0.6,0.6,0.3"

source install/setup.bash
ros2 launch panthera_moveit_config demo.launch.py

cd ~/Panthera-HT_ROS2
source /opt/ros/humble/setup.bash
source install/setup.bash

ros2 launch panthera_gazebo gazebo_moveit.launch.py

cd ~/Panthera-HT_ROS2
source /opt/ros/humble/setup.bash
source install/setup.bash

ros2 control list_controllers

ros2 run panthera_moveit_commander test_moveit

ros2 run panthera_moveit_commander test_gripper

source install/setup.bash
ros2 launch panthera_sdk_control arm_control.launch.py

ros2 launch panthera_sdk_control arm_control.launch.py tip_link:=link6

# 查看当前关节角度（弧度）
ros2 topic echo /joint_states_single --once

# 查看机械臂状态（使能、故障、夹爪等）
ros2 topic echo /arm_status --once

# 查看末端执行器位姿（xyz + rpy，20Hz）
ros2 topic echo /end_pose_euler --once

# 通过话题发送关节角度（6个值，弧度）
ros2 topic pub --once /arm_joint_cmd example_interfaces/msg/Float64MultiArray \
  "{data: [0.0, 0.8, 0.8, 0.3, 0.0, 0.0]}"

# 通过服务发送关节角度（支持速度缩放 0.0~1.0）
ros2 service call /move_to_joint panthera_interfaces/srv/MoveToJoint \
  "{joint_angles: [0.0, 0.8, 0.8, 0.3, 0.0, 0.0], velocity_scaling: 0.5}"

# 回零位
ros2 service call /go_zero_srv panthera_interfaces/srv/GoZero "{use_mit_mode: false}"

# 通过服务移动到笛卡尔位姿（内置 KDL 逆运动学求解）
ros2 service call /move_to_pose panthera_interfaces/srv/MoveToPose \
  "{x: 0.3, y: 0.0, z: 0.2, roll: 0.0, pitch: 1.57, yaw: 0.0, velocity_scaling: 0.5}"

# 笛卡尔直线路径（cartesian_path: true，1cm步长线性插值）
ros2 service call /move_to_pose panthera_interfaces/srv/MoveToPose \
  "{x: 0.3, y: 0.0, z: 0.2, roll: 0.0, pitch: 1.57, yaw: 0.0, cartesian_path: true, velocity_scaling: 0.3}"

# 通过话题发送位姿命令（mode1: 0=关节空间规划, 1=笛卡尔直线路径）
ros2 topic pub --once /pos_cmd panthera_interfaces/msg/PosCmd \
  "{x: 0.3, y: 0.0, z: 0.2, roll: 0.0, pitch: 1.57, yaw: 0.0, gripper: 1.0, mode1: 0}"

# 话题控制（true=打开, false=关闭）
ros2 topic pub --once /gripper_cmd example_interfaces/msg/Bool "{data: true}"
ros2 topic pub --once /gripper_cmd example_interfaces/msg/Bool "{data: false}"

# 服务控制（open / close / half_open）
ros2 service call /gripper_control panthera_interfaces/srv/GripperControl "{action: 'open'}"
ros2 service call /gripper_control panthera_interfaces/srv/GripperControl "{action: 'close'}"

# 精细控制（角度 + 力矩）
ros2 service call /gripper_srv panthera_interfaces/srv/GripperSrv \
  "{gripper_angle: 0.5, gripper_effort: 1.0, gripper_code: 0, set_zero: false}"

# 使能/失能（话题）
ros2 topic pub --once /enable_flag example_interfaces/msg/Bool "{data: true}"
ros2 topic pub --once /enable_flag example_interfaces/msg/Bool "{data: false}"

# 使能/失能（服务）
ros2 service call /enable_srv panthera_interfaces/srv/Enable "{enable_request: true}"
ros2 service call /enable_srv panthera_interfaces/srv/Enable "{enable_request: false}"

# 急停
ros2 service call /stop_srv std_srvs/srv/Trigger "{}"

# 复位（清除错误并重新使能）
ros2 service call /reset_srv std_srvs/srv/Trigger "{}"

source install/setup.bash

# 画圆轨迹
ros2 run panthera_moveit_commander draw_circle

# 三平面画圆
ros2 run panthera_moveit_commander draw_three_circles

# 夹爪测试
ros2 run panthera_moveit_commander test_gripper

# 正弦轨迹
ros2 run panthera_moveit_commander sin_trajectory_control

source install/setup.bash

# 获取机器人关节状态
# 实时打印 6 个关节和夹爪的位置、速度、力矩，每 0.5 秒刷新一次。
# 用于确认机械臂通信正常和查看当前姿态，Ctrl+C 退出。
ros2 run hightorque_robot 0_robot_get_state

# 设置关节零位
# 将机械臂当前位置设为所有关节的零位参考点，设置后实时显示关节状态。
# 使用前请先将机械臂摆到期望的零位姿态。
ros2 run hightorque_robot 0_robot_set_zero

# PD 关节控制
# 控制律：τ = Kp*(q_des - q) + Kd*(0 - v)，电机内部 PD 控制。
# 机械臂移动到预设关节角度 [0, 0.7, 0.7, -0.1, 0, 0] rad 并保持。
# 每秒打印各关节位置、速度、力矩，Ctrl+C 退出后电机掉电。
ros2 run hightorque_robot 1_PD_control

# 位置速度控制
# 使用位置-速度-最大力矩模式，机械臂依次经过多个预设位置：
# 零位 → 位置2 → 位置1 → 位置2 → 零位，每段运动阻塞等待到位。
# 运动过程中演示夹爪开合。适合验证基本运动功能。
ros2 run hightorque_robot 1_PosVel_control

# 关节空间阻抗控制（需要 Pinocchio）
# 控制律：τ = K*(q_des - q) + B*(0 - v) + G(q)
# 机械臂移动到预设关节角度 [0, 0.7, 0.7, -0.1, 0, 0, 0] rad 后保持。
# 用手推动会感受到弹簧般的回复力，松手后自动回到期望位置。
# 重力补偿 G(q) 使机械臂不会因自重下垂。K 和 B 决定"软硬"手感。
ros2 run hightorque_robot 2_joint_impedance_control

# 笛卡尔空间阻抗控制（但是对关节加了PD控制器）（需要 Pinocchio）
# 控制律：τ = J^T * [K*(x_des - x) - B*v_ee] + G(q)
# 根据固定关节角 [0, 0.7, 0.7, -0.1, 0, 0] 正运动学算出末端期望位姿，
# 机械臂末端保持在该笛卡尔位置。推动末端会偏离，松手后回到原位。
# 位置刚度 200 N/m，姿态刚度 4 Nm/rad，手感较硬。
ros2 run hightorque_robot 3_cartesian_impedance_control

# 纯粹的笛卡尔空间阻抗控制（需要 Pinocchio）
# 控制律：τ = J^T * [K*(x_des - x) - B*v_ee] + G(q)，无关节空间约束
# 启动后取当前末端位姿作为期望位置，末端保持不动。
# 推动末端 → 偏离后松手会回到原位。
# 推动机械臂中间连杆 → 关节会移动但末端尽量保持不动（零空间自由运动）。
# 位置刚度 100 N/m，手感比 3_cartesian 更柔顺。
ros2 run hightorque_robot 5_pure_cartesian_impedance_control

# 笛卡尔阻抗 A-B 点往返运动（需要 Pinocchio）
# 控制律：τ = J^T * [K*(x_des - x) - B*(v_ee - v_des)] + G(q)
# 启动后取当前末端位置为 A 点，手动输入 XYZ 偏移量定义 B 点（建议 0.1~0.2m）。
# 机械臂末端以 0.15 m/s 在 A-B 之间匀速往返运动。
# 运动中保持阻抗特性，可用手推动使其偏离，松手后继续向目标点运动。
ros2 run hightorque_robot 3_cartesian_impedance_ab_motion

ros2 run hightorque_robot 0_robot_get_state /path/to/your/config.yaml

source install/setup.bash

ros2 run hightorque_robot motor_feedback       # 电机反馈
ros2 run hightorque_robot motor_run            # 电机运行
ros2 run hightorque_robot motor_move_zero      # 电机回零
ros2 run hightorque_robot motor_set_zero       # 设置电机零位
ros2 run hightorque_robot canboard_update      # CAN 板固件更新
ros2 run hightorque_robot parse_demo           # 参数解析示例