Grasping-robotiq85xuchenhan.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "6465edcf",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-03-26T14:45:06.984247Z",
     "start_time": "2024-03-26T14:45:06.502377Z"
    }
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pydrake.all import (\n",
    "    AddMultibodyPlantSceneGraph,\n",
    "    DiagramBuilder,\n",
    "    MeshcatVisualizer,\n",
    "    Parser,\n",
    "    RigidTransform,\n",
    "    RotationMatrix,\n",
    "    StartMeshcat,\n",
    ")\n",
    "\n",
    "from manipulation.scenarios import SetColor"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "10c2ccae",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-03-26T14:45:07.897214Z",
     "start_time": "2024-03-26T14:45:07.889192Z"
    }
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "INFO:drake:Meshcat listening for connections at http://localhost:7003\n"
     ]
    }
   ],
   "source": [
    "# Start the visualizer.\n",
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "63fbc339",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-03-26T14:45:09.591921Z",
     "start_time": "2024-03-26T14:45:09.375541Z"
    }
   },
   "outputs": [
    {
     "ename": "RuntimeError",
     "evalue": "/home/dan/Projects/Pydrake-robotiq140/models/robotiq_85_gripper_xuchenhan-tri.urdf:10: error: URI 'package://movo_description/meshes/manipulation/robotiq/visual/kinova_robotiq_coupler.obj' refers to unknown package 'movo_description'",
     "output_type": "error",
     "traceback": [
      "\u001B[0;31m---------------------------------------------------------------------------\u001B[0m",
      "\u001B[0;31mRuntimeError\u001B[0m                              Traceback (most recent call last)",
      "Cell \u001B[0;32mIn[3], line 114\u001B[0m\n\u001B[1;32m    107\u001B[0m \u001B[38;5;250m    \u001B[39m\u001B[38;5;124;03m''''plant.GetJointByName(\"right_finger_sliding_joint\", pregrasp).set_translation(\u001B[39;00m\n\u001B[1;32m    108\u001B[0m \u001B[38;5;124;03m        plant_context, 0.054\u001B[39;00m\n\u001B[1;32m    109\u001B[0m \u001B[38;5;124;03m    )'''\u001B[39;00m\n\u001B[1;32m    111\u001B[0m     diagram\u001B[38;5;241m.\u001B[39mForcedPublish(context)\n\u001B[0;32m--> 114\u001B[0m \u001B[43mgrasp_poses_example\u001B[49m\u001B[43m(\u001B[49m\u001B[43m)\u001B[49m\n\u001B[1;32m    116\u001B[0m \u001B[38;5;66;03m### Example warning\u001B[39;00m\n\u001B[1;32m    117\u001B[0m \u001B[38;5;66;03m# WARNING:drake:/home/dan/Projects/Pydrake-robotiq140/catkin_ws/src/robotiq/robotiq_2f_140_gripper_visualization/urdf/robotiq_arg2f_140_model.urdf:264: warning: Joint 'left_inner_knuckle_joint' specifies a mimic element that will be ignored. Mimic elements are currently only supported by MultibodyPlant with a discrete time step and using DiscreteContactSolver::kSap.\u001B[39;00m\n\u001B[1;32m    118\u001B[0m \u001B[38;5;66;03m##https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html#a3296be9fa9bc35e6639460f54029c4c9\u001B[39;00m\n",
      "Cell \u001B[0;32mIn[3], line 33\u001B[0m, in \u001B[0;36mgrasp_poses_example\u001B[0;34m()\u001B[0m\n\u001B[1;32m      8\u001B[0m \u001B[38;5;250m\u001B[39m\u001B[38;5;124;03m'''\u001B[39;00m\n\u001B[1;32m      9\u001B[0m \u001B[38;5;124;03mgrasp = parser.AddModelsFromUrl(\u001B[39;00m\n\u001B[1;32m     10\u001B[0m \u001B[38;5;124;03m    #\"package://drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf\"\u001B[39;00m\n\u001B[0;32m   (...)\u001B[0m\n\u001B[1;32m     19\u001B[0m \u001B[38;5;124;03m)[0]\u001B[39;00m\n\u001B[1;32m     20\u001B[0m \u001B[38;5;124;03m'''\u001B[39;00m\n\u001B[1;32m     22\u001B[0m \u001B[38;5;250m\u001B[39m\u001B[38;5;124;03m'''grasp = parser.AddModelsFromUrl(\u001B[39;00m\n\u001B[1;32m     23\u001B[0m \u001B[38;5;124;03m    #\"package://drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf\"\u001B[39;00m\n\u001B[1;32m     24\u001B[0m \u001B[38;5;124;03m    \"package://drake/examples/simple_gripper/robotiq_85_gripper.urdf\"\u001B[39;00m\n\u001B[0;32m   (...)\u001B[0m\n\u001B[1;32m     30\u001B[0m \u001B[38;5;124;03m    #\"package://drake_models/atlas/robotiq_simple.urdf\"\u001B[39;00m\n\u001B[1;32m     31\u001B[0m \u001B[38;5;124;03m)[0]'''\u001B[39;00m\n\u001B[0;32m---> 33\u001B[0m grasp \u001B[38;5;241m=\u001B[39m \u001B[43mparser\u001B[49m\u001B[38;5;241;43m.\u001B[39;49m\u001B[43mAddModels\u001B[49m\u001B[43m(\u001B[49m\u001B[38;5;124;43m\"\u001B[39;49m\u001B[38;5;124;43m/home/dan/Projects/Pydrake-robotiq140/models/robotiq_85_gripper_xuchenhan-tri.urdf\u001B[39;49m\u001B[38;5;124;43m\"\u001B[39;49m\u001B[43m)\u001B[49m[\u001B[38;5;241m0\u001B[39m]\n\u001B[1;32m     34\u001B[0m pregrasp \u001B[38;5;241m=\u001B[39m parser\u001B[38;5;241m.\u001B[39mAddModels(\u001B[38;5;124m\"\u001B[39m\u001B[38;5;124m/home/dan/Projects/Pydrake-robotiq140/models/robotiq_85_gripper_xuchenhan-tri.urdf\u001B[39m\u001B[38;5;124m\"\u001B[39m)[\u001B[38;5;241m0\u001B[39m]\n\u001B[1;32m     35\u001B[0m brick \u001B[38;5;241m=\u001B[39m parser\u001B[38;5;241m.\u001B[39mAddModelsFromUrl(\n\u001B[1;32m     36\u001B[0m     \u001B[38;5;124m\"\u001B[39m\u001B[38;5;124mpackage://drake/examples/manipulation_station/models/061_foam_brick.sdf\u001B[39m\u001B[38;5;124m\"\u001B[39m\n\u001B[1;32m     37\u001B[0m )[\u001B[38;5;241m0\u001B[39m]\n",
      "\u001B[0;31mRuntimeError\u001B[0m: /home/dan/Projects/Pydrake-robotiq140/models/robotiq_85_gripper_xuchenhan-tri.urdf:10: error: URI 'package://movo_description/meshes/manipulation/robotiq/visual/kinova_robotiq_coupler.obj' refers to unknown package 'movo_description'"
     ]
    }
   ],
   "source": [
    "def grasp_poses_example():\n",
    "    builder = DiagramBuilder()\n",
    "\n",
    "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.1)\n",
    "    parser = Parser(plant, scene_graph)\n",
    "    parser.SetAutoRenaming(True)\n",
    "\n",
    "    '''\n",
    "    grasp = parser.AddModelsFromUrl(\n",
    "        #\"package://drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf\"\n",
    "        \"package://drake_models/atlas/robotiq.urdf\"\n",
    "        #\"package://drake_models/atlas/robotiq_simple.urdf\"\n",
    "    )[0]\n",
    "    pregrasp = parser.AddModelsFromUrl(\n",
    "        \"package://drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf\"\n",
    "    )[0]\n",
    "    brick = parser.AddModelsFromUrl(\n",
    "        \"package://drake/examples/manipulation_station/models/061_foam_brick.sdf\"\n",
    "    )[0]\n",
    "    '''\n",
    "    \n",
    "    '''grasp = parser.AddModelsFromUrl(\n",
    "        #\"package://drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf\"\n",
    "        \"package://drake/examples/simple_gripper/robotiq_85_gripper.urdf\"\n",
    "        #\"package://drake_models/atlas/robotiq_simple.urdf\"\n",
    "    )[0]\n",
    "    pregrasp = parser.AddModelsFromUrl(\n",
    "        #\"package://drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf\"\n",
    "        \"package://drake/examples/simple_gripper/robotiq_85_gripper.urdf\"\n",
    "        #\"package://drake_models/atlas/robotiq_simple.urdf\"\n",
    "    )[0]'''\n",
    "    \n",
    "    grasp = parser.AddModels(\"/home/dan/Projects/Pydrake-robotiq140/models/robotiq_85_gripper_xuchenhan-tri.urdf\")[0]\n",
    "    pregrasp = parser.AddModels(\"/home/dan/Projects/Pydrake-robotiq140/models/robotiq_85_gripper_xuchenhan-tri.urdf\")[0]\n",
    "    brick = parser.AddModelsFromUrl(\n",
    "        \"package://drake/examples/manipulation_station/models/061_foam_brick.sdf\"\n",
    "    )[0]\n",
    "    \n",
    "    #brick = parser.AddModels(\"/home/dan/Projects/Pydrake-robotiq140/models/010_potted_meat_can.sdf\")[0]\n",
    "    \n",
    "    \n",
    "    plant.Finalize()\n",
    "\n",
    "    B_O = plant.GetBodyByName(\"base_link\", brick)\n",
    "    B_Ggrasp = plant.GetBodyByName(\"robotiq_coupler_link\", grasp)\n",
    "    B_Gpregrasp = plant.GetBodyByName(\"robotiq_coupler_link\", pregrasp)\n",
    "\n",
    "    # Set the pregrasp to be green and slightly transparent.\n",
    "    inspector = scene_graph.model_inspector()\n",
    "    for body_index in plant.GetBodyIndices(pregrasp):\n",
    "        SetColor(\n",
    "            scene_graph,\n",
    "            [0, 0.6, 0, 0.5],\n",
    "            plant.get_source_id(),\n",
    "            inspector.GetGeometries(plant.GetBodyFrameIdOrThrow(body_index)),\n",
    "        )\n",
    "\n",
    "    meshcat.Delete()\n",
    "    meshcat.SetProperty(\"/Background\", \"visible\", False)\n",
    "    MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)\n",
    "\n",
    "    diagram = builder.Build()\n",
    "    context = diagram.CreateDefaultContext()\n",
    "    plant_context = plant.GetMyContextFromRoot(context)\n",
    "\n",
    "    # TODO(russt): Set a random pose of the object.\n",
    "\n",
    "    # Get the current object, O, pose\n",
    "    X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O)\n",
    "\n",
    "    p_GgraspO = [0, 0.11, 0]\n",
    "    R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply(\n",
    "        RotationMatrix.MakeZRotation(np.pi / 2.0)\n",
    "    )\n",
    "    X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)\n",
    "    X_OGgrasp = X_GgraspO.inverse()\n",
    "    X_WGgrasp = X_WO.multiply(X_OGgrasp)\n",
    "\n",
    "    # pregrasp is negative y in the gripper frame (see the figure!).\n",
    "    X_GgraspGpregrasp = RigidTransform([0, -0.08, 0])\n",
    "    X_WGpregrasp = X_WGgrasp @ X_GgraspGpregrasp\n",
    "\n",
    "    plant.SetFreeBodyPose(plant_context, B_Ggrasp, X_WGgrasp)\n",
    "    # Open the fingers, too.\n",
    "    \n",
    "    # The desired angle in radians to be stored in context.\n",
    "    # 10 degress = 0.174533 approx. 0.175\n",
    "    plant.GetJointByName(\"gripper_finger1_joint\", grasp).set_angle(\n",
    "        plant_context, 0.523599\n",
    "    )\n",
    "    \n",
    "    '''plant.GetJointByName(\"right_inner_finger_joint\", grasp).set_angle(\n",
    "        plant_context, 0.349066\n",
    "    )'''\n",
    "    \n",
    "    \n",
    "    \n",
    "    '''plant.GetJointByName(\"right_finger_sliding_joint\", grasp).set_translation(\n",
    "        plant_context, 0.054\n",
    "    )'''\n",
    "\n",
    "    plant.SetFreeBodyPose(plant_context, B_Gpregrasp, X_WGpregrasp)\n",
    "    # Open the fingers, too.\n",
    "    '''plant.GetJointByName(\"right_inner_finger_joint\", pregrasp).set_translation(\n",
    "        plant_context, -0.054\n",
    "    )'''\n",
    "    ''''plant.GetJointByName(\"right_finger_sliding_joint\", pregrasp).set_translation(\n",
    "        plant_context, 0.054\n",
    "    )'''\n",
    "\n",
    "    diagram.ForcedPublish(context)\n",
    "\n",
    "\n",
    "grasp_poses_example()\n",
    "\n",
    "### Example warning\n",
    "# WARNING:drake:/home/dan/Projects/Pydrake-robotiq140/catkin_ws/src/robotiq/robotiq_2f_140_gripper_visualization/urdf/robotiq_arg2f_140_model.urdf:264: warning: Joint 'left_inner_knuckle_joint' specifies a mimic element that will be ignored. Mimic elements are currently only supported by MultibodyPlant with a discrete time step and using DiscreteContactSolver::kSap.\n",
    "##https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html#a3296be9fa9bc35e6639460f54029c4c9"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5bde8be9-17df-4ba6-a66c-86cf7547e691",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python (Pydrake-robotiq140)",
   "language": "python",
   "name": "pydrake-robotiq140"
  },
  "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.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}