Broken code

.gitignore

@@ -25,6 +25,7 @@ share/python-wheels/
 .installed.cfg
 *.egg
 MANIFEST
+environment.yml
 
 # PyInstaller
 #  Usually these files are written by a python script from a template

Untitled.ipynb

@@ -0,0 +1,211 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 1,
+   "id": "be766ec9-1f94-49fb-a583-06f443bdf9db",
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stderr",
+     "output_type": "stream",
+     "text": [
+      "pybullet build time: May 17 2025 21:04:57\n"
+     ]
+    }
+   ],
+   "source": [
+    "from inverse_geometry import computeqgrasppose"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 2,
+   "id": "68227652-0c9e-4af6-9d0b-87e217e03f0b",
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "Wrapper tries to connect to server <tcp://127.0.0.1:6000>\n",
+      "You can open the visualizer by visiting the following URL:\n",
+      "http://127.0.0.1:7001/static/\n"
+     ]
+    }
+   ],
+   "source": [
+    "from tools import setupwithmeshcat\n",
+    "robot, cube, viz = setupwithmeshcat(url=\"tcp://127.0.0.1:6000\")"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 3,
+   "id": "7e829a6a-1b19-4fcf-8f6e-68e944f5c495",
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "tcp://127.0.0.1:6000\n"
+     ]
+    }
+   ],
+   "source": [
+    "from config import MESHCAT_URL\n",
+    "print(MESHCAT_URL)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 4,
+   "id": "948cc2a3-c520-442e-be4b-99fa75569f09",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/html": [
+       "\n",
+       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
+       "            <iframe src=\"http://127.0.0.1:7001/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
+       "            </div>\n",
+       "            "
+      ],
+      "text/plain": [
+       "<IPython.core.display.HTML object>"
+      ]
+     },
+     "execution_count": 4,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "hasattr(viz.viewer, 'jupyter_cell') and viz.viewer.jupyter_cell()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 5,
+   "id": "6683d3cc-1a3a-4ad8-a655-edd273432293",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from config import CUBE_PLACEMENT\n",
+    "cubetarget = CUBE_PLACEMENT\n",
+    "qcurrent = robot.q0"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 6,
+   "id": "ae8201e6-2be1-414d-8f39-7e72e0c695a2",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "(array([-3.01785725e-01,  1.33283923e-15, -2.85817151e-16, -9.28742483e-01,\n",
+       "        -3.29665591e-02, -2.05698204e-01,  4.47493515e-18,  2.38664763e-01,\n",
+       "        -3.40271791e-01, -2.63214437e-01, -2.64229802e-01,  5.15769935e-02,\n",
+       "         8.84695564e-18,  2.12652808e-01,  2.13738547e+00]),\n",
+       " True)"
+      ]
+     },
+     "execution_count": 6,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "computeqgrasppose(robot, qcurrent, cube, cubetarget, viz)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 8,
+   "id": "4d05cbce-2ece-4adc-8d70-b434a72a806c",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "True"
+      ]
+     },
+     "execution_count": 8,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "from tools import collision\n",
+    "\n",
+    "collision(robot, robot.q0)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 18,
+   "id": "c3e5bc01-7f2c-42f9-b768-9487d24408d0",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import pinocchio as pin"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 19,
+   "id": "f256b348-1a37-4594-b018-0199beb2f807",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "SE3(array([[-9.99998732e-01,  1.59265292e-03,  8.26883241e-18,  3.99483973e-01],[-1.59265292e-03, -9.99998732e-01,  1.35248866e-17,  5.97082849e-02],[ 4.34329287e-17,  3.45763012e-17,  1.00000000e+00,  9.29934278e-01],[ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]]))"
+      ]
+     },
+     "execution_count": 19,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "pin.computeJointJacobians(robot.model,robot.data,robot.q0)\n",
+    "robot.data.oMf[-1]"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "58eaf60e-ccb0-42c3-ae34-b5eabde6f37d",
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3 (ipykernel)",
+   "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.13.7"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}

bezier.py

@@ -7,7 +7,7 @@ def __init__(self, pointlist, t_min=0.0, t_max=1.0, mult_t=1.0):
         self.T_min_ = t_min
         self.T_max_ = t_max
         self.mult_T_ = mult_t
-        self.size_ = len(pointlist)- 1
+        self.size_ = len(pointlist)
         self.degree_ = self.size_ - 1
         self.control_points_ = pointlist    
         if (self.size_ < 1 or self.T_max_ <= self.T_min_):

config.py

@@ -28,7 +28,7 @@
 LEFT_HOOK = "LARM_HOOK"
 RIGHT_HOOK = "RARM_HOOK"
 
-    
+
 #scene placements
 ROBOT_PLACEMENT= pin.XYZQUATToSE3(np.array([0.,0.,0.85,0.,0.,0.,1.]))
 TABLE_PLACEMENT= pin.SE3(rotate('z',-np.pi/2),np.array([0.8,0.,0.]))
@@ -37,13 +37,13 @@
 CUBE_PLACEMENT_TARGET= pin.SE3(rotate('z', 0),np.array([0.4, 0.11, 0.93]))
 
 #do not edit this part unless you know what you are doing
-MODELS_PATH = join(dirname(str(abspath(__file__))), "models") 
-MESH_DIR = MODELS_PATH 
+MODELS_PATH = join(dirname(str(abspath(__file__))), "models")
+MESH_DIR = MODELS_PATH
 NEXTAGE_URDF_PATH = MODELS_PATH + '/nextagea_description/urdf/'
 NEXTAGE_URDF = NEXTAGE_URDF_PATH + 'NextageaOpen.urdf'
 NEXTAGE_SRDF = NEXTAGE_URDF_PATH + 'NextageAOpen.srdf'
 TABLE_URDF = MODELS_PATH + '/table/table_tallerscaled.urdf'
-TABLE_MESH = MODELS_PATH +  "/table/" 
+TABLE_MESH = MODELS_PATH +  "/table/"
 OBSTACLE_URDF = MODELS_PATH + '/cubes/obstacle.urdf'
 OBSTACLE_MESH = MODELS_PATH + '/cubes/'
 CUBE_URDF = MODELS_PATH + '/cubes/cube_small.urdf'

constants.py

@@ -0,0 +1,59 @@
+"""Project-wide numeric constants and defaults.
+
+Place all tunable "magic numbers" here so they are easy to discover and tweak.
+"""
+from typing import Tuple
+
+# Sampling (table / cube placement)
+DEFAULT_TABLE_Z_RANGE: Tuple[float, float] = (0.93, 2)
+DEFAULT_CHECK_COLLISIONS: bool = True
+
+# RRT / planning
+DEFAULT_NUM_ITER: int = 500
+DEFAULT_DISCRETISATION_STEPS: int = 100
+DEFAULT_CHECK_EDGE_STEPS: int = 1000
+EDGE_DISTANCE_TOL: float = 1e-3
+
+# Visualization / interactive refresh defaults
+DEFAULT_VIZ_DELAY: float = 0.01  # seconds to sleep after updating viz
+SAMPLE_DISPLAY_EVERY: int = 1    # display every N samples when visualizing sampling
+# Fraction of the table extents to use when creating a restricted sampling region
+DEFAULT_SAMPLER_SHRINK: float = 0.3
+# When sampling on the vertical plane, allow a small half-width (meters) perpendicular to the plane
+DEFAULT_PLANE_HALF_WIDTH: float = 0.3
+
+# Planner-specific defaults (tunable)
+DEFAULT_REPAIR_ATTEMPTS: int = 5
+DEFAULT_GOAL_BIAS: float = 0.03
+DEFAULT_MAX_IK_ATTEMPTS: int = 3
+DEFAULT_MAX_TIME_S: float = 300
+
+MIN_DIST_TO_OBS: float = 0.02
+
+# Preset bundles for convenience: 'fast', 'default', 'robust'
+PRESETS = {
+	'fast': {
+		'num_iter': 200,
+		'discretisation': 8,
+		'repair_attempts': 3,
+		'goal_bias': 0.02,
+		'max_ik_attempts': 3,
+		'max_time_s': 15.0,
+	},
+	'default': {
+		'num_iter': 500,
+		'discretisation': 8,
+		'repair_attempts': 5,
+		'goal_bias': 0.03,
+		'max_ik_attempts': 3,
+		'max_time_s': 30.0,
+	},
+	'robust': {
+		'num_iter': 1000,
+		'discretisation': 10,
+		'repair_attempts': 8,
+		'goal_bias': 0.05,
+		'max_ik_attempts': 5,
+		'max_time_s': 60.0,
+	}
+}