{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "导入机械臂连接模块\n"
     ]
    }
   ],
   "source": [
    "from utils_robot import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'192.168.0.68'"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "controllers[0].ip_address"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.start_sys()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "for i in range(1, 16):\n",
    "    robot.set_voice(i, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.teach_mode()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.end_teach_mode()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(1.4062768872941616,\n",
       " -1.7172914920379754,\n",
       " -1.565427394037297,\n",
       " -1.4375317458473316,\n",
       " 1.4158642672184467,\n",
       " -6.025572408614042)"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.get_actual_joint_positions()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{'rx': 3.116116,'ry': 0.076297,'rz': 0.141872,'x': 0.074583,'y': 0.282467,'z': 0.227661}"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.get_actual_tcp_pose()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "移动至俯视姿态\n"
     ]
    }
   ],
   "source": [
    "move_to_top_view()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "388"
      ]
     },
     "execution_count": 23,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "joint_pose = [1.54927, -1.54927, -1.54927, -1.54927, 1.54927, 0] # 目标位姿关节数据\n",
    "a = 0.5 # 关节加速度 (rad/s2)\n",
    "v = 0.2 # 关节速度 (rad/s)\n",
    "t = 0   # 运动时间 (s)。 当 t > 0 时，参数速度 v 和加速度 a 无效\n",
    "r = 0   # 交融半径 (m)。用于指定路径的平滑效果\n",
    "robot.movej(joint_pose, a, v, t, r) # 关节运动 https://help.lebai.ltd/sdk/motion.html#%E5%85%B3%E8%8A%82%E8%BF%90%E5%8A%A8"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cartesian_pose = {'x' : -0.383, 'y' : -0.121, 'z' : 0.36, 'rz' : -1.57, 'ry' : 0, 'rx' : 1.57} # 目标位姿笛卡尔数据\n",
    "robot.movel(cartesian_pose, a, v, t, r) # 直线运动 https://help.lebai.ltd/sdk/motion.html#%E7%9B%B4%E7%BA%BF%E8%BF%90%E5%8A%A8\n",
    "robot.wait_move() # 等待运动完成"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 设置夹爪参数\n",
    "# force  力度(0-100)\n",
    "# amplitude  张合幅度(0-100)\n",
    "robot.set_claw(50, 100)\n",
    "time.sleep(2)\n",
    "robot.set_claw(20, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.set_claw(20, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "    移动至俯视姿态\n",
      "移动至俯视姿态\n",
      "    保存至temp/vl_now.jpg\n"
     ]
    }
   ],
   "source": [
    "top_view_shot(check=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "    夹爪移动至物体上方\n",
      "    夹爪向下抓取物体\n",
      "    升起物体\n",
      "    搬运物体至目标上方\n",
      "    向下放下物体\n",
      "    移动至俯视姿态\n",
      "移动至俯视姿态\n"
     ]
    }
   ],
   "source": [
    "gripper_move()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "    夹爪移动至物体上方\n",
      "    夹爪向下抓取物体\n",
      "    升起物体\n",
      "    搬运物体至目标上方\n",
      "    向下放下物体\n",
      "    移动至俯视姿态\n",
      "移动至俯视姿态\n"
     ]
    }
   ],
   "source": [
    "gripper_move(XY_START=[0.065, 0.300], XY_END=[0.2, 0.2])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 指定点在图像中的像素坐标\n",
    "X_im, Y_im = 460, 370\n",
    "X_mc, Y_mc = eye2hand(X_im, Y_im)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.027669678674176357 0.3151503787878788\n"
     ]
    }
   ],
   "source": [
    "print(X_mc, Y_mc)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "    夹爪移动至物体上方\n",
      "    夹爪向下抓取物体\n",
      "    升起物体\n",
      "    搬运物体至目标上方\n",
      "    向下放下物体\n",
      "    移动至俯视姿态\n",
      "移动至俯视姿态\n"
     ]
    }
   ],
   "source": [
    "gripper_move(XY_START=(X_mc, Y_mc), XY_END=[0.2, 0.2])"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "vlm_agent",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}