Arm_test.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "import RobotControl_func_ros1_chong\n",
    "import rospy\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "rospy.init_node(\"RobotTEST\",anonymous=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = RobotControl_func_ros1_chong.RobotControl_Func()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[546.3766710335036,\n",
       " -100.4645351208886,\n",
       " 405.38592529296875,\n",
       " 179.99990844726562,\n",
       " -7.330447988351807e-05,\n",
       " 45.00006484985351]"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.get_TMPos()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'self' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[10], line 4\u001b[0m\n\u001b[1;32m      1\u001b[0m RC2G \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39mload(\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mCamera2Gripper_chong.npy\u001b[39m\u001b[38;5;124m'\u001b[39m)[\u001b[38;5;241m0\u001b[39m:\u001b[38;5;241m3\u001b[39m,\u001b[38;5;241m0\u001b[39m:\u001b[38;5;241m3\u001b[39m]\n\u001b[1;32m      2\u001b[0m TC2G \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39mload(\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mCamera2Gripper_chong.npy\u001b[39m\u001b[38;5;124m'\u001b[39m)[:\u001b[38;5;241m3\u001b[39m,\u001b[38;5;241m3\u001b[39m]\n\u001b[0;32m----> 4\u001b[0m d \u001b[38;5;241m=\u001b[39m \u001b[38;5;28mfloat\u001b[39m(\u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241m.\u001b[39mdepth_value) \n\u001b[1;32m      5\u001b[0m \u001b[38;5;28mprint\u001b[39m(d)\n\u001b[1;32m      6\u001b[0m uv \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39marray([realX \u001b[38;5;241m*\u001b[39m d, realY \u001b[38;5;241m*\u001b[39m d, d])\n",
      "\u001b[0;31mNameError\u001b[0m: name 'self' is not defined"
     ]
    }
   ],
   "source": [
    "RC2G = np.load('Camera2Gripper_chong.npy')[0:3,0:3]\n",
    "TC2G = np.load('Camera2Gripper_chong.npy')[:3,3]\n",
    "\n",
    "d = float(480) \n",
    "print(d)\n",
    "uv = np.array([320 * d, realY * d, d])\n",
    "xyz = np.linalg.inv(intrinsic) @ uv\n",
    "XYZ = RC2G @ xyz   TC2G.flatten()\n",
    "print(xyz)\n",
    "print(XYZ)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "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.8.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}