# 设置代理
export PROXY="http://192.168.31.39:7890"
export http_proxy="$PROXY"
export https_proxy="$PROXY"
export ftp_proxy="$PROXY"
export socks_proxy="$PROXY"

PATH=/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin

# 设置虚拟can接口
sudo ip link add dev can1 type vcan
sudo ip link set up can1
sudo ip link set can1 type vcan bitrate 1000000

# 设置can波特率
ip link set can1 type can bitrate 1000000

# 启动can
ip link set can0 up

# 设置slcan
sudo slcand -o -c -s8 -S1000000 /dev/ttyACM0 can0
sudo slcand -o -c -s8 -S1000000  /dev/serial/by-id/usb-1a86_USB_Serial-if00-port0 can1

# 查看can信息
ip -details -statistics link show can1
###################

# 绑定接口名
sudo vim /etc/udev/rules.d/72-gs-usb-can.rules

SUBSYSTEM=="net", ACTION=="add", ATTRS{serial}=="004F004F3630501120353355", NAME="can6"
SUBSYSTEM=="net", ACTION=="add", ATTRS{serial}=="004C004F4D4D501020383834", NAME="can7"
SUBSYSTEM=="net", ACTION=="add", ATTRS{serial}=="0061005A3945501620303651", NAME="can4"
SUBSYSTEM=="net", ACTION=="add", ATTRS{serial}=="006500573945501620303651", NAME="can5"

sudo udevadm control --reload-rules
sudo udevadm trigger
#######################

# 安装ros依赖
rosdepc install --from-paths src --ignore-src --rosdistro=humble -y    

# 启动单臂
ros2 launch openarm_bringup openarm.launch.py arm_type:=v10 use_fake_hardware:=false can_interface:=can6

# 设置单臂运动
ros2 action send_goal /joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory '{trajectory: {joint_names: ["openarm_joint1", "openarm_joint2", "openarm_joint3", "openarm_joint4", "openarm_joint5", "openarm_joint6", "openarm_joint7"], points: [{positions: [0.15, 0.15, 0.15, 0.15, 0.15, 0.15, 0.15], time_from_start: {sec: 1, nanosec: 0}}]}}'

ros2 action send_goal /joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory '{trajectory: {joint_names: ["openarm_joint1", "openarm_joint2", "openarm_joint3", "openarm_joint4", "openarm_joint5", "openarm_joint6", "openarm_joint7"], points: [{positions: [0, 0, 0, 0, 0, 0, 0], time_from_start: {sec: 1, nanosec: 0}}]}}'

# 设置右臂运动
ros2 action send_goal /right_joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory '{trajectory: {joint_names: ["openarm_right_joint1", "openarm_right_joint2", "openarm_right_joint3", "openarm_right_joint4", "openarm_right_joint5", "openarm_right_joint6", "openarm_right_joint7"], points: [{positions: [0.15, 0.15, 0.15, 0.15, 0.15, 0.15, 0.15], time_from_start: {sec: 1, nanosec: 0}}]}}'

ros2 action send_goal /right_joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory '{trajectory: {joint_names: ["openarm_right_joint1", "openarm_right_joint2", "openarm_right_joint3", "openarm_right_joint4", "openarm_right_joint5", "openarm_right_joint6", "openarm_right_joint7"], points: [{positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], time_from_start: {sec: 1, nanosec: 0}}]}}'

# 启动双臂
ros2 launch openarm_bringup openarm.bimanual.launch.py arm_type:=v10 use_fake_hardware:=false gravity_compensation_enabled:=false gravity_compensation_use_kdl:=false gravity_torque_mode:=false enable_joint_state_udp_broadcast:=true joint_state_udp_payload_format:=json

# 启动双臂位置控制器
ros2 launch openarm_bringup openarm.bimanual.launch.py arm_type:=v10 use_fake_hardware:=false gravity_compensation_enabled:=false gravity_compensation_use_kdl:=false gravity_torque_mode:=false robot_controller:=forward_position_controller enable_joint_state_udp_broadcast:=true joint_state_udp_payload_format:=json

# 启动双臂重力补偿
ros2 launch openarm_bringup openarm.bimanual.launch.py arm_type:=v10 use_fake_hardware:=false gravity_compensation_enabled:=true gravity_compensation_use_kdl:=true gravity_torque_mode:=true gripper_gravity_compensation_enabled:=true enable_joint_state_udp_broadcast:=true joint_state_udp_payload_format:=json

# 启动moveit
ros2 launch openarm_bimanual_moveit_config demo.launch.py use_fake_hardware:=false
ros2 launch openarm_bimanual_moveit_config demo.launch.py use_fake_hardware:=false robot_controller:=forward_velocity_controller

# 启动动作录制
# 启动节点
ros2 launch openarm_joint_recorder joint_trajectory_recorder.launch.py

# 开始录制
ros2 service call /joint_trajectory_recorder/set_recording std_srvs/srv/SetBool "{data: true}"

# 停止录制
ros2 service call /joint_trajectory_recorder/set_recording std_srvs/srv/SetBool "{data: false}"

# 回放
ros2 service call /joint_trajectory_recorder/play std_srvs/srv/Trigger "{}"

# 保存轨迹
ros2 service call /joint_trajectory_recorder/save std_srvs/srv/Trigger "{}"

# 加载轨迹
ros2 service call /joint_trajectory_recorder/load std_srvs/srv/Trigger "{}"

# 标定
ros2 run camera_calibration cameracalibrator --size 9x6 --square 0.255 \
  left:=/camera/left/image_raw right:=/camera/right/image_raw \
  left_camera:=camera_frame_left right_camera:=camera_frame_right

# 下载数据集
hf download --repo-type dataset --token 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 pixparse/cc3m-wds --local-dir /home/D/ --resume-download

export HF_ENDPOINT=https://hf-mirror.com

# huggingface token
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

# 设置can
lerobot-setup-can --mode=setup --interfaces=can0,can2 --use_fd=false
lerobot-setup-can --mode=test --interfaces=can0,can2 --use_fd=false

# 测试相机
lerobot-find-cameras opencv

# 启动单臂
lerobot-teleoperate \
    --robot.type=openarm_follower \
    --robot.port=can0 \
    --robot.side=right \
    --robot.cameras="{ front: {type: opencv, index_or_path: 0, width: 640, height: 480, fps: 30, fourcc: "MJPG"}}" \
    --robot.id=my_follower --robot.use_can_fd=false \
    --teleop.type=openarm_leader \
    --teleop.port=can2 \
    --teleop.id=my_leader --teleop.use_can_fd=false \
    --display_data=true
    
# 录制单臂 
lerobot-record \
    --robot.type=openarm_follower \
    --robot.port=can2 \
    --robot.side=right \
    --robot.cameras="{ front: {type: opencv, index_or_path: 0, width: 640, height: 480, fps: 30, fourcc: "MJPG"}}" \
    --robot.id=my_follower --robot.use_can_fd=false \
    --teleop.type=openarm_leader \
    --teleop.port=can0 \
    --teleop.id=my_leader --teleop.use_can_fd=false \
    --display_data=true \
    --dataset.repo_id=shenchenayng/lerobot_openarm_test \
    --dataset.num_episodes=40 \
    --dataset.single_task="Grab Oranges" \
    --dataset.push_to_hub=false \
    --dataset.episode_time_s=10 \
    --dataset.reset_time_s=2
    
# 上传数据集
hf upload shenchenayng/lerobot_openarm_test . --repo-type=dataset

# wandb token
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

# 训练
lerobot-train \
  --dataset.repo_id=shenchenayng/lerobot_openarm_test \
  --dataset.root=/home/shen/.cache/huggingface/lerobot/shenchenayng/lerobot_openarm_test \
  --dataset.revision=v0.1.0 \
  --dataset.streaming=false \
  --policy.type=act \
  --output_dir=~/lerobot_openarm_modle \
  --job_name=lerobot_openarm_test \
  --policy.device=cuda \
  --wandb.enable=true \
  --wandb.project=lerobot_openarm_test \
  --policy.push_to_hub=false \
  --steps=2000 \
  --batch_size=8 \
  --save_freq=100
  
lerobot-train --dataset.repo_id=shenchenayng/lerobot_openarm_test --dataset.root=/home/featurize/lerobot_openarm_test --dataset.revision=v0.1.0 --dataset.streaming=false --policy.type=act --output_dir=/home/featurize/lerobot_openarm_modle/a --job_name=lerobot_openarm_test --policy.device=cuda --wandb.enable=true --wandb.project=lerobot_openarm_test --policy.push_to_hub=false --steps=2000   --batch_size=8 --save_freq=100


lerobot-record  \
  --robot.type=openarm_follower \
  --robot.port=can0 \
  --robot.side=right \
  --robot.use_can_fd=false \
  --robot.cameras="{ front: {type: opencv, index_or_path: 0, width: 640, height: 480, fps: 30, fourcc: "MJPG"}}" \
  --robot.id=my_follower \
  --display_data=false \
  --dataset.repo_id=shenchenayng/eval_lerobot_openarm_test \
  --dataset.single_task="Grab Oranges" \
  --dataset.episode_time_s=1000 \
  --dataset.push_to_hub=false \
  --policy.path=/home/shen/lerobot_openarm_modle/202603260100_model/202603260100/pretrained_model