From dd74632ef1d704c9ff3e8c72fd88ae4ccb0c9efe Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Thu, 9 Oct 2025 13:27:25 +0100 Subject: [PATCH 01/23] Broken code --- Untitled.ipynb | 177 ++++++++++++++++++++++++++++++++++++++++++++ inverse_geometry.py | 54 +++++++++++++- 2 files changed, 228 insertions(+), 3 deletions(-) create mode 100644 Untitled.ipynb diff --git a/Untitled.ipynb b/Untitled.ipynb new file mode 100644 index 0000000..c1fd765 --- /dev/null +++ b/Untitled.ipynb @@ -0,0 +1,177 @@ +{ + "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 \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", + "
\n", + " \n", + "
\n", + " " + ], + "text/plain": [ + "" + ] + }, + "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_TARGET\n", + "cubetarget = CUBE_PLACEMENT_TARGET\n", + "qcurrent = robot.q0" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "ae8201e6-2be1-414d-8f39-7e72e0c695a2", + "metadata": {}, + "outputs": [ + { + "ename": "ValueError", + "evalue": "matmul: Input operand 1 has a mismatch in its core dimension 0, with gufunc signature (n?,k),(k,m?)->(n?,m?) (size 15 is different from 6)", + "output_type": "error", + "traceback": [ + "\u001b[31m---------------------------------------------------------------------------\u001b[39m", + "\u001b[31mValueError\u001b[39m Traceback (most recent call last)", + "\u001b[36mCell\u001b[39m\u001b[36m \u001b[39m\u001b[32mIn[6]\u001b[39m\u001b[32m, line 1\u001b[39m\n\u001b[32m----> \u001b[39m\u001b[32m1\u001b[39m q, success = \u001b[43mcomputeqgrasppose\u001b[49m\u001b[43m(\u001b[49m\u001b[43mrobot\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mqcurrent\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mcube\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mcubetarget\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mviz\u001b[49m\u001b[43m)\u001b[49m\n", + "\u001b[36mFile \u001b[39m\u001b[32m~/Code/ARO/lab/inverse_geometry.py:56\u001b[39m, in \u001b[36mcomputeqgrasppose\u001b[39m\u001b[34m(robot, qcurrent, cube, cubetarget, viz)\u001b[39m\n\u001b[32m 54\u001b[39m vq = pinv(lhand_Jlhand)\u001b[38;5;129m@lhand_nu\u001b[39m\n\u001b[32m 55\u001b[39m Prhand = np.eye(robot.nv)-pinv(o_Jrhand) @ o_Jrhand\n\u001b[32m---> \u001b[39m\u001b[32m56\u001b[39m vq += pinv(rhand_Jrhand @ Prhand) @ (\u001b[43m(\u001b[49m\u001b[43mo_Jlhand\u001b[49m\u001b[43m \u001b[49m\u001b[43m-\u001b[49m\u001b[43m \u001b[49m\u001b[43mo_Jrhand\u001b[49m\u001b[43m)\u001b[49m\u001b[43m.\u001b[49m\u001b[43mT\u001b[49m\u001b[43m \u001b[49m\u001b[43m@\u001b[49m\u001b[43m \u001b[49m\u001b[43mvq\u001b[49m)\n\u001b[32m 58\u001b[39m q = pin.integrate(robot.model,q, vq)\n\u001b[32m 60\u001b[39m lerror = norm(oMlhand.translation - oMlhook.translation)\n", + "\u001b[31mValueError\u001b[39m: matmul: Input operand 1 has a mismatch in its core dimension 0, with gufunc signature (n?,k),(k,m?)->(n?,m?) (size 15 is different from 6)" + ] + } + ], + "source": [ + "q, success = computeqgrasppose(robot, qcurrent, cube, cubetarget, viz)" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "id": "4d05cbce-2ece-4adc-8d70-b434a72a806c", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "True" + ] + }, + "execution_count": 10, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "from tools import collision\n", + "\n", + "collision(robot, robot.q0)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c3e5bc01-7f2c-42f9-b768-9487d24408d0", + "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 +} diff --git a/inverse_geometry.py b/inverse_geometry.py index 8f03044..e77e09b 100644 --- a/inverse_geometry.py +++ b/inverse_geometry.py @@ -9,6 +9,7 @@ import pinocchio as pin import numpy as np from numpy.linalg import pinv,inv,norm,svd,eig +import time from tools import collision, getcubeplacement, setcubeplacement, projecttojointlimits from config import LEFT_HOOK, RIGHT_HOOK, LEFT_HAND, RIGHT_HAND, EPSILON from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET @@ -18,9 +19,56 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None): '''Return a collision free configuration grasping a cube at a specific location and a success flag''' setcubeplacement(robot, cube, cubetarget) - #TODO implement - print ("TODO: implement me") - return robot.q0, False + + q = qcurrent.copy() + + lhand_id = robot.model.getFrameId(LEFT_HAND) + rhand_id = robot.model.getFrameId(RIGHT_HAND) + + oMlhook = getcubeplacement(cube, LEFT_HOOK) + oMrhook = getcubeplacement(cube, RIGHT_HOOK) + + success = False + + for i in range(1000): + pin.framesForwardKinematics(robot.model,robot.data,q) + pin.computeJointJacobians(robot.model,robot.data,q) + + oMlhand = robot.data.oMf[lhand_id] + oMrhand = robot.data.oMf[rhand_id] + + lhandMlhook = oMlhand.inverse()*oMlhook + lhand_nu = pin.log(lhandMlhook).vector + + rhandMrhook = oMrhand.inverse()*oMrhook + rhand_nu = pin.log(rhandMrhook).vector + + oRlhand = oMlhand.rotation + lhand_Jlhand = pin.computeFrameJacobian(robot.model,robot.data,q,lhand_id,pin.LOCAL_WORLD_ALIGNED) + o_Jlhand = pin.computeFrameJacobian(robot.model,robot.data,q,lhand_id, pin.WORLD) + + oRrhand = oMrhand.rotation + rhand_Jrhand = pin.computeFrameJacobian(robot.model,robot.data,q,rhand_id,pin.LOCAL_WORLD_ALIGNED) + o_Jrhand = pin.computeFrameJacobian(robot.model,robot.data,q,rhand_id, pin.WORLD) + + vq = pinv(lhand_Jlhand)@lhand_nu + Prhand = np.eye(robot.nv)-pinv(o_Jrhand) @ o_Jrhand + vq += pinv(rhand_Jrhand @ Prhand) @ (o_Jlhand - o_Jrhand @ vq) + + q = pin.integrate(robot.model,q, vq) + + lerror = norm(oMlhand.translation - oMlhook.translation) + rerror = norm(oMrhand.translation - oMrhook.translation) + + if viz: + viz.display(q) + time.sleep(1e-3) + + if lerror < EPSILON and not collision(robot, q): + success = True + break + + return q, success if __name__ == "__main__": from tools import setupwithmeshcat From cba45e3a665291066a704b4ad3c3c9d8eff3b065 Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Fri, 10 Oct 2025 22:02:43 +0100 Subject: [PATCH 02/23] Part 1 - Geometry solved --- Untitled.ipynb | 66 ++++++++++++++++++++++++++++++++++----------- inverse_geometry.py | 35 ++++++++++++------------ 2 files changed, 67 insertions(+), 34 deletions(-) diff --git a/Untitled.ipynb b/Untitled.ipynb index c1fd765..058bf42 100644 --- a/Untitled.ipynb +++ b/Untitled.ipynb @@ -93,8 +93,8 @@ "metadata": {}, "outputs": [], "source": [ - "from config import CUBE_PLACEMENT_TARGET\n", - "cubetarget = CUBE_PLACEMENT_TARGET\n", + "from config import CUBE_PLACEMENT\n", + "cubetarget = CUBE_PLACEMENT\n", "qcurrent = robot.q0" ] }, @@ -105,25 +105,27 @@ "metadata": {}, "outputs": [ { - "ename": "ValueError", - "evalue": "matmul: Input operand 1 has a mismatch in its core dimension 0, with gufunc signature (n?,k),(k,m?)->(n?,m?) (size 15 is different from 6)", - "output_type": "error", - "traceback": [ - "\u001b[31m---------------------------------------------------------------------------\u001b[39m", - "\u001b[31mValueError\u001b[39m Traceback (most recent call last)", - "\u001b[36mCell\u001b[39m\u001b[36m \u001b[39m\u001b[32mIn[6]\u001b[39m\u001b[32m, line 1\u001b[39m\n\u001b[32m----> \u001b[39m\u001b[32m1\u001b[39m q, success = \u001b[43mcomputeqgrasppose\u001b[49m\u001b[43m(\u001b[49m\u001b[43mrobot\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mqcurrent\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mcube\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mcubetarget\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mviz\u001b[49m\u001b[43m)\u001b[49m\n", - "\u001b[36mFile \u001b[39m\u001b[32m~/Code/ARO/lab/inverse_geometry.py:56\u001b[39m, in \u001b[36mcomputeqgrasppose\u001b[39m\u001b[34m(robot, qcurrent, cube, cubetarget, viz)\u001b[39m\n\u001b[32m 54\u001b[39m vq = pinv(lhand_Jlhand)\u001b[38;5;129m@lhand_nu\u001b[39m\n\u001b[32m 55\u001b[39m Prhand = np.eye(robot.nv)-pinv(o_Jrhand) @ o_Jrhand\n\u001b[32m---> \u001b[39m\u001b[32m56\u001b[39m vq += pinv(rhand_Jrhand @ Prhand) @ (\u001b[43m(\u001b[49m\u001b[43mo_Jlhand\u001b[49m\u001b[43m \u001b[49m\u001b[43m-\u001b[49m\u001b[43m \u001b[49m\u001b[43mo_Jrhand\u001b[49m\u001b[43m)\u001b[49m\u001b[43m.\u001b[49m\u001b[43mT\u001b[49m\u001b[43m \u001b[49m\u001b[43m@\u001b[49m\u001b[43m \u001b[49m\u001b[43mvq\u001b[49m)\n\u001b[32m 58\u001b[39m q = pin.integrate(robot.model,q, vq)\n\u001b[32m 60\u001b[39m lerror = norm(oMlhand.translation - oMlhook.translation)\n", - "\u001b[31mValueError\u001b[39m: matmul: Input operand 1 has a mismatch in its core dimension 0, with gufunc signature (n?,k),(k,m?)->(n?,m?) (size 15 is different from 6)" - ] + "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": [ - "q, success = computeqgrasppose(robot, qcurrent, cube, cubetarget, viz)" + "computeqgrasppose(robot, qcurrent, cube, cubetarget, viz)" ] }, { "cell_type": "code", - "execution_count": 10, + "execution_count": 8, "id": "4d05cbce-2ece-4adc-8d70-b434a72a806c", "metadata": {}, "outputs": [ @@ -133,7 +135,7 @@ "True" ] }, - "execution_count": 10, + "execution_count": 8, "metadata": {}, "output_type": "execute_result" } @@ -146,10 +148,42 @@ }, { "cell_type": "code", - "execution_count": null, + "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": [] } ], diff --git a/inverse_geometry.py b/inverse_geometry.py index e77e09b..c6bc465 100644 --- a/inverse_geometry.py +++ b/inverse_geometry.py @@ -18,8 +18,6 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None): '''Return a collision free configuration grasping a cube at a specific location and a success flag''' - setcubeplacement(robot, cube, cubetarget) - q = qcurrent.copy() lhand_id = robot.model.getFrameId(LEFT_HAND) @@ -31,29 +29,30 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None): success = False for i in range(1000): - pin.framesForwardKinematics(robot.model,robot.data,q) - pin.computeJointJacobians(robot.model,robot.data,q) + pin.framesForwardKinematics(robot.model, robot.data, q) + pin.computeJointJacobians(robot.model, robot.data, q) oMlhand = robot.data.oMf[lhand_id] oMrhand = robot.data.oMf[rhand_id] lhandMlhook = oMlhand.inverse()*oMlhook - lhand_nu = pin.log(lhandMlhook).vector - rhandMrhook = oMrhand.inverse()*oMrhook - rhand_nu = pin.log(rhandMrhook).vector + + nu_L = pin.log(lhandMlhook).vector + nu_R = pin.log(rhandMrhook).vector + + J_L = pin.computeFrameJacobian(robot.model, robot.data, q, lhand_id, pin.LOCAL) + J_R = pin.computeFrameJacobian(robot.model, robot.data, q, rhand_id, pin.LOCAL) - oRlhand = oMlhand.rotation - lhand_Jlhand = pin.computeFrameJacobian(robot.model,robot.data,q,lhand_id,pin.LOCAL_WORLD_ALIGNED) - o_Jlhand = pin.computeFrameJacobian(robot.model,robot.data,q,lhand_id, pin.WORLD) + JL_p = pinv(J_L) + v1 = JL_p @ nu_L + N1 = np.eye(robot.nv) - JL_p @ J_L - oRrhand = oMrhand.rotation - rhand_Jrhand = pin.computeFrameJacobian(robot.model,robot.data,q,rhand_id,pin.LOCAL_WORLD_ALIGNED) - o_Jrhand = pin.computeFrameJacobian(robot.model,robot.data,q,rhand_id, pin.WORLD) + JRN = J_R @ N1 + JRN_p = pinv(JRN) - vq = pinv(lhand_Jlhand)@lhand_nu - Prhand = np.eye(robot.nv)-pinv(o_Jrhand) @ o_Jrhand - vq += pinv(rhand_Jrhand @ Prhand) @ (o_Jlhand - o_Jrhand @ vq) + v2 = JRN_p @ (nu_R - J_R @ v1) + vq = v1 + N1 @ v2 q = pin.integrate(robot.model,q, vq) @@ -62,9 +61,9 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None): if viz: viz.display(q) - time.sleep(1e-3) + time.sleep(1) - if lerror < EPSILON and not collision(robot, q): + if lerror < EPSILON and rerror < EPSILON and not collision(robot, q): success = True break From 00a37adb2c86dee6114f2dcab5cd4b41ffb8fe69 Mon Sep 17 00:00:00 2001 From: Monica Sekoyan Date: Sun, 12 Oct 2025 23:59:21 +0100 Subject: [PATCH 03/23] add lab 1 solved Signed-off-by: Monica Sekoyan --- inverse_geometry.py | 122 ++++++++++++++++++++++++++++++++++++++++++-- tools.py | 2 +- 2 files changed, 120 insertions(+), 4 deletions(-) diff --git a/inverse_geometry.py b/inverse_geometry.py index 8f03044..b4b7814 100644 --- a/inverse_geometry.py +++ b/inverse_geometry.py @@ -18,9 +18,125 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None): '''Return a collision free configuration grasping a cube at a specific location and a success flag''' setcubeplacement(robot, cube, cubetarget) - #TODO implement - print ("TODO: implement me") - return robot.q0, False + + q = qcurrent.copy() + DT = EPSILON * 10 + Kpost = 1 # penalty term for initial postural change + + max_updates = 1000 + updates_done = 0 + q_opt_found = False + + oMLhook = getcubeplacement(cube, LEFT_HOOK) + oMRhook = getcubeplacement(cube, RIGHT_HOOK) + + while updates_done < max_updates: + + pin.framesForwardKinematics(robot.model, robot.data, q) + pin.computeJointJacobians(robot.model, robot.data, q) + + left_hand_frameid = robot.model.getFrameId(LEFT_HAND) + oMLhand = robot.data.oMf[left_hand_frameid] + + right_hand_frameid = robot.model.getFrameId(RIGHT_HAND) + oMRhand = robot.data.oMf[right_hand_frameid] + + # FIRST TASK: moving left hand to the left hook + + # get the homogeneous matrix that takes from left hand frame to hook frame + lhandMLhook = oMLhand.inverse() * oMLhook + + # 6D spatial vector that is needed to be applied in 1 second + # so the left hand's frame will be in the position of hook frame's position + lhand_nu_lhand = pin.log(lhandMLhook).vector + + # get the 6D vector in the world frame + lhand_nu_world = oMLhand.action @ lhand_nu_lhand + + # Get corresponding jacobian for lhand + o_Jlhand = pin.computeFrameJacobian( + robot.model, + robot.data, + q, + left_hand_frameid, + pin.WORLD + ) + + # get the joint velocities to move left hand coordinates to the hook position + lhand_vq = pinv(o_Jlhand) @ lhand_nu_world + + # get the null-space projector matrix of the left hand Jacobian + # any vector multiplied by Plhand is projected into the null space of o_Jlhand + # meaning if that vector is multiplied by o_Jlhand = 0 => lhand is not moved + Plhand = np.eye(robot.nv) - pinv(o_Jlhand) @ o_Jlhand + + # SECOND TASK: moving right hand to the right hook + # Get corresponding jacobian for rhand + o_Jrhand = pin.computeFrameJacobian( + robot.model, + robot.data, + q, + right_hand_frameid, + pin.WORLD + ) + + o_Jrhand_lhand_null = o_Jrhand @ Plhand + + # Spatial velocity of right hand frame + rhand_nu_world_from_lhand_vq = o_Jrhand @ lhand_vq + + # get the right hook into the right hand frame + rhandMLhook = oMRhand.inverse() * oMRhook + + # get the 6D spacial velocity matrix + rhand_nu_rhand = pin.log(rhandMLhook).vector + + # get the 6D vector in the world frame + rhand_nu_world = oMRhand.action @ rhand_nu_rhand + + # substract the spatial velocity that from left hand + rhand_nu_world -= rhand_nu_world_from_lhand_vq + + # calculate right hand joint spatial velocities projected + # into the left hand jacobian matrix's null space + rhand_vq_null = pinv(o_Jrhand_lhand_null) @ rhand_nu_world + + + # THIRD TASK: add postural bias + # as the initial configuration of the robot is natural, add penalty of getting far from the q_ref + Plhandrhand = Plhand - pinv(o_Jrhand_lhand_null) @ o_Jrhand @ Plhand + delta_q_to_ref = pin.difference(robot.model, q, robot.q0) + v_post = Kpost * delta_q_to_ref + postural_vq_null = Plhandrhand @ v_post + + # combine both hands' joint velocities + vq = lhand_vq + rhand_vq_null + postural_vq_null + + + # check if the angle and translation to the targets are small enough + # so we can early stop the optimization + rotation_to_left_hook = norm(lhand_nu_lhand[:3]) + translation_to_left_hook = norm(lhand_nu_lhand[3:]) + + rotation_to_right_hook = norm(rhand_nu_world[:3]) + translation_to_right_hook = norm(rhand_nu_world[3:]) + + left_hook_close_enough = rotation_to_left_hook <= EPSILON and translation_to_left_hook <= EPSILON + right_hook_close_enough = rotation_to_right_hook <= EPSILON and translation_to_right_hook <= EPSILON + + if left_hook_close_enough and right_hook_close_enough: + if not collision(robot, q): + q_opt_found = True + break + + # get the q by integrating the current q and unit time joint velocities multiplied by DT + q = pin.integrate(robot.model, q, vq * DT) + updates_done += 1 + + if viz: + viz.display(q) + + return q, q_opt_found if __name__ == "__main__": from tools import setupwithmeshcat diff --git a/tools.py b/tools.py index 667b3fc..9ad021c 100644 --- a/tools.py +++ b/tools.py @@ -76,7 +76,7 @@ def setcubeplacement(robot, cube, oMf): def setupwithmeshcat(url=MESHCAT_URL): '''setups everything to work with the robot and meshcat''' robot, table, obstacle, cube = setuppinocchio() - viz = setupmeshcat(robot) + viz = setupmeshcat(robot, url) return robot, cube, viz from setup_pybullet import setuppybullet From c5ba941e35c93d10855c230532f49ca110b5282a Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Tue, 14 Oct 2025 13:09:44 +0100 Subject: [PATCH 04/23] Update gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 68bc17f..a3b73ff 100644 --- a/.gitignore +++ b/.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 From 4293d8387443d6ccac64bf2df8970b53ef4d8103 Mon Sep 17 00:00:00 2001 From: Monica Sekoyan Date: Sat, 18 Oct 2025 20:06:10 +0100 Subject: [PATCH 05/23] add path planning with rrt Signed-off-by: Monica Sekoyan --- path.py | 456 +++++++++++++++++++++++++++++++++++++++++++++++++++++-- tools.py | 2 +- 2 files changed, 446 insertions(+), 12 deletions(-) diff --git a/path.py b/path.py index 03d3b90..4260877 100644 --- a/path.py +++ b/path.py @@ -13,16 +13,452 @@ from config import LEFT_HAND, RIGHT_HAND import time +from typing import Callable, Tuple, List +from pinocchio.utils import rotate +from tools import setcubeplacement + + +#### Helper Functions for Random Cube Placement Sampling #### + +def get_table_borders(table: pin.robot_wrapper.RobotWrapper) -> Tuple[np.array, np.array]: + """ Returns the minimum and maximum x, y, z coordinates of the table. + Args: + table: table object + Returns: + xyz_min: minimum x, y, z coordinates of the table's leftmost and bottommost corner + xyz_max: maximum x, y, z coordinates of the table's rightmost and topmost corner + """ + pin.updateGeometryPlacements(table.model, table.data, table.collision_model, table.collision_data) + table_surface = table.collision_model.geometryObjects[0].geometry + oMtable_surface = table.collision_data.oMg[0] + R, t = oMtable_surface.rotation, oMtable_surface.translation + + + half = table_surface.halfSide + local_corners = np.array([[sx, sy, sz] + for sx in [+1, -1] + for sy in [+1, -1] + for sz in [+1, -1]]) * half + world_corners = (R @ local_corners.T).T + t + + xyz_min = world_corners.min(axis=0) + xyz_max = world_corners.max(axis=0) + + return xyz_min, xyz_max + + + +def random_cube_placement( + robot: pin.robot_wrapper.RobotWrapper, + cube: pin.robot_wrapper.RobotWrapper, + table: pin.robot_wrapper.RobotWrapper, + check_collisions: bool = True + ) -> pin.pinocchio_pywrap_default.SE3: + """ Returns a random cube placement on the table that is collision free. + Args: + robot: robot object + cube: cube object + table: table object + check_collisions: whether to check for collisions + Returns: + valid_placement: random cube placement on the table that is collision free + """ + xyz_min, xyz_max = get_table_borders(table) + + collision = True + valid_placement = None + + while collision: + x = np.random.uniform(xyz_min[0], xyz_max[0]) + y = np.random.uniform(xyz_min[1], xyz_max[1]) + # TODO: change harcoded max z to a more clever bounding + z = np.random.uniform(0.93, 2) + + placement = pin.SE3(rotate('z', 0.), np.array([x, y, z])) + setcubeplacement(robot, cube, placement) + if check_collisions: + collision = pin.computeCollisions(cube.collision_model, cube.collision_data, False) + valid_placement = placement if not collision else valid_placement + else: + valid_placement = placement + collision = False + + return valid_placement + + +#### Helper Functions and Classes for RRT #### + +class Vertex: + def __init__(self, q, grasping_q, parent): + self.q = q + self.grasping_q = grasping_q + self.parent = parent + +class RRT: + def __init__(self, + root_q: pin.pinocchio_pywrap_default.SE3, + root_grasping_q: np.array, + robot: pin.robot_wrapper.RobotWrapper, + cube: pin.robot_wrapper.RobotWrapper, + get_grasping_poseq: Callable): + + self.robot = robot + self.cube = cube + self.get_grasping_poseq = get_grasping_poseq + + # initialize the tree + self.nodes = [ + Vertex(q=root_q, grasping_q=root_grasping_q, parent=None) + ] + + @staticmethod + def calc_distance( + q1: np.array, + q2: np.array) -> float: + '''Return the euclidian distance between two configurations''' + return np.linalg.norm(q2 - q1) + + @staticmethod + def lerp( + q0: np.array, + q1: np.array, + t: float + ) -> np.array: + """ + Performs linear interpolation between the given points and for the step t + """ + return RRT.get_placement_from_translation((1 - t) * q0 + t * q1) + + @staticmethod + def get_placement_from_translation(translation): + return pin.SE3(rotate('z', 0.), np.array(translation)) + + def add_edge(self, + parent_idx: int, + new_q: pin.pinocchio_pywrap_default.SE3, + new_grasping_q: np.array) -> None: + + """ + Constructs and adds a new vertex to the tree. + """ + + self.nodes.append( + Vertex(q=new_q, + grasping_q=new_grasping_q, + parent=self.nodes[parent_idx]) + ) + + def get_nearest_vertex(self, + q: pin.pinocchio_pywrap_default.SE3 + ) -> Tuple[int, Vertex]: + + """ + Args: + q: random configuration of the cube (SE3) + Returns: + nearest_vertex_idx: nearest vertex index in the tree + nearest_vertex: nearest vertex + """ + nearest_vertex_idx = None + smallest_distance = None + + for i, vertex in enumerate(self.nodes): + dist = self.calc_distance(vertex.q.translation, q.translation) + if not smallest_distance or dist <= smallest_distance: + smallest_distance = dist + nearest_vertex_idx = i + + return nearest_vertex_idx, self.nodes[nearest_vertex_idx] + + def get_q_new(self, + q_nearest_vertex: Vertex, + q_rand: pin.pinocchio_pywrap_default.SE3, + discretisation_steps: int, + max_delta_q: int = None, + ) -> Tuple[pin.pinocchio_pywrap_default.SE3, np.array, bool]: + + """ Returns a new configuration that is the closest to the random configuration `q_rand` that can be grasped and is collison free. + Args: + q_nearest_vertex: nearest vertex + q_rand: random configuration + discretisation_steps: number of discretisation steps + max_delta_q: maximum delta q + Returns: + q_new: new configuration + grasping_q: grasping pose for the new configuration + q_new_found: True if the new configuration (other than the `q_nearest_vertex`) is found, False otherwise + """ + + q_new_found = True + q_end = q_rand.copy() + + dist = self.calc_distance( + q_nearest_vertex.q.translation, + q_rand.translation) + + if max_delta_q is not None and dist > max_delta_q: + q_end = self.lerp( + q_nearest_vertex.q.translation, + q_rand.translation, + max_delta_q/dist) + + if self.get_grasping_poseq is not None: + dt = 1 / discretisation_steps + for i in range(1, discretisation_steps): + q = self.lerp( + q_nearest_vertex.q.translation, + q_end.translation, + dt*i) + grasping_q, found_grasping_pose = self.get_grasping_poseq( + robot=self.robot, + qcurrent=q_nearest_vertex.grasping_q, + cube=self.cube, + cubetarget=q, + ) + if not found_grasping_pose: + if i - 1 == 0: + q_new_found = False + return self.lerp( + q_nearest_vertex.q.translation, + q_end.translation, + dt*(i-1)), grasping_q, q_new_found + else: + grasping_q = None + + return q_end, grasping_q, q_new_found + + def check_edge( + self, + q_latest_new: pin.pinocchio_pywrap_default.SE3, + q_latest_new_grasping_pose: np.array, + q_goal: pin.pinocchio_pywrap_default.SE3, + discretisation_steps: int = 1000, + ) -> bool: + + + """ Checks if there is a valid edge between the latest new configuration and the goal configuration. + Args: + q_latest_new: latest new configuration + q_latest_new_grasping_pose: grasping pose for the latest new configuration + q_goal: goal configuration + discretisation_steps: number of discretisation steps + Returns: + valid_edge: True if there is a valid edge, False otherwise + """ + + valid_edge = False + q_latest_new_vertex = Vertex( + q=q_latest_new, + grasping_q=q_latest_new_grasping_pose, + parent=None + ) + + # check if we can get to the q_goal from the q_new directly + q_new, q_new_grasping_q, q_new_found = self.get_q_new( + q_nearest_vertex=q_latest_new_vertex, + q_rand=q_goal, + discretisation_steps=discretisation_steps, + max_delta_q=None + ) + + if q_new_found: + distance_to_target = self.calc_distance(q_new.translation, q_goal.translation) + if distance_to_target < 1e-3: + valid_edge = True + + return valid_edge + + def get_path_from_rrt(self) -> List[Vertex]: + """ Returns a list of vertices from the root to the latest vertex. + If there was a valid edge between the latest vertex and the goal configuration, the latest vertex is the vertex with the goal configuration. + Returns: + vertices: list of vertices + """ + vertices = [] + last_vertex = self.nodes[-1] + + while last_vertex.parent is not None: + vertices.insert(0, last_vertex) + last_vertex = last_vertex.parent + + vertices.insert(0, last_vertex) + + return vertices + + def get_shortcut_from_rrt_path(self, + discretisation_steps: int = 100, + ) -> List[Vertex]: + + """ Returns a shortcut path from the root to the latest vertex. + Args: + discretisation_steps: number of discretisation steps + Returns: + short_path: list of vertices from the root to the latest vertex that has no redundant intermediate vertices + """ + + path_vertices = self.get_path_from_rrt() + short_path = [] + start_idx = 0 + + while start_idx < len(path_vertices) - 3: + current_vertex = path_vertices[start_idx] + short_path.append(current_vertex) + + # search in the next nodes starting from the furthest one + next_vertex_idx = len(path_vertices) - 1 + while next_vertex_idx > start_idx + 1: + next_vertex = path_vertices[next_vertex_idx] + is_valid_edge = self.check_edge( + q_latest_new=current_vertex.q, + q_latest_new_grasping_pose=current_vertex.grasping_q, + q_goal=next_vertex.q, + discretisation_steps=discretisation_steps + ) + if is_valid_edge: + start_idx = next_vertex_idx + break + next_vertex_idx -= 1 + + if not is_valid_edge: + start_idx += 1 + + short_path.extend(path_vertices[start_idx:]) + + return short_path + + + +#### The main function for constructing the RRT #### + +def construct_rrt(robot: pin.robot_wrapper.RobotWrapper, + cube: pin.robot_wrapper.RobotWrapper, + q_init: np.array, + q_goal: np.array, + cubeplacementq0: pin.pinocchio_pywrap_default.SE3, + cubeplacementqgoal: pin.pinocchio_pywrap_default.SE3, + num_iter: int, + radnom_sampler: Callable, + max_delta_q: float = None, + discretisation_steps: int = 100, + get_grasping_poseq: Callable = None, + ): + + """ Constructs the RRT from the root to the goal configuration. + Args: + robot: robot object + cube: cube object + q_init: initial grasping pose + q_goal: goal grasping pose + cubeplacementq0: initial cube placement + cubeplacementqgoal: goal cube placement + num_iter: number of iterations of sampling random cube placements + radnom_sampler: random cube placement sampler + max_delta_q: maximum delta q between the current and the end configuration used to get q_new configuration + discretisation_steps: number of discretisation steps used in the get_q_new and check_edge function + get_grasping_poseq: grasping pose calculator from the cube placement (inverse geometry). + Returns: + rrt: RRT object + """ + rrt = RRT( + root_q=cubeplacementq0, + root_grasping_q=q_init, + robot=robot, + cube=cube, + get_grasping_poseq=get_grasping_poseq + ) + + for _ in range(num_iter): + random_cp_q = radnom_sampler() + cp_q_nearest_idx, cp_q_nearest_vertex = rrt.get_nearest_vertex(random_cp_q) + + cp_q_new, cp_q_new_grasping_pose, cp_q_new_found = rrt.get_q_new( + q_nearest_vertex=cp_q_nearest_vertex, + q_rand=random_cp_q, + discretisation_steps=discretisation_steps, + max_delta_q=max_delta_q) + + if cp_q_new_found: + rrt.add_edge( + parent_idx=cp_q_nearest_idx, + new_q=cp_q_new, + new_grasping_q=cp_q_new_grasping_pose, + ) + + is_valid_edge = rrt.check_edge( + q_latest_new=cp_q_new, + q_latest_new_grasping_pose=cp_q_new_grasping_pose, + q_goal=cubeplacementqgoal, + discretisation_steps=discretisation_steps) + + if is_valid_edge: + rrt.add_edge(parent_idx=len(rrt.nodes) - 1, + new_q=cubeplacementqgoal, + new_grasping_q=q_goal) + + return rrt + + return None + #returns a collision free path from qinit to qgoal under grasping constraints #the path is expressed as a list of configurations -def computepath(qinit,qgoal,cubeplacementq0, cubeplacementqgoal): - #TODO - return [qinit, qgoal] - pass +def computepath(qinit, qgoal, cubeplacementq0, cubeplacementqgoal): + """ Returns a collision free path from qinit to qgoal under grasping constraints. + Args: + qinit: initial configuration + qgoal: goal configuration + cubeplacementq0: initial cube placement + cubeplacementqgoal: goal cube placement + Returns: + path: list of configurations + """ + + global robot + global cube + global table + global computeqgrasppose + + NUM_ITER = 200 + RANDOM_SAMPLER = lambda: random_cube_placement(robot=robot, cube=cube, table=table) + MAX_DELTA_Q = None + DISCRETISATION_STEPS = 100 + GET_GRAPSING_POSEQ = computeqgrasppose + print('Searching for a valid path...(this may take some time)') + rrt = construct_rrt( + robot=robot, + cube=cube, + q_init=qinit, + q_goal=qgoal, + cubeplacementq0=cubeplacementq0, + cubeplacementqgoal=cubeplacementqgoal, + num_iter=NUM_ITER, + radnom_sampler=RANDOM_SAMPLER, + max_delta_q=MAX_DELTA_Q, + discretisation_steps=DISCRETISATION_STEPS, + get_grasping_poseq=GET_GRAPSING_POSEQ, + ) -def displaypath(robot,path,dt,viz): - for q in path: + if not rrt: + print( + """Valid Path was not found! + Try to increase discretisation steps or try with different start and end configurations.""") + + return [], [] + + print('Path found! Retrieving shortcut path from RRT...') + + path_vertices = rrt.get_shortcut_from_rrt_path() + + path_configurations = [vertex.grasping_q for vertex in path_vertices] + cube_placements = [vertex.q for vertex in path_vertices] + + print('Path is ready!') + + return path_configurations, cube_placements + +def displaypath(robot, cube, path, cube_placements, dt, viz): + for q, cube_placement in zip(path, cube_placements): + setcubeplacement(robot, cube,cube_placement) viz.display(q) time.sleep(dt) @@ -32,8 +468,7 @@ def displaypath(robot,path,dt,viz): from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET from inverse_geometry import computeqgrasppose - robot, cube, viz = setupwithmeshcat() - + robot, cube, table, viz = setupwithmeshcat() q = robot.q0.copy() q0,successinit = computeqgrasppose(robot, q, cube, CUBE_PLACEMENT, viz) @@ -42,7 +477,6 @@ def displaypath(robot,path,dt,viz): if not(successinit and successend): print ("error: invalid initial or end configuration") - path = computepath(q0,qe,CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET) - - displaypath(robot,path,dt=0.5,viz=viz) #you ll probably want to lower dt + path, cube_placements = computepath(q0, qe, CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET) + displaypath(robot, cube, path, cube_placements, dt=0.5, viz=viz) #you ll probably want to lower dt \ No newline at end of file diff --git a/tools.py b/tools.py index 9ad021c..a3a49f9 100644 --- a/tools.py +++ b/tools.py @@ -77,7 +77,7 @@ def setupwithmeshcat(url=MESHCAT_URL): '''setups everything to work with the robot and meshcat''' robot, table, obstacle, cube = setuppinocchio() viz = setupmeshcat(robot, url) - return robot, cube, viz + return robot, cube, table, viz from setup_pybullet import setuppybullet def setupwithpybullet(): From e883bda745667e2b41d564928772dbc7475bf196 Mon Sep 17 00:00:00 2001 From: Monica Sekoyan Date: Tue, 28 Oct 2025 14:06:07 +0000 Subject: [PATCH 06/23] add fix for global vars Signed-off-by: Monica Sekoyan --- control.py | 56 +++++++++++++++++++++++++++++++++++++++++++++--------- path.py | 18 ++++++++++++------ tools.py | 2 +- 3 files changed, 60 insertions(+), 16 deletions(-) diff --git a/control.py b/control.py index 8ad6f1a..1b31abf 100644 --- a/control.py +++ b/control.py @@ -16,8 +16,25 @@ def controllaw(sim, robot, trajs, tcurrent, cube): q, vq = sim.getpybulletstate() - #TODO - torques = [0.0 for _ in sim.bulletCtrlJointsInPinOrder] + + q_target = trajs[0](tcurrent) + vq_target = trajs[1](tcurrent) + + + error = q_target - q + vvq_des = Kp * error + Kv * vq_target + + # data = robot.data + + # Mass matrix + M = robot.mass(q) + h = robot.nle(q, vq) + + torques = M @ vvq_des + h + + # print('Order: ', sim.bulletCtrlJointsInPinOrder) + + # torques = [torques[i] for i in sim.bulletCtrlJointsInPinOrder] sim.step(torques) if __name__ == "__main__": @@ -25,7 +42,7 @@ def controllaw(sim, robot, trajs, tcurrent, cube): from tools import setupwithpybullet, setupwithpybulletandmeshcat, rununtil from config import DT - robot, sim, cube = setupwithpybullet() + robot, sim, table, cube = setupwithpybullet() from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET @@ -34,7 +51,19 @@ def controllaw(sim, robot, trajs, tcurrent, cube): q0,successinit = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT, None) qe,successend = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT_TARGET, None) - path = computepath(q0,qe,CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET) + path, cube_placements = computepath( + q0, + qe, + CUBE_PLACEMENT, + CUBE_PLACEMENT_TARGET, + robot=robot, + cube=cube, + table=table, + computeqgrasppose=computeqgrasppose) + + + print('Success Init: ', successinit) + print('Success End: ', successend) #setting initial configuration @@ -44,22 +73,31 @@ def controllaw(sim, robot, trajs, tcurrent, cube): #TODO this is just an example, you are free to do as you please. #In any case this trajectory does not follow the path #0 init and end velocities - def maketraj(q0,q1,T): #TODO compute a real trajectory ! - q_of_t = Bezier([q0,q0,q1,q1],t_max=T) + def maketraj(q0,q1,path, T): #TODO compute a real trajectory ! + poimtlist = [q0, q0] + path + [q1, q1] + # point = [q0, q0, q1, q1] + q_of_t = Bezier(poimtlist,t_max=T) vq_of_t = q_of_t.derivative(1) vvq_of_t = vq_of_t.derivative(1) return q_of_t, vq_of_t, vvq_of_t #TODO this is just a random trajectory, you need to do this yourself - total_time=4. - trajs = maketraj(q0, qe, total_time) + total_time=10. + trajs = maketraj(q0, qe, path, total_time) tcur = 0. while tcur < total_time: - rununtil(controllaw, DT, sim, robot, trajs, tcur, cube) + rununtil( + controllaw, + DT, + sim, + robot, + trajs, + tcur, + cube) tcur += DT diff --git a/path.py b/path.py index 4260877..3129510 100644 --- a/path.py +++ b/path.py @@ -401,7 +401,14 @@ def construct_rrt(robot: pin.robot_wrapper.RobotWrapper, #returns a collision free path from qinit to qgoal under grasping constraints #the path is expressed as a list of configurations -def computepath(qinit, qgoal, cubeplacementq0, cubeplacementqgoal): +def computepath(qinit, + qgoal, + cubeplacementq0, + cubeplacementqgoal, + robot=None, + cube=None, + table=None, + computeqgrasppose=None): """ Returns a collision free path from qinit to qgoal under grasping constraints. Args: qinit: initial configuration @@ -411,11 +418,10 @@ def computepath(qinit, qgoal, cubeplacementq0, cubeplacementqgoal): Returns: path: list of configurations """ - - global robot - global cube - global table - global computeqgrasppose + robot = robot or globals().get("robot") + cube = cube or globals().get("cube") + table = table or globals().get("table") + computeqgrasppose = computeqgrasppose or globals().get("computeqgrasppose") NUM_ITER = 200 RANDOM_SAMPLER = lambda: random_cube_placement(robot=robot, cube=cube, table=table) diff --git a/tools.py b/tools.py index a3a49f9..fc9e0da 100644 --- a/tools.py +++ b/tools.py @@ -85,7 +85,7 @@ def setupwithpybullet(): robot, table, obstacle, cube = setuppinocchio() sim = setuppybullet(robot) sim.setTorqueControlMode() - return robot, sim, cube + return robot, sim, table, cube def setupwithpybulletandmeshcat(url=MESHCAT_URL): From c4ed3eefe3117e5e0d799d0f36c5e226d69144a6 Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Sun, 9 Nov 2025 12:16:17 +0000 Subject: [PATCH 07/23] Speed up RRT --- constants.py | 23 ++ control.py | 9 +- ik_utils.py | 30 ++ inverse_geometry.py | 22 +- path.py | 691 +++++++++++++++++--------------------------- rrt.py | 425 +++++++++++++++++++++++++++ sampling.py | 263 +++++++++++++++++ tools.py | 4 +- 8 files changed, 1036 insertions(+), 431 deletions(-) create mode 100644 constants.py create mode 100644 ik_utils.py create mode 100644 rrt.py create mode 100644 sampling.py diff --git a/constants.py b/constants.py new file mode 100644 index 0000000..482f25f --- /dev/null +++ b/constants.py @@ -0,0 +1,23 @@ +"""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, 1.3) +DEFAULT_CHECK_COLLISIONS: bool = True + +# RRT / planning +DEFAULT_NUM_ITER: int = 200 +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.02 diff --git a/control.py b/control.py index 1b31abf..9e604bb 100644 --- a/control.py +++ b/control.py @@ -42,7 +42,9 @@ def controllaw(sim, robot, trajs, tcurrent, cube): from tools import setupwithpybullet, setupwithpybulletandmeshcat, rununtil from config import DT - robot, sim, table, cube = setupwithpybullet() + # `setupwithpybullet` currently returns (robot, sim, cube). + # Unpack and avoid depending on any `table` object. + robot, sim, cube = setupwithpybullet() from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET @@ -54,11 +56,10 @@ def controllaw(sim, robot, trajs, tcurrent, cube): path, cube_placements = computepath( q0, qe, - CUBE_PLACEMENT, + CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET, - robot=robot, + robot=robot, cube=cube, - table=table, computeqgrasppose=computeqgrasppose) diff --git a/ik_utils.py b/ik_utils.py new file mode 100644 index 0000000..9a2fb16 --- /dev/null +++ b/ik_utils.py @@ -0,0 +1,30 @@ +"""IK helper utilities. + +Provides a thin wrapper `call_ik` that calls an IK callback with a +backwards-compatible set of kwargs, times the call, and returns +((q, success), dt). The wrapper will inspect the callback signature and +only pass supported keyword arguments so older callbacks remain compatible. +""" +from typing import Callable, Any, Tuple +import time +import inspect + + +def call_ik(callback: Callable[..., Any], /, **kwargs) -> Tuple[Any, float]: + """Call an IK callback while timing it and keeping backwards compatibility. + + The callback is expected to return a tuple like (q, success). `kwargs` + may include: robot, qcurrent, cube, cubetarget, viz, max_attempts, viz_sleep. + + Returns: + (result, dt) where `result` is whatever the callback returned and dt is + the wall-time seconds spent in the call. + """ + sig = inspect.signature(callback) + # Filter kwargs to what the callback actually accepts + call_kwargs = {k: v for k, v in kwargs.items() if k in sig.parameters} + + t0 = time.perf_counter() + res = callback(**call_kwargs) + dt = time.perf_counter() - t0 + return res, dt diff --git a/inverse_geometry.py b/inverse_geometry.py index a2aeac0..4de1e3e 100644 --- a/inverse_geometry.py +++ b/inverse_geometry.py @@ -16,9 +16,20 @@ from tools import setcubeplacement -def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None): - '''Return a collision free configuration grasping a cube at a specific location and a success flag''' +def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None, max_attempts: int = 1000, viz_sleep: bool = True): + '''Return a collision free configuration grasping a cube at a specific location and a success flag. + + Args: + robot: RobotWrapper + qcurrent: initial joint configuration to seed the IK + cube: cube body + cubetarget: desired cube SE3 placement + viz: optional visualizer + max_attempts: maximum number of IK iteration steps to attempt (default 1000) + ''' q = qcurrent.copy() + # Ensure the cube is placed at the requested target before solving IK + setcubeplacement(robot, cube, cubetarget) lhand_id = robot.model.getFrameId(LEFT_HAND) rhand_id = robot.model.getFrameId(RIGHT_HAND) @@ -28,7 +39,7 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None): success = False - for i in range(1000): + for i in range(max_attempts): pin.framesForwardKinematics(robot.model, robot.data, q) pin.computeJointJacobians(robot.model, robot.data, q) @@ -65,8 +76,11 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None): rerror = norm(oMrhand.translation - oMrhook.translation) if viz: + # allow callers to request that we display without sleeping so the + # planner can validate IK quickly without incurring a sleep cost. viz.display(q) - time.sleep(1) + if viz_sleep: + time.sleep(0.1) if lerror < EPSILON and rerror < EPSILON and not collision(robot, q): success = True diff --git a/path.py b/path.py index 3129510..4344c3f 100644 --- a/path.py +++ b/path.py @@ -1,414 +1,48 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Thu Sep 21 11:44:32 2023 +"""Compatibility wrapper module. -@author: stonneau +This file used to contain the RRT and sampling implementation. To keep +backwards compatibility for imports that expect `path.py`, it re-exports the +cleaner implementations from `sampling.py` and `rrt.py`. """ +from sampling import random_cube_placement +import sampling as sampling_module +from rrt import Vertex, RRT, construct_rrt +from constants import DEFAULT_NUM_ITER, DEFAULT_DISCRETISATION_STEPS, DEFAULT_VIZ_DELAY, DEFAULT_SAMPLER_SHRINK, DEFAULT_PLANE_HALF_WIDTH, DEFAULT_TABLE_Z_RANGE +from tools import setcubeplacement +import time +import logging +from typing import List import pinocchio as pin import numpy as np -from numpy.linalg import pinv - -from config import LEFT_HAND, RIGHT_HAND -import time - -from typing import Callable, Tuple, List -from pinocchio.utils import rotate -from tools import setcubeplacement - - -#### Helper Functions for Random Cube Placement Sampling #### -def get_table_borders(table: pin.robot_wrapper.RobotWrapper) -> Tuple[np.array, np.array]: - """ Returns the minimum and maximum x, y, z coordinates of the table. - Args: - table: table object - Returns: - xyz_min: minimum x, y, z coordinates of the table's leftmost and bottommost corner - xyz_max: maximum x, y, z coordinates of the table's rightmost and topmost corner - """ - pin.updateGeometryPlacements(table.model, table.data, table.collision_model, table.collision_data) - table_surface = table.collision_model.geometryObjects[0].geometry - oMtable_surface = table.collision_data.oMg[0] - R, t = oMtable_surface.rotation, oMtable_surface.translation +logger = logging.getLogger(__name__) +__all__ = [ + "random_cube_placement", + "Vertex", + "RRT", + "construct_rrt", +] - half = table_surface.halfSide - local_corners = np.array([[sx, sy, sz] - for sx in [+1, -1] - for sy in [+1, -1] - for sz in [+1, -1]]) * half - world_corners = (R @ local_corners.T).T + t - xyz_min = world_corners.min(axis=0) - xyz_max = world_corners.max(axis=0) - - return xyz_min, xyz_max - - - -def random_cube_placement( - robot: pin.robot_wrapper.RobotWrapper, - cube: pin.robot_wrapper.RobotWrapper, - table: pin.robot_wrapper.RobotWrapper, - check_collisions: bool = True - ) -> pin.pinocchio_pywrap_default.SE3: - """ Returns a random cube placement on the table that is collision free. - Args: - robot: robot object - cube: cube object - table: table object - check_collisions: whether to check for collisions - Returns: - valid_placement: random cube placement on the table that is collision free - """ - xyz_min, xyz_max = get_table_borders(table) - - collision = True - valid_placement = None - - while collision: - x = np.random.uniform(xyz_min[0], xyz_max[0]) - y = np.random.uniform(xyz_min[1], xyz_max[1]) - # TODO: change harcoded max z to a more clever bounding - z = np.random.uniform(0.93, 2) - - placement = pin.SE3(rotate('z', 0.), np.array([x, y, z])) - setcubeplacement(robot, cube, placement) - if check_collisions: - collision = pin.computeCollisions(cube.collision_model, cube.collision_data, False) - valid_placement = placement if not collision else valid_placement - else: - valid_placement = placement - collision = False - - return valid_placement - - -#### Helper Functions and Classes for RRT #### - -class Vertex: - def __init__(self, q, grasping_q, parent): - self.q = q - self.grasping_q = grasping_q - self.parent = parent - -class RRT: - def __init__(self, - root_q: pin.pinocchio_pywrap_default.SE3, - root_grasping_q: np.array, - robot: pin.robot_wrapper.RobotWrapper, - cube: pin.robot_wrapper.RobotWrapper, - get_grasping_poseq: Callable): - - self.robot = robot - self.cube = cube - self.get_grasping_poseq = get_grasping_poseq - - # initialize the tree - self.nodes = [ - Vertex(q=root_q, grasping_q=root_grasping_q, parent=None) - ] - - @staticmethod - def calc_distance( - q1: np.array, - q2: np.array) -> float: - '''Return the euclidian distance between two configurations''' - return np.linalg.norm(q2 - q1) - - @staticmethod - def lerp( - q0: np.array, - q1: np.array, - t: float - ) -> np.array: - """ - Performs linear interpolation between the given points and for the step t - """ - return RRT.get_placement_from_translation((1 - t) * q0 + t * q1) - - @staticmethod - def get_placement_from_translation(translation): - return pin.SE3(rotate('z', 0.), np.array(translation)) - - def add_edge(self, - parent_idx: int, - new_q: pin.pinocchio_pywrap_default.SE3, - new_grasping_q: np.array) -> None: - - """ - Constructs and adds a new vertex to the tree. - """ - - self.nodes.append( - Vertex(q=new_q, - grasping_q=new_grasping_q, - parent=self.nodes[parent_idx]) - ) - - def get_nearest_vertex(self, - q: pin.pinocchio_pywrap_default.SE3 - ) -> Tuple[int, Vertex]: - - """ - Args: - q: random configuration of the cube (SE3) - Returns: - nearest_vertex_idx: nearest vertex index in the tree - nearest_vertex: nearest vertex - """ - nearest_vertex_idx = None - smallest_distance = None - - for i, vertex in enumerate(self.nodes): - dist = self.calc_distance(vertex.q.translation, q.translation) - if not smallest_distance or dist <= smallest_distance: - smallest_distance = dist - nearest_vertex_idx = i - - return nearest_vertex_idx, self.nodes[nearest_vertex_idx] - - def get_q_new(self, - q_nearest_vertex: Vertex, - q_rand: pin.pinocchio_pywrap_default.SE3, - discretisation_steps: int, - max_delta_q: int = None, - ) -> Tuple[pin.pinocchio_pywrap_default.SE3, np.array, bool]: - - """ Returns a new configuration that is the closest to the random configuration `q_rand` that can be grasped and is collison free. - Args: - q_nearest_vertex: nearest vertex - q_rand: random configuration - discretisation_steps: number of discretisation steps - max_delta_q: maximum delta q - Returns: - q_new: new configuration - grasping_q: grasping pose for the new configuration - q_new_found: True if the new configuration (other than the `q_nearest_vertex`) is found, False otherwise - """ - - q_new_found = True - q_end = q_rand.copy() - - dist = self.calc_distance( - q_nearest_vertex.q.translation, - q_rand.translation) - - if max_delta_q is not None and dist > max_delta_q: - q_end = self.lerp( - q_nearest_vertex.q.translation, - q_rand.translation, - max_delta_q/dist) - - if self.get_grasping_poseq is not None: - dt = 1 / discretisation_steps - for i in range(1, discretisation_steps): - q = self.lerp( - q_nearest_vertex.q.translation, - q_end.translation, - dt*i) - grasping_q, found_grasping_pose = self.get_grasping_poseq( - robot=self.robot, - qcurrent=q_nearest_vertex.grasping_q, - cube=self.cube, - cubetarget=q, - ) - if not found_grasping_pose: - if i - 1 == 0: - q_new_found = False - return self.lerp( - q_nearest_vertex.q.translation, - q_end.translation, - dt*(i-1)), grasping_q, q_new_found - else: - grasping_q = None - - return q_end, grasping_q, q_new_found - - def check_edge( - self, - q_latest_new: pin.pinocchio_pywrap_default.SE3, - q_latest_new_grasping_pose: np.array, - q_goal: pin.pinocchio_pywrap_default.SE3, - discretisation_steps: int = 1000, - ) -> bool: - - - """ Checks if there is a valid edge between the latest new configuration and the goal configuration. - Args: - q_latest_new: latest new configuration - q_latest_new_grasping_pose: grasping pose for the latest new configuration - q_goal: goal configuration - discretisation_steps: number of discretisation steps - Returns: - valid_edge: True if there is a valid edge, False otherwise - """ - - valid_edge = False - q_latest_new_vertex = Vertex( - q=q_latest_new, - grasping_q=q_latest_new_grasping_pose, - parent=None - ) - - # check if we can get to the q_goal from the q_new directly - q_new, q_new_grasping_q, q_new_found = self.get_q_new( - q_nearest_vertex=q_latest_new_vertex, - q_rand=q_goal, - discretisation_steps=discretisation_steps, - max_delta_q=None - ) - - if q_new_found: - distance_to_target = self.calc_distance(q_new.translation, q_goal.translation) - if distance_to_target < 1e-3: - valid_edge = True - - return valid_edge - - def get_path_from_rrt(self) -> List[Vertex]: - """ Returns a list of vertices from the root to the latest vertex. - If there was a valid edge between the latest vertex and the goal configuration, the latest vertex is the vertex with the goal configuration. - Returns: - vertices: list of vertices - """ - vertices = [] - last_vertex = self.nodes[-1] - - while last_vertex.parent is not None: - vertices.insert(0, last_vertex) - last_vertex = last_vertex.parent - - vertices.insert(0, last_vertex) - - return vertices - - def get_shortcut_from_rrt_path(self, - discretisation_steps: int = 100, - ) -> List[Vertex]: - - """ Returns a shortcut path from the root to the latest vertex. - Args: - discretisation_steps: number of discretisation steps - Returns: - short_path: list of vertices from the root to the latest vertex that has no redundant intermediate vertices - """ - - path_vertices = self.get_path_from_rrt() - short_path = [] - start_idx = 0 - - while start_idx < len(path_vertices) - 3: - current_vertex = path_vertices[start_idx] - short_path.append(current_vertex) - - # search in the next nodes starting from the furthest one - next_vertex_idx = len(path_vertices) - 1 - while next_vertex_idx > start_idx + 1: - next_vertex = path_vertices[next_vertex_idx] - is_valid_edge = self.check_edge( - q_latest_new=current_vertex.q, - q_latest_new_grasping_pose=current_vertex.grasping_q, - q_goal=next_vertex.q, - discretisation_steps=discretisation_steps - ) - if is_valid_edge: - start_idx = next_vertex_idx - break - next_vertex_idx -= 1 - - if not is_valid_edge: - start_idx += 1 - - short_path.extend(path_vertices[start_idx:]) - - return short_path - - - -#### The main function for constructing the RRT #### - -def construct_rrt(robot: pin.robot_wrapper.RobotWrapper, - cube: pin.robot_wrapper.RobotWrapper, - q_init: np.array, - q_goal: np.array, - cubeplacementq0: pin.pinocchio_pywrap_default.SE3, - cubeplacementqgoal: pin.pinocchio_pywrap_default.SE3, - num_iter: int, - radnom_sampler: Callable, - max_delta_q: float = None, - discretisation_steps: int = 100, - get_grasping_poseq: Callable = None, - ): - - """ Constructs the RRT from the root to the goal configuration. - Args: - robot: robot object - cube: cube object - q_init: initial grasping pose - q_goal: goal grasping pose - cubeplacementq0: initial cube placement - cubeplacementqgoal: goal cube placement - num_iter: number of iterations of sampling random cube placements - radnom_sampler: random cube placement sampler - max_delta_q: maximum delta q between the current and the end configuration used to get q_new configuration - discretisation_steps: number of discretisation steps used in the get_q_new and check_edge function - get_grasping_poseq: grasping pose calculator from the cube placement (inverse geometry). - Returns: - rrt: RRT object - """ - rrt = RRT( - root_q=cubeplacementq0, - root_grasping_q=q_init, - robot=robot, - cube=cube, - get_grasping_poseq=get_grasping_poseq - ) - - for _ in range(num_iter): - random_cp_q = radnom_sampler() - cp_q_nearest_idx, cp_q_nearest_vertex = rrt.get_nearest_vertex(random_cp_q) - - cp_q_new, cp_q_new_grasping_pose, cp_q_new_found = rrt.get_q_new( - q_nearest_vertex=cp_q_nearest_vertex, - q_rand=random_cp_q, - discretisation_steps=discretisation_steps, - max_delta_q=max_delta_q) - - if cp_q_new_found: - rrt.add_edge( - parent_idx=cp_q_nearest_idx, - new_q=cp_q_new, - new_grasping_q=cp_q_new_grasping_pose, - ) - - is_valid_edge = rrt.check_edge( - q_latest_new=cp_q_new, - q_latest_new_grasping_pose=cp_q_new_grasping_pose, - q_goal=cubeplacementqgoal, - discretisation_steps=discretisation_steps) - - if is_valid_edge: - rrt.add_edge(parent_idx=len(rrt.nodes) - 1, - new_q=cubeplacementqgoal, - new_grasping_q=q_goal) - - return rrt - - return None - -#returns a collision free path from qinit to qgoal under grasping constraints -#the path is expressed as a list of configurations -def computepath(qinit, - qgoal, - cubeplacementq0, +def computepath(qinit, + qgoal, + cubeplacementq0, cubeplacementqgoal, - robot=None, + robot=None, cube=None, - table=None, - computeqgrasppose=None): + computeqgrasppose=None, + num_iter: int = DEFAULT_NUM_ITER, + discretisation_steps: int = DEFAULT_DISCRETISATION_STEPS, + max_ik_attempts: int = None, + post_path_wait: float = 5.0, + random_sampler=None, + repair_sampler=None, + repair_max_attempts: int = 5, + goal_bias: float = 0.02, + viz=None, + viz_delay: float = DEFAULT_VIZ_DELAY): """ Returns a collision free path from qinit to qgoal under grasping constraints. Args: qinit: initial configuration @@ -420,16 +54,67 @@ def computepath(qinit, """ robot = robot or globals().get("robot") cube = cube or globals().get("cube") - table = table or globals().get("table") computeqgrasppose = computeqgrasppose or globals().get("computeqgrasppose") - NUM_ITER = 200 - RANDOM_SAMPLER = lambda: random_cube_placement(robot=robot, cube=cube, table=table) + # If no sampler supplied, build a constrained sampler on the vertical plane + # that intersects the initial and final cube placements. + sampling_box_ranges = None + if random_sampler is None: + p0 = np.array(cubeplacementq0.translation) + p1 = np.array(cubeplacementqgoal.translation) + + p0_xy = p0[:2] + p1_xy = p1[:2] + seg = p1_xy - p0_xy + L = np.linalg.norm(seg) + if L < 1e-6: + u = np.array([1.0, 0.0]) + else: + u = seg / L + perp = np.array([-u[1], u[0]]) + + half_width = DEFAULT_PLANE_HALF_WIDTH + + # z range uses the default table z range + zmin, zmax = DEFAULT_TABLE_Z_RANGE + + def plane_sampler(): + while True: + s = np.random.uniform(0.0, 1.0) + t = np.random.uniform(-half_width, half_width) + xy = (1.0 - s) * p0_xy + s * p1_xy + perp * t + z = np.random.uniform(zmin, zmax) + placement = pin.SE3(np.eye(3), np.array([xy[0], xy[1], z])) + setcubeplacement(robot, cube, placement) + has_collision = pin.computeCollisions(cube.collision_model, cube.collision_data, False) + if not has_collision: + return placement + + RANDOM_SAMPLER = plane_sampler + + # sampling box for visualization: a thin box centered between p0 and p1 + center_xy = (p0_xy + p1_xy) / 2.0 + center = np.array([center_xy[0], center_xy[1], (zmin + zmax) / 2.0]) + dims_u = max(L, 1e-6) + dims_perp = 2.0 * half_width + dims_z = zmax - zmin + + # build axis-aligned x/y range for visualization convenience + corners_xy = [p0_xy + perp * (-half_width), p0_xy + perp * (half_width), + p1_xy + perp * (-half_width), p1_xy + perp * (half_width)] + xs = [c[0] for c in corners_xy] + ys = [c[1] for c in corners_xy] + x_range = (min(xs), max(xs)) + y_range = (min(ys), max(ys)) + z_range = (zmin, zmax) + sampling_box_ranges = (x_range, y_range, z_range) + else: + RANDOM_SAMPLER = random_sampler MAX_DELTA_Q = None - DISCRETISATION_STEPS = 100 GET_GRAPSING_POSEQ = computeqgrasppose print('Searching for a valid path...(this may take some time)') + # (no sampling-box visualization) rrt = construct_rrt( robot=robot, cube=cube, @@ -437,18 +122,26 @@ def computepath(qinit, q_goal=qgoal, cubeplacementq0=cubeplacementq0, cubeplacementqgoal=cubeplacementqgoal, - num_iter=NUM_ITER, - radnom_sampler=RANDOM_SAMPLER, + num_iter=num_iter, + random_sampler=RANDOM_SAMPLER, + repair_sampler=repair_sampler, + repair_max_attempts=repair_max_attempts, + goal_bias=goal_bias, max_delta_q=MAX_DELTA_Q, - discretisation_steps=DISCRETISATION_STEPS, + discretisation_steps=discretisation_steps, get_grasping_poseq=GET_GRAPSING_POSEQ, + viz=viz, + viz_delay=viz_delay, + max_ik_attempts=max_ik_attempts, + max_time_s=None, + progress_log_every=50, ) if not rrt: print( """Valid Path was not found! Try to increase discretisation steps or try with different start and end configurations.""") - + return [], [] print('Path found! Retrieving shortcut path from RRT...') @@ -460,29 +153,185 @@ def computepath(qinit, print('Path is ready!') + # visualise the cube placements by stepping the robot so user sees the grasping poses + if viz: + # pause briefly so user sees the final planning state before playback + time.sleep(post_path_wait) + for v in path_vertices: + setcubeplacement(robot, cube, v.q) + viz.display(v.grasping_q) + time.sleep(max(0.01, viz_delay)) + + # log metrics if rrt stored them + if hasattr(rrt, 'metrics'): + logger.info('Path metrics: %s', rrt.metrics) + return path_configurations, cube_placements + def displaypath(robot, cube, path, cube_placements, dt, viz): for q, cube_placement in zip(path, cube_placements): - setcubeplacement(robot, cube,cube_placement) + setcubeplacement(robot, cube, cube_placement) viz.display(q) time.sleep(dt) if __name__ == "__main__": + import argparse from tools import setupwithmeshcat from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET from inverse_geometry import computeqgrasppose - - robot, cube, table, viz = setupwithmeshcat() - + import logging + + parser = argparse.ArgumentParser(description='Run RRT planner for cube placements') + parser.add_argument('--no-viz', action='store_true', help='Disable visualization during planning') + parser.add_argument('--discretisation', type=int, default=DEFAULT_DISCRETISATION_STEPS, help='Discretisation steps for interpolation') + parser.add_argument('--num-iter', type=int, default=DEFAULT_NUM_ITER, help='Maximum RRT iterations/samples') + parser.add_argument('--sampler', choices=['plane', 'smart'], default='plane', help='Which placement sampler to use') + # smart sampler params + parser.add_argument('--smart-init-sigma', type=float, default=0.02, help='Initial sigma (m) for smart midpoint sampler') + parser.add_argument('--smart-scale', type=float, default=1.5, help='Sigma inflation factor for smart sampler') + parser.add_argument('--smart-max-sigma', type=float, default=0.2, help='Maximum sigma (m) for smart sampler') + parser.add_argument('--smart-max-attempts', type=int, default=30, help='Max attempts for smart sampler before fallback') + parser.add_argument('--max-time', type=float, default=None, help='Maximum wall time (s) for planner (abort early)') + parser.add_argument('--max-ik-attempts', type=int, default=None, help='Maximum IK iterations per interpolation point') + parser.add_argument('--max-delta-q', type=float, default=None, help='Maximum delta (translation) per extension (meters)') + parser.add_argument('--viz-delay', type=float, default=DEFAULT_VIZ_DELAY, help='Visualization delay in seconds') + parser.add_argument('--progress-every', type=int, default=50, help='How many iterations between progress logs') + parser.add_argument('--repair-attempts', type=int, default=5, help='How many local repair attempts to try when an extension fails') + parser.add_argument('--smart-global', action='store_true', help='Use the smart sampler as the global sampler instead of repair-only') + parser.add_argument('--goal-bias', type=float, default=0.02, help='Probability to sample the goal directly on each iteration (0.0-1.0)') + parser.add_argument('--playback', action='store_true', help='Show the final path even if planning ran with --no-viz') + args = parser.parse_args() + + logging.basicConfig(level=logging.INFO) + + # `setupwithmeshcat` returns (robot, cube, viz) in the current helpers. + # Unpack accordingly and avoid depending on any `table` object. + robot, cube, viz = setupwithmeshcat() + q = robot.q0.copy() - q0,successinit = computeqgrasppose(robot, q, cube, CUBE_PLACEMENT, viz) - qe,successend = computeqgrasppose(robot, q, cube, CUBE_PLACEMENT_TARGET, viz) - - if not(successinit and successend): - print ("error: invalid initial or end configuration") - - path, cube_placements = computepath(q0, qe, CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET) - - displaypath(robot, cube, path, cube_placements, dt=0.5, viz=viz) #you ll probably want to lower dt \ No newline at end of file + q0, successinit = computeqgrasppose(robot, q, cube, CUBE_PLACEMENT, viz=None) + qe, successend = computeqgrasppose(robot, q, cube, CUBE_PLACEMENT_TARGET, viz=None) + + if not (successinit and successend): + print("error: invalid initial or end configuration") + raise SystemExit(1) + + viz_obj = None if args.no_viz else viz + # choose sampler: either let computepath build the plane sampler, or build a smart sampler here + RANDOM_SAMPLER = None + REPAIR_SAMPLER = None + if args.sampler == 'smart': + # build a plane sampler to use as fallback + p0 = np.array(CUBE_PLACEMENT.translation) + p1 = np.array(CUBE_PLACEMENT_TARGET.translation) + + p0_xy = p0[:2] + p1_xy = p1[:2] + seg = p1_xy - p0_xy + L = np.linalg.norm(seg) + if L < 1e-6: + u = np.array([1.0, 0.0]) + else: + u = seg / L + perp = np.array([-u[1], u[0]]) + + half_width = DEFAULT_PLANE_HALF_WIDTH + zmin, zmax = DEFAULT_TABLE_Z_RANGE + + def plane_sampler(): + while True: + s = np.random.uniform(0.0, 1.0) + t = np.random.uniform(-half_width, half_width) + xy = (1.0 - s) * p0_xy + s * p1_xy + perp * t + z = np.random.uniform(zmin, zmax) + placement = pin.SE3(np.eye(3), np.array([xy[0], xy[1], z])) + setcubeplacement(robot, cube, placement) + has_collision = pin.computeCollisions(cube.collision_model, cube.collision_data, False) + if not has_collision: + return placement + + # If --smart-global selected, use the smart sampler as the global + # sampler; otherwise keep the plane sampler global and use smart as a + # local repair strategy. This keeps exploration simple by default but + # lets the user switch behaviour. + if args.smart_global: + RANDOM_SAMPLER = sampling_module.make_smart_midpoint_sampler( + robot=robot, + cube=cube, + p0=CUBE_PLACEMENT, + p1=CUBE_PLACEMENT_TARGET, + initial_sigma=args.smart_init_sigma, + sigma_scale=args.smart_scale, + max_sigma=args.smart_max_sigma, + max_attempts=args.smart_max_attempts, + fallback_sampler=plane_sampler, + anisotropic=True, + z_range=DEFAULT_TABLE_Z_RANGE, + viz=(None if args.no_viz else viz), + viz_delay=args.viz_delay, + ) + REPAIR_SAMPLER = None + else: + # use the plane sampler as the global sampler and the smart sampler as a + # local repair strategy. This keeps global exploration simple while the + # smart sampler is used to attempt local fixes when an extension fails. + RANDOM_SAMPLER = plane_sampler + + # create the smart sampler closure (used as repair_sampler) + REPAIR_SAMPLER = sampling_module.make_smart_midpoint_sampler( + robot=robot, + cube=cube, + p0=CUBE_PLACEMENT, + p1=CUBE_PLACEMENT_TARGET, + initial_sigma=args.smart_init_sigma, + sigma_scale=args.smart_scale, + max_sigma=args.smart_max_sigma, + max_attempts=args.smart_max_attempts, + fallback_sampler=plane_sampler, + anisotropic=True, + z_range=DEFAULT_TABLE_Z_RANGE, + viz=(None if args.no_viz else viz), + viz_delay=args.viz_delay, + ) + + path, cube_placements = computepath( + q0, qe, CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET, + robot=robot, + cube=cube, + computeqgrasppose=computeqgrasppose, + num_iter=args.num_iter, + discretisation_steps=args.discretisation, + random_sampler=RANDOM_SAMPLER, + repair_sampler=REPAIR_SAMPLER, + viz=viz_obj, + viz_delay=args.viz_delay, + max_ik_attempts=args.max_ik_attempts, + ) + + # optionally playback the final path. If --playback is set we show the + # final trajectory even when planning was run headless with --no-viz. + if path and (args.playback or not args.no_viz): + # ensure visualiser is ready (short pause) then play back slowly + try: + time.sleep(0.5) + displaypath(robot, cube, path, cube_placements, dt=0.5, viz=viz) + except Exception: + # Best-effort: if viz isn't available or playback fails, continue. + logger.exception('Playback failed') + + if hasattr(rrt := __import__('rrt'), 'rrt'): + # noop — ensure rrt module imported so metrics are available in namespace + pass + + # if rrt metrics exist log them + try: + import rrt as _rrt + if hasattr(_rrt, 'RRT') and hasattr(_rrt, 'construct_rrt'): + # rrt.metrics is attached to the returned rrt instance; cannot access it here unless we returned it + pass + except Exception: + pass + + print('Done') \ No newline at end of file diff --git a/rrt.py b/rrt.py new file mode 100644 index 0000000..e954d72 --- /dev/null +++ b/rrt.py @@ -0,0 +1,425 @@ +#!/usr/bin/env python3 +"""Standard RRT implementation for cube placement planning. + +This module contains a focused RRT implementation that works with cube +placements (pin.SE3) and optional grasping-pose callbacks. The implementation +is intentionally minimal and readable. +""" +from typing import Callable, Tuple, List, Optional + +import logging +import numpy as np +import pinocchio as pin +from pinocchio.utils import rotate + +from constants import EDGE_DISTANCE_TOL, DEFAULT_DISCRETISATION_STEPS, DEFAULT_CHECK_EDGE_STEPS +from tools import setcubeplacement +from constants import DEFAULT_VIZ_DELAY +import time +import sampling + +logger = logging.getLogger(__name__) + + +from dataclasses import dataclass + + +@dataclass +class Vertex: + q: pin.SE3 + grasping_q: Optional[np.ndarray] + parent: Optional['Vertex'] = None + + +class RRT: + """A small, explicit RRT tree that stores vertices of cube placements. + + Attributes: + nodes: list of Vertex objects, root at index 0. + """ + + def __init__(self, + root_q: pin.SE3, + root_grasping_q: Optional[np.ndarray], + robot: pin.robot_wrapper.RobotWrapper, + cube: pin.robot_wrapper.RobotWrapper, + get_grasping_poseq: Optional[Callable] = None, + viz=None, + viz_delay: float = DEFAULT_VIZ_DELAY, + discretisation_steps: int = DEFAULT_DISCRETISATION_STEPS, + max_ik_attempts: Optional[int] = None): + + self.robot = robot + self.cube = cube + self.get_grasping_poseq = get_grasping_poseq + + self.nodes: List[Vertex] = [Vertex(q=root_q, grasping_q=root_grasping_q, parent=None)] + # store common runtime parameters on the RRT instance so methods don't + # need to be passed them repeatedly + self.viz = viz + self.viz_delay = viz_delay + self.discretisation_steps = discretisation_steps + self.max_ik_attempts = max_ik_attempts + # profiling counters for IK, viz and edge checks + self._profile = { + 'ik_calls': 0, + 'time_ik': 0.0, + 'viz_calls': 0, + 'time_viz': 0.0, + 'edge_checks': 0, + 'time_edge_checks': 0.0, + } + + @staticmethod + def calc_distance(q1, q2) -> float: + """Euclidean distance between two translations or SE3s.""" + if hasattr(q1, 'translation'): + t1 = np.array(q1.translation) + else: + t1 = np.array(q1) + + if hasattr(q2, 'translation'): + t2 = np.array(q2.translation) + else: + t2 = np.array(q2) + + return np.linalg.norm(t2 - t1) + + @staticmethod + def lerp(q0, q1, t: float): + """Linear interpolation of translations and return a pin.SE3 placement.""" + if hasattr(q0, 'translation'): + a = np.array(q0.translation) + else: + a = np.array(q0) + + if hasattr(q1, 'translation'): + b = np.array(q1.translation) + else: + b = np.array(q1) + + trans = (1 - t) * a + t * b + return pin.SE3(rotate('z', 0.0), trans) + + def add_edge(self, parent_idx: int, new_q: pin.SE3, new_grasping_q: Optional[np.ndarray]) -> None: + self.nodes.append(Vertex(q=new_q, grasping_q=new_grasping_q, parent=self.nodes[parent_idx])) + + def get_nearest_vertex(self, q: pin.SE3) -> Tuple[int, Vertex]: + nearest_idx = 0 + smallest = float('inf') + for i, v in enumerate(self.nodes): + dist = self.calc_distance(v.q, q) + if dist < smallest: + smallest = dist + nearest_idx = i + return nearest_idx, self.nodes[nearest_idx] + + def get_q_new(self, + q_nearest_vertex: Vertex, + q_rand: pin.SE3, + discretisation_steps: Optional[int] = None, + max_delta_q: Optional[float] = None) -> Tuple[pin.SE3, Optional[np.ndarray], bool]: + + q_new_found = True + q_end = q_rand + + dist = self.calc_distance(q_nearest_vertex.q, q_rand) + if max_delta_q is not None and dist > max_delta_q: + q_end = self.lerp(q_nearest_vertex.q, q_rand, max_delta_q / dist) + + grasping_q = None + if self.get_grasping_poseq is not None: + # allow caller to override discretisation for this call; otherwise use instance default + discretisation_steps = discretisation_steps if discretisation_steps is not None else self.discretisation_steps + dt = 1.0 / discretisation_steps + # seed IK with the nearest vertex grasping pose and then update the seed + seed_grasping_q = q_nearest_vertex.grasping_q + for i in range(1, discretisation_steps): + q = self.lerp(q_nearest_vertex.q, q_end, dt * i) + # optionally update visualization so user sees the robot attempt + if self.viz is not None: + t_v = time.perf_counter() + setcubeplacement(self.robot, self.cube, q) + q_disp = getattr(self.robot, 'q', None) + if q_disp is not None: + self.viz.display(q_disp) + else: + if hasattr(self.viz, 'render'): + self.viz.render() + dt_v = time.perf_counter() - t_v + self._profile['viz_calls'] += 1 + self._profile['time_viz'] += dt_v + time.sleep(self.viz_delay) + + # Use the IK wrapper to call the IK callback in a + # backwards-compatible way and time the call. We set + # viz_sleep=False so display calls (if any) don't sleep. + from ik_utils import call_ik + ik_kwargs = dict( + robot=self.robot, + qcurrent=seed_grasping_q, + cube=self.cube, + cubetarget=q, + viz=(self.viz if self.viz is not None else None), + max_attempts=(self.max_ik_attempts if self.max_ik_attempts is not None else 1000), + viz_sleep=False, + ) + (grasping_q, found), dt_ik = call_ik(self.get_grasping_poseq, **ik_kwargs) + self._profile['ik_calls'] += 1 + self._profile['time_ik'] += dt_ik + # update seed for the next interpolation step if IK succeeded + if found and grasping_q is not None: + seed_grasping_q = grasping_q + if not found: + # cannot progress past the previous valid step + if i - 1 == 0: + q_new_found = False + return q_nearest_vertex.q, q_nearest_vertex.grasping_q, False + return self.lerp(q_nearest_vertex.q, q_end, dt * (i - 1)), grasping_q, True + return q_end, grasping_q, q_new_found + + def check_edge(self, q_latest_new: pin.SE3, q_latest_new_grasping_pose: Optional[np.ndarray], + q_goal: pin.SE3, discretisation_steps: Optional[int] = None) -> bool: + # Try to progress from q_latest_new to q_goal using get_q_new semantics + dummy_vertex = Vertex(q=q_latest_new, grasping_q=q_latest_new_grasping_pose, parent=None) + t_edge = time.perf_counter() + q_new, q_new_grasping_q, q_new_found = self.get_q_new( + q_nearest_vertex=dummy_vertex, + q_rand=q_goal, + discretisation_steps=discretisation_steps, + max_delta_q=None, + ) + dt_edge = time.perf_counter() - t_edge + self._profile['edge_checks'] += 1 + self._profile['time_edge_checks'] += dt_edge + if not q_new_found: + return False + distance_to_target = self.calc_distance(q_new, q_goal) + return distance_to_target < EDGE_DISTANCE_TOL + + def get_path_from_rrt(self) -> List[Vertex]: + path: List[Vertex] = [] + last = self.nodes[-1] + while last is not None: + path.insert(0, last) + last = last.parent + return path + + def get_shortcut_from_rrt_path(self, discretisation_steps: int = DEFAULT_DISCRETISATION_STEPS, + max_ik_attempts: Optional[int] = None) -> List[Vertex]: + path_vertices = self.get_path_from_rrt() + if len(path_vertices) <= 2: + return path_vertices + + short_path: List[Vertex] = [] + start_idx = 0 + while start_idx < len(path_vertices) - 1: + current_vertex = path_vertices[start_idx] + # Try to find the furthest vertex we can directly connect to + found_idx = None + for next_idx in range(len(path_vertices) - 1, start_idx, -1): + if self.check_edge(current_vertex.q, current_vertex.grasping_q, path_vertices[next_idx].q, + discretisation_steps=discretisation_steps): + found_idx = next_idx + break + if found_idx is None: + # no shortcut found; step by one + short_path.append(current_vertex) + start_idx += 1 + else: + short_path.append(current_vertex) + start_idx = found_idx + + # always append the final vertex + if path_vertices: + short_path.append(path_vertices[-1]) + return short_path + + +def construct_rrt(robot: pin.robot_wrapper.RobotWrapper, + cube: pin.robot_wrapper.RobotWrapper, + q_init: np.ndarray, + q_goal: np.ndarray, + cubeplacementq0: pin.SE3, + cubeplacementqgoal: pin.SE3, + num_iter: int, + random_sampler: Callable, + max_delta_q: Optional[float] = None, + discretisation_steps: int = DEFAULT_DISCRETISATION_STEPS, + get_grasping_poseq: Optional[Callable] = None, + viz=None, + viz_delay: float = DEFAULT_VIZ_DELAY, + max_ik_attempts: Optional[int] = None, + max_time_s: Optional[float] = None, + progress_log_every: int = 50, + repair_sampler: Optional[Callable] = None, + repair_max_attempts: int = 5, + goal_bias: float = 0.02) -> Optional[RRT]: + """Construct a simple RRT that samples cube placements using `random_sampler`. + + This is intentionally straightforward: we sample, extend toward the sample, + and stop when we can connect to the goal. + """ + rrt = RRT(root_q=cubeplacementq0, root_grasping_q=q_init, robot=robot, cube=cube, + get_grasping_poseq=get_grasping_poseq) + + # Metrics for logging + total_samples = 0 + successful_extensions = 0 + start_time = time.perf_counter() + # reset sampling module metrics for this run + try: + sampling.metrics = { + 'samples': 0, + 'geom_updates': 0, + 'collision_checks': 0, + 'time_collision_checks': 0.0, + 'time_viz': 0.0, + 'time_sampling_loop': 0.0, + } + except Exception: + pass + + for i in range(num_iter): + # Periodic progress logging so long runs show activity + if progress_log_every and i % progress_log_every == 0 and i > 0: + elapsed_now = time.perf_counter() - start_time + samples_per_s_now = total_samples / elapsed_now if elapsed_now > 0 else float('inf') + logger.info("RRT progress: %d/%d iterations, samples: %d, elapsed: %.2fs, samples/s: %.2f", + i, num_iter, total_samples, elapsed_now, samples_per_s_now) + + # Abort the run if we've exceeded a user-specified wall-time budget + if max_time_s is not None: + elapsed_now = time.perf_counter() - start_time + if elapsed_now > max_time_s: + logger.warning("RRT aborted after %.2fs (max_time_s=%.2fs) — returning no path", elapsed_now, max_time_s) + # include profiling into rrt.metrics before returning + try: + rrt.metrics = { + 'total_samples': total_samples, + 'successful_extensions': successful_extensions, + 'iterations': i, + 'time_s': elapsed_now, + 'samples_per_s': total_samples / elapsed_now if elapsed_now > 0 else float('inf'), + 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, + } + rrt.metrics['profiling'] = rrt._profile.copy() + rrt.metrics['sampling'] = sampling.metrics.copy() + except Exception: + pass + return None + # With small probability sample the goal directly (goal bias) to + # encourage connection attempts. + import random as _random + if goal_bias and _random.random() < goal_bias: + random_cp_q = cubeplacementqgoal + else: + total_samples += 1 + random_cp_q = random_sampler() + nearest_idx, nearest_vertex = rrt.get_nearest_vertex(random_cp_q) + + cp_q_new, cp_q_new_grasping_pose, cp_q_new_found = rrt.get_q_new( + q_nearest_vertex=nearest_vertex, + q_rand=random_cp_q, + discretisation_steps=discretisation_steps, + max_delta_q=max_delta_q, + ) + + # If the extension succeeded use it. Otherwise, optionally try a + # local repair sampler (smart sampler) a few times before continuing. + if not cp_q_new_found and repair_sampler is not None: + for r_attempt in range(repair_max_attempts): + total_samples += 1 + try: + repair_target = repair_sampler() + except Exception: + # if the repair sampler misbehaves, stop trying repairs + break + nearest_idx, nearest_vertex = rrt.get_nearest_vertex(repair_target) + cp_q_new, cp_q_new_grasping_pose, cp_q_new_found = rrt.get_q_new( + q_nearest_vertex=nearest_vertex, + q_rand=repair_target, + discretisation_steps=discretisation_steps, + max_delta_q=max_delta_q, + ) + if cp_q_new_found: + successful_extensions += 1 + rrt.add_edge(parent_idx=nearest_idx, new_q=cp_q_new, new_grasping_q=cp_q_new_grasping_pose) + # check connection to goal and return if found + if rrt.check_edge(cp_q_new, cp_q_new_grasping_pose, cubeplacementqgoal, + discretisation_steps=discretisation_steps): + rrt.add_edge(parent_idx=len(rrt.nodes) - 1, new_q=cubeplacementqgoal, new_grasping_q=q_goal) + elapsed = time.perf_counter() - start_time + # attach metrics to rrt for later inspection + rrt.metrics = { + 'total_samples': total_samples, + 'successful_extensions': successful_extensions, + 'iterations': i + 1, + 'time_s': elapsed, + 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), + 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, + } + try: + rrt.metrics['profiling'] = rrt._profile.copy() + except Exception: + rrt.metrics['profiling'] = {} + try: + rrt.metrics['sampling'] = sampling.metrics.copy() + except Exception: + rrt.metrics['sampling'] = {} + logger.info("RRT found a path after %d iterations (%.3fs)", i + 1, elapsed) + logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", + total_samples, successful_extensions, rrt.metrics['success_rate'], rrt.metrics['samples_per_s']) + return rrt + break + + if cp_q_new_found: + successful_extensions += 1 + rrt.add_edge(parent_idx=nearest_idx, new_q=cp_q_new, new_grasping_q=cp_q_new_grasping_pose) + + if rrt.check_edge(cp_q_new, cp_q_new_grasping_pose, cubeplacementqgoal, discretisation_steps=discretisation_steps): + rrt.add_edge(parent_idx=len(rrt.nodes) - 1, new_q=cubeplacementqgoal, new_grasping_q=q_goal) + elapsed = time.perf_counter() - start_time + # attach metrics to rrt for later inspection + rrt.metrics = { + 'total_samples': total_samples, + 'successful_extensions': successful_extensions, + 'iterations': i + 1, + 'time_s': elapsed, + 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), + 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, + } + # attach profiling collected by the RRT and sampling modules + try: + rrt.metrics['profiling'] = rrt._profile.copy() + except Exception: + rrt.metrics['profiling'] = {} + try: + rrt.metrics['sampling'] = sampling.metrics.copy() + except Exception: + rrt.metrics['sampling'] = {} + logger.info("RRT found a path after %d iterations (%.3fs)", i + 1, elapsed) + logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", + total_samples, successful_extensions, rrt.metrics['success_rate'], rrt.metrics['samples_per_s']) + return rrt + + elapsed = time.perf_counter() - start_time + # include profiling even on failure + try: + rrt.metrics = { + 'total_samples': total_samples, + 'successful_extensions': successful_extensions, + 'iterations': num_iter, + 'time_s': elapsed, + 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), + 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, + } + rrt.metrics['profiling'] = rrt._profile.copy() + rrt.metrics['sampling'] = sampling.metrics.copy() + except Exception: + pass + logger.info("RRT completed %d iterations without connecting to goal (%.3fs)", num_iter, elapsed) + logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", + total_samples, successful_extensions, (successful_extensions / total_samples) if total_samples > 0 else 0.0, + total_samples / elapsed if elapsed > 0 else float('inf')) + return None diff --git a/sampling.py b/sampling.py new file mode 100644 index 0000000..fcd8a8e --- /dev/null +++ b/sampling.py @@ -0,0 +1,263 @@ +#!/usr/bin/env python3 +"""Sampling helpers for cube placement on a table. + +This module contains pure sampling logic (no RRT) so it can be read, +tested, and understood independently. +""" +from typing import Tuple, Callable + +import logging +import numpy as np +import pinocchio as pin +from pinocchio.utils import rotate + +from tools import setcubeplacement +from constants import DEFAULT_TABLE_Z_RANGE, DEFAULT_CHECK_COLLISIONS, DEFAULT_VIZ_DELAY, SAMPLE_DISPLAY_EVERY +import time + +logger = logging.getLogger(__name__) + +# Lightweight profiling counters populated during sampling calls. +# Accessible as sampling.metrics after running the sampler. +metrics = { + 'samples': 0, + 'geom_updates': 0, + 'collision_checks': 0, + 'time_collision_checks': 0.0, + 'time_viz': 0.0, + 'time_sampling_loop': 0.0, +} + + +def get_table_borders(table: pin.robot_wrapper.RobotWrapper) -> Tuple[np.array, np.array]: + """Return the axis-aligned bounding box (min, max) of the table collision surface. + + Args: + table: pinocchio RobotWrapper for the table which contains geometry information. + + Returns: + (xyz_min, xyz_max): two length-3 numpy arrays with world-frame corner coordinates. + """ + # ensure geometry placements are up-to-date + pin.updateGeometryPlacements(table.model, table.data, table.collision_model, table.collision_data) + table_surface = table.collision_model.geometryObjects[0].geometry + oMtable_surface = table.collision_data.oMg[0] + R, t = oMtable_surface.rotation, oMtable_surface.translation + + half = table_surface.halfSide + local_corners = np.array([[sx, sy, sz] + for sx in [+1, -1] + for sy in [+1, -1] + for sz in [+1, -1]]) * half + world_corners = (R @ local_corners.T).T + t + + xyz_min = world_corners.min(axis=0) + xyz_max = world_corners.max(axis=0) + + logger.debug("Table borders: min=%s max=%s", xyz_min, xyz_max) + return xyz_min, xyz_max + + +def random_cube_placement( + robot: pin.robot_wrapper.RobotWrapper, + cube: pin.robot_wrapper.RobotWrapper, + x_range: Tuple[float, float], + y_range: Tuple[float, float], + z_range: Tuple[float, float] = DEFAULT_TABLE_Z_RANGE, + check_collisions: bool = DEFAULT_CHECK_COLLISIONS, + sampler: Callable[[], float] = None, + viz=None, + viz_delay: float = DEFAULT_VIZ_DELAY, + display_every: int = SAMPLE_DISPLAY_EVERY, + collision_check_every: int = 1, +) -> pin.SE3: + """Sample a random placement (SE3) for the cube on the table surface. + + The function places the cube at a random (x,y) inside the table AABB and a + z sampled from `z_range`. It sets the cube placement on the robot via + `setcubeplacement` and optionally checks for collisions. + + Args: + robot, cube, table: pinocchio wrappers used for updating placements. + check_collisions: if True, verify the placement is collision-free. + z_range: tuple (z_min, z_max) for sampling height above world origin. + sampler: optional callable returning a float in [0,1] used to sample x/y. + + Returns: + A pin.SE3 placement that is collision-free (if check_collisions) or the + last sampled placement if checks are disabled. + """ + sampler = sampler or np.random.random + + sample_count = 0 + last_placement = None + while True: + x = np.random.uniform(x_range[0], x_range[1]) + y = np.random.uniform(y_range[0], y_range[1]) + z = np.random.uniform(z_range[0], z_range[1]) + + t_loop = time.perf_counter() + placement = pin.SE3(rotate('z', 0.0), np.array([x, y, z])) + last_placement = placement + + sample_count += 1 + metrics['samples'] += 1 + + # only update the visualiser occasionally to avoid blocking + if viz is not None and (sample_count % display_every) == 0: + t_v = time.perf_counter() + q_disp = getattr(robot, 'q', None) + if q_disp is not None: + viz.display(q_disp) + else: + if hasattr(viz, 'render'): + viz.render() + dt_v = time.perf_counter() - t_v + metrics['time_viz'] += dt_v + time.sleep(viz_delay) + + if not check_collisions: + metrics['time_sampling_loop'] += time.perf_counter() - t_loop + return last_placement + + # perform the (more expensive) geometry update and collision check only every N samples + if (sample_count % max(1, collision_check_every)) != 0: + # don't call geometry updates for this sample, keep sampling + metrics['time_sampling_loop'] += time.perf_counter() - t_loop + continue + + # now set placement on the scene and do collision checking + from tools import set_and_check_cube_placement + metrics['geom_updates'] += 1 + has_collision, dt_coll = set_and_check_cube_placement(robot, cube, last_placement) + metrics['collision_checks'] += 1 + metrics['time_collision_checks'] += dt_coll + metrics['time_sampling_loop'] += time.perf_counter() - t_loop + if not has_collision: + logger.debug("Found collision-free placement after %d samples", sample_count) + return last_placement + # otherwise keep sampling + logger.debug("Sampled placement in collision at sample %d, retrying", sample_count) + + +def make_smart_midpoint_sampler(robot: pin.robot_wrapper.RobotWrapper, + cube: pin.robot_wrapper.RobotWrapper, + p0: pin.SE3, + p1: pin.SE3, + initial_sigma: float = 0.02, + sigma_scale: float = 1.5, + max_sigma: float = 0.2, + max_attempts: int = 30, + fallback_sampler: Callable[[], pin.SE3] = None, + anisotropic: bool = True, + z_range: Tuple[float, float] = DEFAULT_TABLE_Z_RANGE, + viz=None, + viz_delay: float = DEFAULT_VIZ_DELAY) -> Callable[[], pin.SE3]: + """Create a sampler that prefers the straight-line midpoint between p0 and p1. + + The returned callable takes no arguments and returns a collision-free + pin.SE3 placement. It first tries the exact midpoint, then samples from a + Gaussian around the midpoint with adaptive variance inflation until a + collision-free placement is found or max_attempts is reached. If nothing + found, calls fallback_sampler if provided. + """ + # Precompute useful values + p0_xy = np.array(p0.translation)[:2] + p1_xy = np.array(p1.translation)[:2] + mid_xy = (p0_xy + p1_xy) / 2.0 + mid_z = (np.array(p0.translation)[2] + np.array(p1.translation)[2]) / 2.0 + + seg = p1_xy - p0_xy + L = np.linalg.norm(seg) + if L < 1e-6: + u = np.array([1.0, 0.0]) + else: + u = seg / L + perp = np.array([-u[1], u[0]]) + + def sampler() -> pin.SE3: + # try exact midpoint first + placement = pin.SE3(np.eye(3), np.array([mid_xy[0], mid_xy[1], mid_z])) + # do a timed collision check for the midpoint + from tools import set_and_check_cube_placement + metrics['geom_updates'] += 1 + has_collision, dt = set_and_check_cube_placement(robot, cube, placement) + metrics['collision_checks'] += 1 + metrics['time_collision_checks'] += dt + if not has_collision: + return placement + + # deterministic-offset-first: try a small set of deterministic offsets + # (along the segment and perpendicular) before doing stochastic sampling. + attempts_total = 1 + # choose a small grid of offsets based on initial_sigma + base_eps = max(1e-4, initial_sigma) + eps_factors = [0.5, 1.0, 2.0] + det_offsets = [] + for f in eps_factors: + eps = base_eps * f + # along, perp, and diagonal combinations + det_offsets.extend([ + (eps, 0.0), (-eps, 0.0), + (0.0, eps), (0.0, -eps), + (eps, eps), (eps, -eps), (-eps, eps), (-eps, -eps) + ]) + + for along_eps, perp_eps in det_offsets: + if attempts_total >= max_attempts: + break + xy = mid_xy + u * along_eps + perp * perp_eps + # bias z toward midpoint + zmin, zmax = z_range + z = np.clip(np.random.normal(mid_z, initial_sigma * 0.5), zmin, zmax) + placement = pin.SE3(np.eye(3), np.array([xy[0], xy[1], z])) + from tools import set_and_check_cube_placement + metrics['geom_updates'] += 1 + has_collision, dt = set_and_check_cube_placement(robot, cube, placement) + metrics['collision_checks'] += 1 + metrics['time_collision_checks'] += dt + attempts_total += 1 + if not has_collision: + logger.debug("Smart sampler found placement on deterministic offset after %d attempts", attempts_total) + return placement + + sigma = initial_sigma + attempts = attempts_total + while attempts < max_attempts: + attempts += 1 + # anisotropic sampling in the xy-plane: smaller along segment, larger perpendicular + if anisotropic: + sigma_along = max(1e-6, sigma * 0.5) + sigma_perp = sigma + r_along = np.random.normal(0.0, sigma_along) + r_perp = np.random.normal(0.0, sigma_perp) + xy = mid_xy + u * r_along + perp * r_perp + else: + xy = mid_xy + np.random.normal(0.0, sigma, size=2) + + # sample z from the provided z_range, but bias toward midpoint z + zmin, zmax = z_range + z = np.clip(np.random.normal(mid_z, sigma), zmin, zmax) + + placement = pin.SE3(np.eye(3), np.array([xy[0], xy[1], z])) + from tools import set_and_check_cube_placement + metrics['geom_updates'] += 1 + has_collision, dt = set_and_check_cube_placement(robot, cube, placement) + metrics['collision_checks'] += 1 + metrics['time_collision_checks'] += dt + if not has_collision: + logger.debug("Smart sampler found placement after %d attempts (sigma=%.4f)", attempts, sigma) + return placement + + # increase variance and try again + sigma = min(max_sigma, sigma * sigma_scale) + + # fallback if provided + if fallback_sampler is not None: + logger.debug("Smart sampler failing after %d attempts; using fallback sampler", attempts) + return fallback_sampler() + + # as a last resort return the midpoint (may be colliding) + return placement + + return sampler diff --git a/tools.py b/tools.py index fc9e0da..9ad021c 100644 --- a/tools.py +++ b/tools.py @@ -77,7 +77,7 @@ def setupwithmeshcat(url=MESHCAT_URL): '''setups everything to work with the robot and meshcat''' robot, table, obstacle, cube = setuppinocchio() viz = setupmeshcat(robot, url) - return robot, cube, table, viz + return robot, cube, viz from setup_pybullet import setuppybullet def setupwithpybullet(): @@ -85,7 +85,7 @@ def setupwithpybullet(): robot, table, obstacle, cube = setuppinocchio() sim = setuppybullet(robot) sim.setTorqueControlMode() - return robot, sim, table, cube + return robot, sim, cube def setupwithpybulletandmeshcat(url=MESHCAT_URL): From 088743e80c808db58d921d66f28d3c0768b8982c Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Sun, 9 Nov 2025 13:55:21 +0000 Subject: [PATCH 08/23] Mid RRT refactor --- path.py | 20 ++- rrt.py | 459 ++++++++++++++++++++++++++++++++------------------------ 2 files changed, 276 insertions(+), 203 deletions(-) diff --git a/path.py b/path.py index 4344c3f..38df116 100644 --- a/path.py +++ b/path.py @@ -115,12 +115,21 @@ def plane_sampler(): print('Searching for a valid path...(this may take some time)') # (no sampling-box visualization) - rrt = construct_rrt( - robot=robot, - cube=cube, + # Instantiate RRT with desired runtime parameters then run it. This uses + # the newer instance API so callers can inspect the tree if desired. + rrt = RRT(root_q=cubeplacementq0, + root_grasping_q=qinit, + robot=robot, + cube=cube, + get_grasping_poseq=GET_GRAPSING_POSEQ, + viz=viz, + viz_delay=viz_delay, + discretisation_steps=discretisation_steps, + max_ik_attempts=max_ik_attempts) + + rrt = rrt.run( q_init=qinit, q_goal=qgoal, - cubeplacementq0=cubeplacementq0, cubeplacementqgoal=cubeplacementqgoal, num_iter=num_iter, random_sampler=RANDOM_SAMPLER, @@ -130,9 +139,6 @@ def plane_sampler(): max_delta_q=MAX_DELTA_Q, discretisation_steps=discretisation_steps, get_grasping_poseq=GET_GRAPSING_POSEQ, - viz=viz, - viz_delay=viz_delay, - max_ik_attempts=max_ik_attempts, max_time_s=None, progress_log_every=50, ) diff --git a/rrt.py b/rrt.py index e954d72..5c6d9dd 100644 --- a/rrt.py +++ b/rrt.py @@ -9,39 +9,6 @@ import logging import numpy as np -import pinocchio as pin -from pinocchio.utils import rotate - -from constants import EDGE_DISTANCE_TOL, DEFAULT_DISCRETISATION_STEPS, DEFAULT_CHECK_EDGE_STEPS -from tools import setcubeplacement -from constants import DEFAULT_VIZ_DELAY -import time -import sampling - -logger = logging.getLogger(__name__) - - -from dataclasses import dataclass - - -@dataclass -class Vertex: - q: pin.SE3 - grasping_q: Optional[np.ndarray] - parent: Optional['Vertex'] = None - - -class RRT: - """A small, explicit RRT tree that stores vertices of cube placements. - - Attributes: - nodes: list of Vertex objects, root at index 0. - """ - - def __init__(self, - root_q: pin.SE3, - root_grasping_q: Optional[np.ndarray], - robot: pin.robot_wrapper.RobotWrapper, cube: pin.robot_wrapper.RobotWrapper, get_grasping_poseq: Optional[Callable] = None, viz=None, @@ -69,172 +36,249 @@ def __init__(self, 'edge_checks': 0, 'time_edge_checks': 0.0, } - - @staticmethod - def calc_distance(q1, q2) -> float: - """Euclidean distance between two translations or SE3s.""" - if hasattr(q1, 'translation'): - t1 = np.array(q1.translation) - else: - t1 = np.array(q1) - - if hasattr(q2, 'translation'): - t2 = np.array(q2.translation) - else: - t2 = np.array(q2) - - return np.linalg.norm(t2 - t1) - - @staticmethod - def lerp(q0, q1, t: float): - """Linear interpolation of translations and return a pin.SE3 placement.""" - if hasattr(q0, 'translation'): - a = np.array(q0.translation) - else: - a = np.array(q0) - - if hasattr(q1, 'translation'): - b = np.array(q1.translation) - else: - b = np.array(q1) - - trans = (1 - t) * a + t * b - return pin.SE3(rotate('z', 0.0), trans) - - def add_edge(self, parent_idx: int, new_q: pin.SE3, new_grasping_q: Optional[np.ndarray]) -> None: - self.nodes.append(Vertex(q=new_q, grasping_q=new_grasping_q, parent=self.nodes[parent_idx])) - - def get_nearest_vertex(self, q: pin.SE3) -> Tuple[int, Vertex]: - nearest_idx = 0 - smallest = float('inf') - for i, v in enumerate(self.nodes): - dist = self.calc_distance(v.q, q) - if dist < smallest: - smallest = dist - nearest_idx = i - return nearest_idx, self.nodes[nearest_idx] - - def get_q_new(self, - q_nearest_vertex: Vertex, - q_rand: pin.SE3, - discretisation_steps: Optional[int] = None, - max_delta_q: Optional[float] = None) -> Tuple[pin.SE3, Optional[np.ndarray], bool]: - - q_new_found = True - q_end = q_rand - - dist = self.calc_distance(q_nearest_vertex.q, q_rand) - if max_delta_q is not None and dist > max_delta_q: - q_end = self.lerp(q_nearest_vertex.q, q_rand, max_delta_q / dist) - - grasping_q = None - if self.get_grasping_poseq is not None: - # allow caller to override discretisation for this call; otherwise use instance default - discretisation_steps = discretisation_steps if discretisation_steps is not None else self.discretisation_steps - dt = 1.0 / discretisation_steps - # seed IK with the nearest vertex grasping pose and then update the seed - seed_grasping_q = q_nearest_vertex.grasping_q - for i in range(1, discretisation_steps): - q = self.lerp(q_nearest_vertex.q, q_end, dt * i) - # optionally update visualization so user sees the robot attempt - if self.viz is not None: - t_v = time.perf_counter() - setcubeplacement(self.robot, self.cube, q) - q_disp = getattr(self.robot, 'q', None) - if q_disp is not None: - self.viz.display(q_disp) - else: - if hasattr(self.viz, 'render'): - self.viz.render() - dt_v = time.perf_counter() - t_v - self._profile['viz_calls'] += 1 - self._profile['time_viz'] += dt_v - time.sleep(self.viz_delay) - - # Use the IK wrapper to call the IK callback in a - # backwards-compatible way and time the call. We set - # viz_sleep=False so display calls (if any) don't sleep. - from ik_utils import call_ik - ik_kwargs = dict( - robot=self.robot, - qcurrent=seed_grasping_q, - cube=self.cube, - cubetarget=q, - viz=(self.viz if self.viz is not None else None), - max_attempts=(self.max_ik_attempts if self.max_ik_attempts is not None else 1000), - viz_sleep=False, - ) - (grasping_q, found), dt_ik = call_ik(self.get_grasping_poseq, **ik_kwargs) - self._profile['ik_calls'] += 1 - self._profile['time_ik'] += dt_ik - # update seed for the next interpolation step if IK succeeded - if found and grasping_q is not None: - seed_grasping_q = grasping_q - if not found: - # cannot progress past the previous valid step - if i - 1 == 0: - q_new_found = False - return q_nearest_vertex.q, q_nearest_vertex.grasping_q, False - return self.lerp(q_nearest_vertex.q, q_end, dt * (i - 1)), grasping_q, True - return q_end, grasping_q, q_new_found - - def check_edge(self, q_latest_new: pin.SE3, q_latest_new_grasping_pose: Optional[np.ndarray], - q_goal: pin.SE3, discretisation_steps: Optional[int] = None) -> bool: - # Try to progress from q_latest_new to q_goal using get_q_new semantics - dummy_vertex = Vertex(q=q_latest_new, grasping_q=q_latest_new_grasping_pose, parent=None) - t_edge = time.perf_counter() - q_new, q_new_grasping_q, q_new_found = self.get_q_new( - q_nearest_vertex=dummy_vertex, - q_rand=q_goal, - discretisation_steps=discretisation_steps, - max_delta_q=None, - ) - dt_edge = time.perf_counter() - t_edge - self._profile['edge_checks'] += 1 - self._profile['time_edge_checks'] += dt_edge - if not q_new_found: - return False - distance_to_target = self.calc_distance(q_new, q_goal) - return distance_to_target < EDGE_DISTANCE_TOL - - def get_path_from_rrt(self) -> List[Vertex]: - path: List[Vertex] = [] - last = self.nodes[-1] - while last is not None: - path.insert(0, last) - last = last.parent - return path - - def get_shortcut_from_rrt_path(self, discretisation_steps: int = DEFAULT_DISCRETISATION_STEPS, - max_ik_attempts: Optional[int] = None) -> List[Vertex]: - path_vertices = self.get_path_from_rrt() - if len(path_vertices) <= 2: - return path_vertices - - short_path: List[Vertex] = [] - start_idx = 0 - while start_idx < len(path_vertices) - 1: - current_vertex = path_vertices[start_idx] - # Try to find the furthest vertex we can directly connect to - found_idx = None - for next_idx in range(len(path_vertices) - 1, start_idx, -1): - if self.check_edge(current_vertex.q, current_vertex.grasping_q, path_vertices[next_idx].q, - discretisation_steps=discretisation_steps): - found_idx = next_idx - break - if found_idx is None: - # no shortcut found; step by one - short_path.append(current_vertex) - start_idx += 1 - else: - short_path.append(current_vertex) - start_idx = found_idx - + # runtime parameters that control a run; populated by callers before + # invoking `run()` so the instance method can operate without args. + self.random_sampler = None + self.num_iter = None + self.max_delta_q = None + self.max_time_s = None + self.progress_log_every = 50 + self.repair_sampler = None + self.repair_max_attempts = 5 + self.goal_bias = 0.02 + # goal / initial placements and grasping configs + self.q_init = None + self.q_goal = None + self.cubeplacementqgoal = None + total_samples, successful_extensions, self.metrics['success_rate'], self.metrics['samples_per_s']) + return self + break + + if cp_q_new_found: + successful_extensions += 1 + self.add_edge(parent_idx=nearest_idx, new_q=cp_q_new, new_grasping_q=cp_q_new_grasping_pose) + + if self.check_edge(cp_q_new, cp_q_new_grasping_pose, cubeplacementqgoal, discretisation_steps=discretisation_steps): + self.add_edge(parent_idx=len(self.nodes) - 1, new_q=cubeplacementqgoal, new_grasping_q=q_goal) + elapsed = time.perf_counter() - start_time + # attach metrics to self for later inspection + self.metrics = { + 'total_samples': total_samples, + 'successful_extensions': successful_extensions, + 'iterations': i + 1, + 'time_s': elapsed, + 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), + 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, + } + # attach profiling collected by the RRT and sampling modules + try: + self.metrics['profiling'] = self._profile.copy() + except Exception: + self.metrics['profiling'] = {} + try: + self.metrics['sampling'] = sampling.metrics.copy() + except Exception: + self.metrics['sampling'] = {} + logger.info("RRT found a path after %d iterations (%.3fs)", i + 1, elapsed) + logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", + total_samples, successful_extensions, self.metrics['success_rate'], self.metrics['samples_per_s']) + return self + + elapsed = time.perf_counter() - start_time + # include profiling even on failure + try: + self.metrics = { + 'total_samples': total_samples, + 'successful_extensions': successful_extensions, + 'iterations': num_iter, + 'time_s': elapsed, + 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), + 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, + } + self.metrics['profiling'] = self._profile.copy() + self.metrics['sampling'] = sampling.metrics.copy() + except Exception: + pass + logger.info("RRT completed %d iterations without connecting to goal (%.3fs)", num_iter, elapsed) + logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", + total_samples, successful_extensions, (successful_extensions / total_samples) if total_samples > 0 else 0.0, + total_samples / elapsed if elapsed > 0 else float('inf')) + return None # always append the final vertex if path_vertices: short_path.append(path_vertices[-1]) return short_path + def run(self, + q_init: np.ndarray, + q_goal: np.ndarray, + cubeplacementqgoal: pin.SE3, + num_iter: int, + random_sampler: Callable, + max_delta_q: Optional[float] = None, + discretisation_steps: int = DEFAULT_DISCRETISATION_STEPS, + get_grasping_poseq: Optional[Callable] = None, + max_time_s: Optional[float] = None, + progress_log_every: int = 50, + repair_sampler: Optional[Callable] = None, + repair_max_attempts: int = 5, + goal_bias: float = 0.02) -> Optional['RRT']: + """Grow the RRT using the same algorithm formerly in construct_rrt. + + This is an instance method so callers can instantiate RRT(...) with + desired runtime parameters and then run the search. Returns self on + success, or None on timeout/failure. + """ + # Metrics for logging + total_samples = 0 + successful_extensions = 0 + start_time = time.perf_counter() + + # reset sampling module metrics for this run + try: + sampling.metrics = { + 'samples': 0, + 'geom_updates': 0, + 'collision_checks': 0, + 'time_collision_checks': 0.0, + 'time_viz': 0.0, + 'time_sampling_loop': 0.0, + } + except Exception: + pass + + for i in range(num_iter): + # Periodic progress logging so long runs show activity + if progress_log_every and i % progress_log_every == 0 and i > 0: + elapsed_now = time.perf_counter() - start_time + samples_per_s_now = total_samples / elapsed_now if elapsed_now > 0 else float('inf') + logger.info("RRT progress: %d/%d iterations, samples: %d, elapsed: %.2fs, samples/s: %.2f", + i, num_iter, total_samples, elapsed_now, samples_per_s_now) + + # Abort the run if we've exceeded a user-specified wall-time budget + if max_time_s is not None: + elapsed_now = time.perf_counter() - start_time + if elapsed_now > max_time_s: + logger.warning("RRT aborted after %.2fs (max_time_s=%.2fs) — returning no path", elapsed_now, max_time_s) + # include profiling into self.metrics before returning + try: + self.metrics = { + 'total_samples': total_samples, + 'successful_extensions': successful_extensions, + 'iterations': i, + 'time_s': elapsed_now, + 'samples_per_s': total_samples / elapsed_now if elapsed_now > 0 else float('inf'), + 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, + } + self.metrics['profiling'] = self._profile.copy() + self.metrics['sampling'] = sampling.metrics.copy() + except Exception: + pass + return None + + # With small probability sample the goal directly (goal bias) + import random as _random + if goal_bias and _random.random() < goal_bias: + random_cp_q = cubeplacementqgoal + else: + total_samples += 1 + random_cp_q = random_sampler() + + nearest_idx, nearest_vertex = self.get_nearest_vertex(random_cp_q) + def run(self) -> Optional['RRT']: + repair_target = repair_sampler() + except Exception: + # if the repair sampler misbehaves, stop trying repairs + break + nearest_idx, nearest_vertex = self.get_nearest_vertex(repair_target) + cp_q_new, cp_q_new_grasping_pose, cp_q_new_found = self.get_q_new( + q_nearest_vertex=nearest_vertex, + q_rand=repair_target, + discretisation_steps=(discretisation_steps if discretisation_steps is not None else self.discretisation_steps), + max_delta_q=max_delta_q, + ) + if cp_q_new_found: + successful_extensions += 1 + self.add_edge(parent_idx=nearest_idx, new_q=cp_q_new, new_grasping_q=cp_q_new_grasping_pose) + # check connection to goal and return if found + if self.check_edge(cp_q_new, cp_q_new_grasping_pose, cubeplacementqgoal, + discretisation_steps=(discretisation_steps if discretisation_steps is not None else self.discretisation_steps)): + self.add_edge(parent_idx=len(self.nodes) - 1, new_q=cubeplacementqgoal, new_grasping_q=q_goal) + elapsed = time.perf_counter() - start_time + # attach metrics to self for later inspection + self.metrics = { + 'total_samples': total_samples, + 'successful_extensions': successful_extensions, + 'iterations': i + 1, + 'time_s': elapsed, + 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), + 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, + } + try: + self.metrics['profiling'] = self._profile.copy() + except Exception: + self.metrics['profiling'] = {} + try: + self.metrics['sampling'] = sampling.metrics.copy() + except Exception: + self.metrics['sampling'] = {} + logger.info("RRT found a path after %d iterations (%.3fs)", i + 1, elapsed) + logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", + total_samples, successful_extensions, self.metrics['success_rate'], self.metrics['samples_per_s']) + return self + break + + if cp_q_new_found: + successful_extensions += 1 + self.add_edge(parent_idx=nearest_idx, new_q=cp_q_new, new_grasping_q=cp_q_new_grasping_pose) + + if self.check_edge(cp_q_new, cp_q_new_grasping_pose, cubeplacementqgoal, discretisation_steps=(discretisation_steps if discretisation_steps is not None else self.discretisation_steps)): + self.add_edge(parent_idx=len(self.nodes) - 1, new_q=cubeplacementqgoal, new_grasping_q=q_goal) + elapsed = time.perf_counter() - start_time + # attach metrics to self for later inspection + self.metrics = { + 'total_samples': total_samples, + 'successful_extensions': successful_extensions, + 'iterations': i + 1, + 'time_s': elapsed, + 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), + 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, + } + # attach profiling collected by the RRT and sampling modules + try: + self.metrics['profiling'] = self._profile.copy() + except Exception: + self.metrics['profiling'] = {} + try: + self.metrics['sampling'] = sampling.metrics.copy() + except Exception: + self.metrics['sampling'] = {} + logger.info("RRT found a path after %d iterations (%.3fs)", i + 1, elapsed) + logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", + total_samples, successful_extensions, self.metrics['success_rate'], self.metrics['samples_per_s']) + return self + + elapsed = time.perf_counter() - start_time + # include profiling even on failure + try: + self.metrics = { + 'total_samples': total_samples, + 'successful_extensions': successful_extensions, + 'iterations': num_iter, + 'time_s': elapsed, + 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), + 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, + } + self.metrics['profiling'] = self._profile.copy() + self.metrics['sampling'] = sampling.metrics.copy() + except Exception: + pass + logger.info("RRT completed %d iterations without connecting to goal (%.3fs)", num_iter, elapsed) + logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", + total_samples, successful_extensions, (successful_extensions / total_samples) if total_samples > 0 else 0.0, + total_samples / elapsed if elapsed > 0 else float('inf')) + return None + def construct_rrt(robot: pin.robot_wrapper.RobotWrapper, cube: pin.robot_wrapper.RobotWrapper, @@ -260,8 +304,31 @@ def construct_rrt(robot: pin.robot_wrapper.RobotWrapper, This is intentionally straightforward: we sample, extend toward the sample, and stop when we can connect to the goal. """ - rrt = RRT(root_q=cubeplacementq0, root_grasping_q=q_init, robot=robot, cube=cube, - get_grasping_poseq=get_grasping_poseq) + rrt = RRT(root_q=cubeplacementq0, + root_grasping_q=q_init, + robot=robot, + cube=cube, + get_grasping_poseq=get_grasping_poseq, + viz=viz, + viz_delay=viz_delay, + discretisation_steps=discretisation_steps, + max_ik_attempts=max_ik_attempts) + + # Delegate the main loop to the RRT instance; keep this wrapper for + # backwards compatibility. + return rrt.run(q_init=q_init, + q_goal=q_goal, + cubeplacementqgoal=cubeplacementqgoal, + num_iter=num_iter, + random_sampler=random_sampler, + max_delta_q=max_delta_q, + discretisation_steps=discretisation_steps, + get_grasping_poseq=get_grasping_poseq, + max_time_s=max_time_s, + progress_log_every=progress_log_every, + repair_sampler=repair_sampler, + repair_max_attempts=repair_max_attempts, + goal_bias=goal_bias) # Metrics for logging total_samples = 0 From c152ffcf7b20616af75c34a720dd145b84874302 Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Sun, 9 Nov 2025 14:43:14 +0000 Subject: [PATCH 09/23] Continue refactor --- path.py | 30 ++-- rrt.py | 542 +++++++++++++++++++++++++++----------------------------- 2 files changed, 276 insertions(+), 296 deletions(-) diff --git a/path.py b/path.py index 38df116..bd3f019 100644 --- a/path.py +++ b/path.py @@ -125,23 +125,19 @@ def plane_sampler(): viz=viz, viz_delay=viz_delay, discretisation_steps=discretisation_steps, - max_ik_attempts=max_ik_attempts) - - rrt = rrt.run( - q_init=qinit, - q_goal=qgoal, - cubeplacementqgoal=cubeplacementqgoal, - num_iter=num_iter, - random_sampler=RANDOM_SAMPLER, - repair_sampler=repair_sampler, - repair_max_attempts=repair_max_attempts, - goal_bias=goal_bias, - max_delta_q=MAX_DELTA_Q, - discretisation_steps=discretisation_steps, - get_grasping_poseq=GET_GRAPSING_POSEQ, - max_time_s=None, - progress_log_every=50, - ) + max_ik_attempts=max_ik_attempts, + num_iter=num_iter, + random_sampler=RANDOM_SAMPLER, + max_delta_q=MAX_DELTA_Q, + cubeplacementqgoal=cubeplacementqgoal, + q_goal=qgoal, + max_time_s=None, + progress_log_every=50, + repair_sampler=repair_sampler, + repair_max_attempts=repair_max_attempts, + goal_bias=goal_bias) + + rrt = rrt.run() if not rrt: print( diff --git a/rrt.py b/rrt.py index 5c6d9dd..8c417b6 100644 --- a/rrt.py +++ b/rrt.py @@ -9,12 +9,56 @@ import logging import numpy as np +import pinocchio as pin +from pinocchio.utils import rotate + +from constants import EDGE_DISTANCE_TOL, DEFAULT_DISCRETISATION_STEPS, DEFAULT_CHECK_EDGE_STEPS +from tools import setcubeplacement +from constants import DEFAULT_VIZ_DELAY +import time +import sampling + +logger = logging.getLogger(__name__) + + +from dataclasses import dataclass + + +@dataclass +class Vertex: + q: pin.SE3 + grasping_q: Optional[np.ndarray] + parent: Optional['Vertex'] = None + + +class RRT: + """A small, explicit RRT tree that stores vertices of cube placements. + + Attributes: + nodes: list of Vertex objects, root at index 0. + """ + + def __init__(self, + root_q: pin.SE3, + root_grasping_q: Optional[np.ndarray], + robot: pin.robot_wrapper.RobotWrapper, cube: pin.robot_wrapper.RobotWrapper, get_grasping_poseq: Optional[Callable] = None, viz=None, viz_delay: float = DEFAULT_VIZ_DELAY, discretisation_steps: int = DEFAULT_DISCRETISATION_STEPS, - max_ik_attempts: Optional[int] = None): + max_ik_attempts: Optional[int] = None, + # Run-time parameters (moved into the instance so run() needs no args) + num_iter: int = 1000, + random_sampler: Optional[Callable] = None, + max_delta_q: Optional[float] = None, + cubeplacementqgoal: Optional[pin.SE3] = None, + q_goal: Optional[np.ndarray] = None, + max_time_s: Optional[float] = None, + progress_log_every: int = 50, + repair_sampler: Optional[Callable] = None, + repair_max_attempts: int = 5, + goal_bias: float = 0.02): self.robot = robot self.cube = cube @@ -27,6 +71,17 @@ self.viz_delay = viz_delay self.discretisation_steps = discretisation_steps self.max_ik_attempts = max_ik_attempts + # store run-time configuration so run() requires no arguments + self.num_iter = num_iter + self.random_sampler = random_sampler + self.max_delta_q = max_delta_q + self.cubeplacementqgoal = cubeplacementqgoal + self.q_goal = q_goal + self.max_time_s = max_time_s + self.progress_log_every = progress_log_every + self.repair_sampler = repair_sampler + self.repair_max_attempts = repair_max_attempts + self.goal_bias = goal_bias # profiling counters for IK, viz and edge checks self._profile = { 'ik_calls': 0, @@ -36,93 +91,173 @@ 'edge_checks': 0, 'time_edge_checks': 0.0, } - # runtime parameters that control a run; populated by callers before - # invoking `run()` so the instance method can operate without args. - self.random_sampler = None - self.num_iter = None - self.max_delta_q = None - self.max_time_s = None - self.progress_log_every = 50 - self.repair_sampler = None - self.repair_max_attempts = 5 - self.goal_bias = 0.02 - # goal / initial placements and grasping configs - self.q_init = None - self.q_goal = None - self.cubeplacementqgoal = None - total_samples, successful_extensions, self.metrics['success_rate'], self.metrics['samples_per_s']) - return self - break - - if cp_q_new_found: - successful_extensions += 1 - self.add_edge(parent_idx=nearest_idx, new_q=cp_q_new, new_grasping_q=cp_q_new_grasping_pose) - - if self.check_edge(cp_q_new, cp_q_new_grasping_pose, cubeplacementqgoal, discretisation_steps=discretisation_steps): - self.add_edge(parent_idx=len(self.nodes) - 1, new_q=cubeplacementqgoal, new_grasping_q=q_goal) - elapsed = time.perf_counter() - start_time - # attach metrics to self for later inspection - self.metrics = { - 'total_samples': total_samples, - 'successful_extensions': successful_extensions, - 'iterations': i + 1, - 'time_s': elapsed, - 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), - 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, - } - # attach profiling collected by the RRT and sampling modules - try: - self.metrics['profiling'] = self._profile.copy() - except Exception: - self.metrics['profiling'] = {} - try: - self.metrics['sampling'] = sampling.metrics.copy() - except Exception: - self.metrics['sampling'] = {} - logger.info("RRT found a path after %d iterations (%.3fs)", i + 1, elapsed) - logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", - total_samples, successful_extensions, self.metrics['success_rate'], self.metrics['samples_per_s']) - return self - elapsed = time.perf_counter() - start_time - # include profiling even on failure - try: - self.metrics = { - 'total_samples': total_samples, - 'successful_extensions': successful_extensions, - 'iterations': num_iter, - 'time_s': elapsed, - 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), - 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, - } - self.metrics['profiling'] = self._profile.copy() - self.metrics['sampling'] = sampling.metrics.copy() - except Exception: - pass - logger.info("RRT completed %d iterations without connecting to goal (%.3fs)", num_iter, elapsed) - logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", - total_samples, successful_extensions, (successful_extensions / total_samples) if total_samples > 0 else 0.0, - total_samples / elapsed if elapsed > 0 else float('inf')) - return None + @staticmethod + def calc_distance(q1, q2) -> float: + """Euclidean distance between two translations or SE3s.""" + if hasattr(q1, 'translation'): + t1 = np.array(q1.translation) + else: + t1 = np.array(q1) + + if hasattr(q2, 'translation'): + t2 = np.array(q2.translation) + else: + t2 = np.array(q2) + + return np.linalg.norm(t2 - t1) + + @staticmethod + def lerp(q0, q1, t: float): + """Linear interpolation of translations and return a pin.SE3 placement.""" + if hasattr(q0, 'translation'): + a = np.array(q0.translation) + else: + a = np.array(q0) + + if hasattr(q1, 'translation'): + b = np.array(q1.translation) + else: + b = np.array(q1) + + trans = (1 - t) * a + t * b + return pin.SE3(rotate('z', 0.0), trans) + + def add_edge(self, parent_idx: int, new_q: pin.SE3, new_grasping_q: Optional[np.ndarray]) -> None: + self.nodes.append(Vertex(q=new_q, grasping_q=new_grasping_q, parent=self.nodes[parent_idx])) + + def get_nearest_vertex(self, q: pin.SE3) -> Tuple[int, Vertex]: + nearest_idx = 0 + smallest = float('inf') + for i, v in enumerate(self.nodes): + dist = self.calc_distance(v.q, q) + if dist < smallest: + smallest = dist + nearest_idx = i + return nearest_idx, self.nodes[nearest_idx] + + def get_q_new(self, + q_nearest_vertex: Vertex, + q_rand: pin.SE3, + discretisation_steps: Optional[int] = None, + max_delta_q: Optional[float] = None) -> Tuple[pin.SE3, Optional[np.ndarray], bool]: + + q_new_found = True + q_end = q_rand + + dist = self.calc_distance(q_nearest_vertex.q, q_rand) + if max_delta_q is not None and dist > max_delta_q: + q_end = self.lerp(q_nearest_vertex.q, q_rand, max_delta_q / dist) + + grasping_q = None + if self.get_grasping_poseq is not None: + # allow caller to override discretisation for this call; otherwise use instance default + discretisation_steps = discretisation_steps if discretisation_steps is not None else self.discretisation_steps + dt = 1.0 / discretisation_steps + # seed IK with the nearest vertex grasping pose and then update the seed + seed_grasping_q = q_nearest_vertex.grasping_q + for i in range(1, discretisation_steps): + q = self.lerp(q_nearest_vertex.q, q_end, dt * i) + # optionally update visualization so user sees the robot attempt + if self.viz is not None: + t_v = time.perf_counter() + setcubeplacement(self.robot, self.cube, q) + q_disp = getattr(self.robot, 'q', None) + if q_disp is not None: + self.viz.display(q_disp) + else: + if hasattr(self.viz, 'render'): + self.viz.render() + dt_v = time.perf_counter() - t_v + self._profile['viz_calls'] += 1 + self._profile['time_viz'] += dt_v + time.sleep(self.viz_delay) + + # Use the IK wrapper to call the IK callback in a + # backwards-compatible way and time the call. We set + # viz_sleep=False so display calls (if any) don't sleep. + from ik_utils import call_ik + ik_kwargs = dict( + robot=self.robot, + qcurrent=seed_grasping_q, + cube=self.cube, + cubetarget=q, + viz=(self.viz if self.viz is not None else None), + max_attempts=(self.max_ik_attempts if self.max_ik_attempts is not None else 1000), + viz_sleep=False, + ) + (grasping_q, found), dt_ik = call_ik(self.get_grasping_poseq, **ik_kwargs) + self._profile['ik_calls'] += 1 + self._profile['time_ik'] += dt_ik + # update seed for the next interpolation step if IK succeeded + if found and grasping_q is not None: + seed_grasping_q = grasping_q + if not found: + # cannot progress past the previous valid step + if i - 1 == 0: + q_new_found = False + return q_nearest_vertex.q, q_nearest_vertex.grasping_q, False + return self.lerp(q_nearest_vertex.q, q_end, dt * (i - 1)), grasping_q, True + return q_end, grasping_q, q_new_found + + def check_edge(self, q_latest_new: pin.SE3, q_latest_new_grasping_pose: Optional[np.ndarray], + q_goal: pin.SE3, discretisation_steps: Optional[int] = None) -> bool: + # Try to progress from q_latest_new to q_goal using get_q_new semantics + dummy_vertex = Vertex(q=q_latest_new, grasping_q=q_latest_new_grasping_pose, parent=None) + t_edge = time.perf_counter() + q_new, q_new_grasping_q, q_new_found = self.get_q_new( + q_nearest_vertex=dummy_vertex, + q_rand=q_goal, + discretisation_steps=discretisation_steps, + max_delta_q=None, + ) + dt_edge = time.perf_counter() - t_edge + self._profile['edge_checks'] += 1 + self._profile['time_edge_checks'] += dt_edge + if not q_new_found: + return False + distance_to_target = self.calc_distance(q_new, q_goal) + return distance_to_target < EDGE_DISTANCE_TOL + + def get_path_from_rrt(self) -> List[Vertex]: + path: List[Vertex] = [] + last = self.nodes[-1] + while last is not None: + path.insert(0, last) + last = last.parent + return path + + def get_shortcut_from_rrt_path(self, discretisation_steps: int = DEFAULT_DISCRETISATION_STEPS, + max_ik_attempts: Optional[int] = None) -> List[Vertex]: + path_vertices = self.get_path_from_rrt() + if len(path_vertices) <= 2: + return path_vertices + + short_path: List[Vertex] = [] + start_idx = 0 + while start_idx < len(path_vertices) - 1: + current_vertex = path_vertices[start_idx] + # Try to find the furthest vertex we can directly connect to + found_idx = None + for next_idx in range(len(path_vertices) - 1, start_idx, -1): + if self.check_edge(current_vertex.q, current_vertex.grasping_q, path_vertices[next_idx].q, + discretisation_steps=discretisation_steps): + found_idx = next_idx + break + if found_idx is None: + # no shortcut found; step by one + short_path.append(current_vertex) + start_idx += 1 + else: + short_path.append(current_vertex) + start_idx = found_idx + # always append the final vertex if path_vertices: short_path.append(path_vertices[-1]) return short_path - def run(self, - q_init: np.ndarray, - q_goal: np.ndarray, - cubeplacementqgoal: pin.SE3, - num_iter: int, - random_sampler: Callable, - max_delta_q: Optional[float] = None, - discretisation_steps: int = DEFAULT_DISCRETISATION_STEPS, - get_grasping_poseq: Optional[Callable] = None, - max_time_s: Optional[float] = None, - progress_log_every: int = 50, - repair_sampler: Optional[Callable] = None, - repair_max_attempts: int = 5, - goal_bias: float = 0.02) -> Optional['RRT']: + def run(self) -> Optional['RRT']: """Grow the RRT using the same algorithm formerly in construct_rrt. This is an instance method so callers can instantiate RRT(...) with @@ -147,19 +282,19 @@ def run(self, except Exception: pass - for i in range(num_iter): + for i in range(self.num_iter): # Periodic progress logging so long runs show activity - if progress_log_every and i % progress_log_every == 0 and i > 0: + if self.progress_log_every and i % self.progress_log_every == 0 and i > 0: elapsed_now = time.perf_counter() - start_time samples_per_s_now = total_samples / elapsed_now if elapsed_now > 0 else float('inf') logger.info("RRT progress: %d/%d iterations, samples: %d, elapsed: %.2fs, samples/s: %.2f", - i, num_iter, total_samples, elapsed_now, samples_per_s_now) + i, self.num_iter, total_samples, elapsed_now, samples_per_s_now) # Abort the run if we've exceeded a user-specified wall-time budget - if max_time_s is not None: + if self.max_time_s is not None: elapsed_now = time.perf_counter() - start_time - if elapsed_now > max_time_s: - logger.warning("RRT aborted after %.2fs (max_time_s=%.2fs) — returning no path", elapsed_now, max_time_s) + if elapsed_now > self.max_time_s: + logger.warning("RRT aborted after %.2fs (max_time_s=%.2fs) — returning no path", elapsed_now, self.max_time_s) # include profiling into self.metrics before returning try: self.metrics = { @@ -178,15 +313,28 @@ def run(self, # With small probability sample the goal directly (goal bias) import random as _random - if goal_bias and _random.random() < goal_bias: - random_cp_q = cubeplacementqgoal + if self.goal_bias and _random.random() < self.goal_bias: + random_cp_q = self.cubeplacementqgoal else: total_samples += 1 - random_cp_q = random_sampler() + random_cp_q = self.random_sampler() nearest_idx, nearest_vertex = self.get_nearest_vertex(random_cp_q) - def run(self) -> Optional['RRT']: - repair_target = repair_sampler() + + cp_q_new, cp_q_new_grasping_pose, cp_q_new_found = self.get_q_new( + q_nearest_vertex=nearest_vertex, + q_rand=random_cp_q, + discretisation_steps=(self.discretisation_steps if self.discretisation_steps is not None else self.discretisation_steps), + max_delta_q=self.max_delta_q, + ) + + # If the extension succeeded use it. Otherwise, optionally try a + # local repair sampler (smart sampler) a few times before continuing. + if not cp_q_new_found and self.repair_sampler is not None: + for r_attempt in range(self.repair_max_attempts): + total_samples += 1 + try: + repair_target = self.repair_sampler() except Exception: # if the repair sampler misbehaves, stop trying repairs break @@ -194,16 +342,16 @@ def run(self) -> Optional['RRT']: cp_q_new, cp_q_new_grasping_pose, cp_q_new_found = self.get_q_new( q_nearest_vertex=nearest_vertex, q_rand=repair_target, - discretisation_steps=(discretisation_steps if discretisation_steps is not None else self.discretisation_steps), - max_delta_q=max_delta_q, + discretisation_steps=(self.discretisation_steps if self.discretisation_steps is not None else self.discretisation_steps), + max_delta_q=self.max_delta_q, ) if cp_q_new_found: successful_extensions += 1 self.add_edge(parent_idx=nearest_idx, new_q=cp_q_new, new_grasping_q=cp_q_new_grasping_pose) # check connection to goal and return if found - if self.check_edge(cp_q_new, cp_q_new_grasping_pose, cubeplacementqgoal, - discretisation_steps=(discretisation_steps if discretisation_steps is not None else self.discretisation_steps)): - self.add_edge(parent_idx=len(self.nodes) - 1, new_q=cubeplacementqgoal, new_grasping_q=q_goal) + if self.check_edge(cp_q_new, cp_q_new_grasping_pose, self.cubeplacementqgoal, + discretisation_steps=(self.discretisation_steps if self.discretisation_steps is not None else self.discretisation_steps)): + self.add_edge(parent_idx=len(self.nodes) - 1, new_q=self.cubeplacementqgoal, new_grasping_q=self.q_goal) elapsed = time.perf_counter() - start_time # attach metrics to self for later inspection self.metrics = { @@ -232,8 +380,8 @@ def run(self) -> Optional['RRT']: successful_extensions += 1 self.add_edge(parent_idx=nearest_idx, new_q=cp_q_new, new_grasping_q=cp_q_new_grasping_pose) - if self.check_edge(cp_q_new, cp_q_new_grasping_pose, cubeplacementqgoal, discretisation_steps=(discretisation_steps if discretisation_steps is not None else self.discretisation_steps)): - self.add_edge(parent_idx=len(self.nodes) - 1, new_q=cubeplacementqgoal, new_grasping_q=q_goal) + if self.check_edge(cp_q_new, cp_q_new_grasping_pose, self.cubeplacementqgoal, discretisation_steps=(self.discretisation_steps if self.discretisation_steps is not None else self.discretisation_steps)): + self.add_edge(parent_idx=len(self.nodes) - 1, new_q=self.cubeplacementqgoal, new_grasping_q=self.q_goal) elapsed = time.perf_counter() - start_time # attach metrics to self for later inspection self.metrics = { @@ -264,7 +412,7 @@ def run(self) -> Optional['RRT']: self.metrics = { 'total_samples': total_samples, 'successful_extensions': successful_extensions, - 'iterations': num_iter, + 'iterations': self.num_iter, 'time_s': elapsed, 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, @@ -273,7 +421,7 @@ def run(self) -> Optional['RRT']: self.metrics['sampling'] = sampling.metrics.copy() except Exception: pass - logger.info("RRT completed %d iterations without connecting to goal (%.3fs)", num_iter, elapsed) + logger.info("RRT completed %d iterations without connecting to goal (%.3fs)", self.num_iter, elapsed) logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", total_samples, successful_extensions, (successful_extensions / total_samples) if total_samples > 0 else 0.0, total_samples / elapsed if elapsed > 0 else float('inf')) @@ -304,6 +452,7 @@ def construct_rrt(robot: pin.robot_wrapper.RobotWrapper, This is intentionally straightforward: we sample, extend toward the sample, and stop when we can connect to the goal. """ + # instantiate with run-time parameters and delegate to instance.run() rrt = RRT(root_q=cubeplacementq0, root_grasping_q=q_init, robot=robot, @@ -312,181 +461,16 @@ def construct_rrt(robot: pin.robot_wrapper.RobotWrapper, viz=viz, viz_delay=viz_delay, discretisation_steps=discretisation_steps, - max_ik_attempts=max_ik_attempts) - - # Delegate the main loop to the RRT instance; keep this wrapper for - # backwards compatibility. - return rrt.run(q_init=q_init, - q_goal=q_goal, - cubeplacementqgoal=cubeplacementqgoal, - num_iter=num_iter, - random_sampler=random_sampler, - max_delta_q=max_delta_q, - discretisation_steps=discretisation_steps, - get_grasping_poseq=get_grasping_poseq, - max_time_s=max_time_s, - progress_log_every=progress_log_every, - repair_sampler=repair_sampler, - repair_max_attempts=repair_max_attempts, - goal_bias=goal_bias) - - # Metrics for logging - total_samples = 0 - successful_extensions = 0 - start_time = time.perf_counter() - # reset sampling module metrics for this run - try: - sampling.metrics = { - 'samples': 0, - 'geom_updates': 0, - 'collision_checks': 0, - 'time_collision_checks': 0.0, - 'time_viz': 0.0, - 'time_sampling_loop': 0.0, - } - except Exception: - pass - - for i in range(num_iter): - # Periodic progress logging so long runs show activity - if progress_log_every and i % progress_log_every == 0 and i > 0: - elapsed_now = time.perf_counter() - start_time - samples_per_s_now = total_samples / elapsed_now if elapsed_now > 0 else float('inf') - logger.info("RRT progress: %d/%d iterations, samples: %d, elapsed: %.2fs, samples/s: %.2f", - i, num_iter, total_samples, elapsed_now, samples_per_s_now) - - # Abort the run if we've exceeded a user-specified wall-time budget - if max_time_s is not None: - elapsed_now = time.perf_counter() - start_time - if elapsed_now > max_time_s: - logger.warning("RRT aborted after %.2fs (max_time_s=%.2fs) — returning no path", elapsed_now, max_time_s) - # include profiling into rrt.metrics before returning - try: - rrt.metrics = { - 'total_samples': total_samples, - 'successful_extensions': successful_extensions, - 'iterations': i, - 'time_s': elapsed_now, - 'samples_per_s': total_samples / elapsed_now if elapsed_now > 0 else float('inf'), - 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, - } - rrt.metrics['profiling'] = rrt._profile.copy() - rrt.metrics['sampling'] = sampling.metrics.copy() - except Exception: - pass - return None - # With small probability sample the goal directly (goal bias) to - # encourage connection attempts. - import random as _random - if goal_bias and _random.random() < goal_bias: - random_cp_q = cubeplacementqgoal - else: - total_samples += 1 - random_cp_q = random_sampler() - nearest_idx, nearest_vertex = rrt.get_nearest_vertex(random_cp_q) - - cp_q_new, cp_q_new_grasping_pose, cp_q_new_found = rrt.get_q_new( - q_nearest_vertex=nearest_vertex, - q_rand=random_cp_q, - discretisation_steps=discretisation_steps, - max_delta_q=max_delta_q, - ) - - # If the extension succeeded use it. Otherwise, optionally try a - # local repair sampler (smart sampler) a few times before continuing. - if not cp_q_new_found and repair_sampler is not None: - for r_attempt in range(repair_max_attempts): - total_samples += 1 - try: - repair_target = repair_sampler() - except Exception: - # if the repair sampler misbehaves, stop trying repairs - break - nearest_idx, nearest_vertex = rrt.get_nearest_vertex(repair_target) - cp_q_new, cp_q_new_grasping_pose, cp_q_new_found = rrt.get_q_new( - q_nearest_vertex=nearest_vertex, - q_rand=repair_target, - discretisation_steps=discretisation_steps, - max_delta_q=max_delta_q, - ) - if cp_q_new_found: - successful_extensions += 1 - rrt.add_edge(parent_idx=nearest_idx, new_q=cp_q_new, new_grasping_q=cp_q_new_grasping_pose) - # check connection to goal and return if found - if rrt.check_edge(cp_q_new, cp_q_new_grasping_pose, cubeplacementqgoal, - discretisation_steps=discretisation_steps): - rrt.add_edge(parent_idx=len(rrt.nodes) - 1, new_q=cubeplacementqgoal, new_grasping_q=q_goal) - elapsed = time.perf_counter() - start_time - # attach metrics to rrt for later inspection - rrt.metrics = { - 'total_samples': total_samples, - 'successful_extensions': successful_extensions, - 'iterations': i + 1, - 'time_s': elapsed, - 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), - 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, - } - try: - rrt.metrics['profiling'] = rrt._profile.copy() - except Exception: - rrt.metrics['profiling'] = {} - try: - rrt.metrics['sampling'] = sampling.metrics.copy() - except Exception: - rrt.metrics['sampling'] = {} - logger.info("RRT found a path after %d iterations (%.3fs)", i + 1, elapsed) - logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", - total_samples, successful_extensions, rrt.metrics['success_rate'], rrt.metrics['samples_per_s']) - return rrt - break + max_ik_attempts=max_ik_attempts, + num_iter=num_iter, + random_sampler=random_sampler, + max_delta_q=max_delta_q, + cubeplacementqgoal=cubeplacementqgoal, + q_goal=q_goal, + max_time_s=max_time_s, + progress_log_every=progress_log_every, + repair_sampler=repair_sampler, + repair_max_attempts=repair_max_attempts, + goal_bias=goal_bias) - if cp_q_new_found: - successful_extensions += 1 - rrt.add_edge(parent_idx=nearest_idx, new_q=cp_q_new, new_grasping_q=cp_q_new_grasping_pose) - - if rrt.check_edge(cp_q_new, cp_q_new_grasping_pose, cubeplacementqgoal, discretisation_steps=discretisation_steps): - rrt.add_edge(parent_idx=len(rrt.nodes) - 1, new_q=cubeplacementqgoal, new_grasping_q=q_goal) - elapsed = time.perf_counter() - start_time - # attach metrics to rrt for later inspection - rrt.metrics = { - 'total_samples': total_samples, - 'successful_extensions': successful_extensions, - 'iterations': i + 1, - 'time_s': elapsed, - 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), - 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, - } - # attach profiling collected by the RRT and sampling modules - try: - rrt.metrics['profiling'] = rrt._profile.copy() - except Exception: - rrt.metrics['profiling'] = {} - try: - rrt.metrics['sampling'] = sampling.metrics.copy() - except Exception: - rrt.metrics['sampling'] = {} - logger.info("RRT found a path after %d iterations (%.3fs)", i + 1, elapsed) - logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", - total_samples, successful_extensions, rrt.metrics['success_rate'], rrt.metrics['samples_per_s']) - return rrt - - elapsed = time.perf_counter() - start_time - # include profiling even on failure - try: - rrt.metrics = { - 'total_samples': total_samples, - 'successful_extensions': successful_extensions, - 'iterations': num_iter, - 'time_s': elapsed, - 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), - 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, - } - rrt.metrics['profiling'] = rrt._profile.copy() - rrt.metrics['sampling'] = sampling.metrics.copy() - except Exception: - pass - logger.info("RRT completed %d iterations without connecting to goal (%.3fs)", num_iter, elapsed) - logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", - total_samples, successful_extensions, (successful_extensions / total_samples) if total_samples > 0 else 0.0, - total_samples / elapsed if elapsed > 0 else float('inf')) - return None + return rrt.run() From 0a530325aa8a4ef5d396b0b0a7c56e19ac6fb708 Mon Sep 17 00:00:00 2001 From: Monica Sekoyan Date: Sun, 9 Nov 2025 14:47:16 +0000 Subject: [PATCH 10/23] add control Signed-off-by: Monica Sekoyan --- control.py | 174 +++++++++---- cube_trajectory_wrapper.py | 491 +++++++++++++++++++++++++++++++++++++ inverse_geometry.py | 4 +- 3 files changed, 623 insertions(+), 46 deletions(-) create mode 100644 cube_trajectory_wrapper.py diff --git a/control.py b/control.py index 1b31abf..597e42d 100644 --- a/control.py +++ b/control.py @@ -8,49 +8,127 @@ import numpy as np +from zmq.constants import THREAD_AFFINITY_CPU_REMOVE from bezier import Bezier +from typing import List +import pinocchio as pin +from cube_trajectory_wrapper import GlobalMinJerkLinearJTraj +from tools import setcubeplacement +from config import LEFT_HAND, RIGHT_HAND + +import pybullet as pyb # in my solution these gains were good enough for all joints but you might want to tune this. -Kp = 300. # proportional gain (P of PD) -Kv = 2 * np.sqrt(Kp) # derivative gain (D of PD) +Kp = 1000 # proportional gain (P of PD) +Kv = 30 * np.sqrt(Kp) # derivative gain (D of PD) + +Kx_lin = 2000 # Very stiff position control +Dx_lin = 5 * np.sqrt(Kx_lin) + +Kx_rot = 1000 +Dx_rot = 10 -def controllaw(sim, robot, trajs, tcurrent, cube): +def controllaw(sim, robot, trajs, tcurrent): + """Joint trajectory tracking with end-effector force correction""" q, vq = sim.getpybulletstate() q_target = trajs[0](tcurrent) vq_target = trajs[1](tcurrent) + vvq_target = trajs[2](tcurrent) + + vvq_des = vvq_target + Kp * (q_target - q) + Kv * (vq_target - vq) + M = robot.mass(q) + h = robot.nle(q, vq) + tau_base = M @ vvq_des + h + cube_pos, cube_quat = pyb.getBasePositionAndOrientation(sim.cubeId) + R = np.array(pyb.getMatrixFromQuaternion(cube_quat)).reshape(3, 3) + cube_current_SE3 = pin.SE3(R, np.array(cube_pos)) - error = q_target - q - vvq_des = Kp * error + Kv * vq_target + # cube_target_SE3 = pin.SE3(rotate('z', 0.), cube_trajs(tcurrent)) + setcubeplacement(robot, cube, cube_current_SE3) - # data = robot.data + # Forward kinematics + model, data = robot.model, robot.data + left_id = model.getFrameId(LEFT_HAND) + right_id = model.getFrameId(RIGHT_HAND) - # Mass matrix - M = robot.mass(q) - h = robot.nle(q, vq) + pin.forwardKinematics(model, data, trajs[0](0)) + pin.updateFramePlacements(model, data) - torques = M @ vvq_des + h + oMLhand_desired = data.oMf[left_id] # From joint trajectory + oMRhand_desired = data.oMf[right_id] + + + pin.forwardKinematics(model, data, q, vq) + pin.updateFramePlacements(model, data) + pin.computeJointJacobians(model, data, q) + + # Jacobians + J_l = pin.computeFrameJacobian(model, data, q, left_id, + pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) + J_r = pin.computeFrameJacobian(model, data, q, right_id, + pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) + + oMLhand_actual = data.oMf[left_id] + oMRhand_actual = data.oMf[right_id] + + error_pos_hands = oMLhand_actual.translation - oMRhand_actual.translation + + # Orientation errors + R_err_l = oMLhand_desired.rotation @ oMLhand_actual.rotation.T + err_rot_l = pin.log3(R_err_l) - # print('Order: ', sim.bulletCtrlJointsInPinOrder) - - # torques = [torques[i] for i in sim.bulletCtrlJointsInPinOrder] - sim.step(torques) + R_err_r = oMRhand_desired.rotation @ oMRhand_actual.rotation.T + err_rot_r = pin.log3(R_err_r) + # Velocities + v_l = pin.getFrameVelocity(model, data, left_id, + pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) + vel_l = np.hstack([v_l.linear, v_l.angular]) + v_r = pin.getFrameVelocity(model, data, right_id, + pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) + vel_r = np.hstack([v_r.linear, v_r.angular]) + + # Compute correction forces + F_l = np.hstack([ + -Kx_lin * error_pos_hands - Dx_lin * vel_l[:3], + Kx_rot * err_rot_l - Dx_rot * vel_l[3:] + ]) + F_r = np.hstack([ + Kx_lin * error_pos_hands - Dx_lin * vel_r[:3], + Kx_rot * err_rot_r - Dx_rot * vel_r[3:] + ]) + + # Map to joint torques + tau_l = J_l[:, :].T @ F_l + tau_r = J_r[:, :].T @ F_r + + tau_l = np.clip(tau_l, -200.0, 200.0) + tau_r = np.clip(tau_r, -200.0, 200.0) + + # Combine: base tracking + end-effector correction + tau_total = tau_base + tau_r + tau_l + sim.step(tau_total) + if __name__ == "__main__": from tools import setupwithpybullet, setupwithpybulletandmeshcat, rununtil from config import DT - - robot, sim, table, cube = setupwithpybullet() - - + from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET from inverse_geometry import computeqgrasppose from path import computepath - q0,successinit = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT, None) - qe,successend = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT_TARGET, None) + robot, sim, table, cube = setupwithpybullet() + + q0, successinit = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT, None) + qe, successend = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT_TARGET, None) + + if not successinit or not successend: + print('Failed to compute grasp pose') + exit() + path, cube_placements = computepath( q0, qe, @@ -61,34 +139,44 @@ def controllaw(sim, robot, trajs, tcurrent, cube): table=table, computeqgrasppose=computeqgrasppose) - - print('Success Init: ', successinit) - print('Success End: ', successend) - - - #setting initial configuration sim.setqsim(q0) - - - #TODO this is just an example, you are free to do as you please. - #In any case this trajectory does not follow the path - #0 init and end velocities - def maketraj(q0,q1,path, T): #TODO compute a real trajectory ! - poimtlist = [q0, q0] + path + [q1, q1] - # point = [q0, q0, q1, q1] - q_of_t = Bezier(poimtlist,t_max=T) + + def maketraj(q0, q1, path, T): #TODO compute a real trajectory ! + points = [q0, q0] + path + [q1, q1] + q_of_t = Bezier(points,t_max=T) vq_of_t = q_of_t.derivative(1) vvq_of_t = vq_of_t.derivative(1) return q_of_t, vq_of_t, vvq_of_t + + + def maketraj_with_joint_space_linear( + waypoints: List[np.array], + T: int): + joint_space_trajectory = GlobalMinJerkLinearJTraj( + waypoints=waypoints, + T_max=T, + ) + + return ( + joint_space_trajectory, + joint_space_trajectory.derivative(1), + joint_space_trajectory.derivative(2) + ) - - #TODO this is just a random trajectory, you need to do this yourself total_time=10. - trajs = maketraj(q0, qe, path, total_time) - + + trajs = maketraj_with_joint_space_linear( + waypoints=path, + T=total_time + ) + + cube_trajs = maketraj_with_joint_space_linear( + waypoints=[p.translation for p in cube_placements], + T=total_time + )[0] + tcur = 0. - while tcur < total_time: rununtil( controllaw, @@ -96,9 +184,7 @@ def maketraj(q0,q1,path, T): #TODO compute a real trajectory ! sim, robot, trajs, - tcur, - cube) + tcur) tcur += DT - - + \ No newline at end of file diff --git a/cube_trajectory_wrapper.py b/cube_trajectory_wrapper.py new file mode 100644 index 0000000..77e2400 --- /dev/null +++ b/cube_trajectory_wrapper.py @@ -0,0 +1,491 @@ +import numpy as np +from config import DT +import pinocchio as pin +from pinocchio.utils import rotate +from inverse_geometry import computeqgrasppose +from bezier import Bezier + +class RobotTrajectoryWrapper: + def __init__( + self, + q_init, + q_goal, + T_max, + T_min=0.0, + dt: float = DT + ): + self.q_init = q_init + self.q_goal = q_goal + self.T_max = T_max + self.T_min = T_min + self.dt = dt + + self.q_t = { + T_min: q_init, + T_max: q_goal + } + + def __call__(self, t): + return self.get_closest_q(t) + + def set_q_t(self, q, t): + self.q_t[t] = q + + def get_closest_q(self, t): + if t in self.q_t: + return self.q_t[t] + else: + return self.q_t[min(self.q_t.keys(), key=lambda x: abs(x-t))] + + def derivative(self, order, t): + + q_t = self.get_closest_q(t) + + # choose neighbors with clamping + tp = min(t + self.dt, self.T_max) + tm = max(t - self.dt, self.T_min) + q_tp = self.get_closest_q(tp) + q_tm = self.get_closest_q(tm) + + if order == 1: + if tp == t and tm == t: + return np.zeros_like(q_t) + if tp > t and tm < t: # central diff + return (q_tp - q_tm) / (tp - tm) + elif tp > t: # forward one-sided + return (q_tp - q_t) / (tp - t) + else: # backward one-sided + return (q_t - q_tm) / (t - tm) + + if order == 2: + # use symmetric step h when possible + h_p = max(tp - t, 0.0) + h_m = max(t - tm, 0.0) + if h_p > 0 and h_m > 0 and abs(h_p - h_m) < 1e-12: + h = h_p # equal steps + return (q_tp - 2.0*q_t + q_tm) / (h*h) + # fallback: nonuniform 3-point formula (approx) + denom = 0.5*(h_p*h_m*(h_p + h_m)) + 1e-12 + a = 2.0 / denom + return a * ( (q_tp * h_m) - (q_t * (h_p + h_m)) + (q_tm * h_p) ) + raise ValueError("Only derivative orders 1 or 2 supported.") + + +class CubeBezierTrajectory: + def __init__(self, + cube_placements, + robot, + cube, + q_init, + q_goal, + T_max, + T_min=0.0, + dt: float = DT): + + self.cube_placements = cube_placements + self.cube_placements_pos = [p.translation for p in cube_placements] + + self.q_of_t_cube = Bezier(self.cube_placements_pos, T_min, T_max) + + self.robot = robot + self.cube = cube + + self.dt = dt + self.T_min = T_min + self.T_max = T_max + + self.robot_q_trajectory = RobotTrajectoryWrapper( + q_init=q_init, + q_goal=q_goal, + T_max=T_max, + T_min=T_min, + dt=dt + ) + + def _register_robot_q_t(self, q, t): + self.robot_q_trajectory.set_q_t(q, t) + + def __call__(self, t): + cube_pos = self.q_of_t_cube(t) + cube_placement = pin.SE3(rotate('z', 0.), cube_pos) + + q, success = computeqgrasppose( + robot=self.robot, + qcurrent=self.robot_q_trajectory.get_closest_q(t - self.dt), + cube=self.cube, + cubetarget=cube_placement) + if not success: + print(t) + print(cube_placement) + print(self.robot_q_trajectory.get_closest_q(t - self.dt * 2)) + print(q) + raise ValueError("Failed to compute grasp pose") + # print(f"t={t:.3f}, success={success}") + self.robot_q_trajectory.set_q_t(q, t) + return q + + def get_q_derivative(self, t, order): + if t not in self.robot_q_trajectory.q_t: + self.__call__(t) + prev_t = max(self.T_min, t - self.dt) + next_t = min(self.T_max, t + self.dt) + if prev_t not in self.robot_q_trajectory.q_t: + self.__call__(prev_t) + if next_t not in self.robot_q_trajectory.q_t: + self.__call__(next_t) + return self.robot_q_trajectory.derivative(order, t) + + def derivative(self, order): + return lambda t: self.get_q_derivative(t, order) + + +class CubeLinearTrajectory: + def __init__(self, + cube_placements, + robot, + cube, + q_init, + q_goal, + T_max, + T_min=0.0, + dt: float = DT): + + self.cube_placements = cube_placements + self.cube_placements_pos = [p.translation for p in cube_placements] + + self.robot = robot + self.cube = cube + + self.T_max = T_max + self.T_min = T_min + + self.dt = dt + + self.robot_q_trajectory = RobotTrajectoryWrapper( + q_init=q_init, + q_goal=q_goal, + T_max=T_max, + T_min=T_min, + dt=dt + ) + + segments = np.diff(self.cube_placements_pos, axis=0) + self.lengths = np.linalg.norm(segments, axis=1) + self.cumulative_lengths = np.concatenate([[0], np.cumsum(self.lengths)]) + self.total_length = self.cumulative_lengths[-1] + + # constant speed + self.v = self.total_length / T_max + + def _register_robot_q_t(self, q, t): + self.robot_q_trajectory.set_q_t(q, t) + + def _get_cube_pos(self, t): + # current arc length + s = t * self.v + s = np.clip(s, 0, self.total_length) + + # segment index of the current arc length + segment_idx = np.searchsorted(self.cumulative_lengths, s, side='right') - 1 + segment_idx = min(segment_idx, len(self.lengths) - 1) + + # Local parameter within segment + s_local = s - self.cumulative_lengths[segment_idx] + alpha = s_local / self.lengths[segment_idx] if self.lengths[segment_idx] > 0 else 0 + + # Interpolate + p0 = self.cube_placements_pos[segment_idx] + p1 = self.cube_placements_pos[segment_idx + 1] + return (1 - alpha) * p0 + alpha * p1 + + def __call__(self, t): + cube_pos = self._get_cube_pos(t) + cube_placement = pin.SE3(rotate('z', 0.), cube_pos) + + q, success = computeqgrasppose( + robot=self.robot, + qcurrent=self.robot_q_trajectory.get_closest_q(t-self.dt * 10), + cube=self.cube, + cubetarget=cube_placement) + + if not success: + print(f'Failed to compute q for t={t}, cube_pos={cube_pos}, ') + self.robot_q_trajectory.set_q_t(q, t) + return q + + def get_q_derivative(self, t, order): + if t not in self.robot_q_trajectory.q_t: + self.__call__(t) + prev_t = max(self.T_min, t - self.dt) + next_t = min(self.T_max, t + self.dt) + if prev_t not in self.robot_q_trajectory.q_t: + self.__call__(prev_t) + if next_t not in self.robot_q_trajectory.q_t: + self.__call__(next_t) + return self.robot_q_trajectory.derivative(order, t) + + def derivative(self, order): + return lambda t: self.get_q_derivative(t, order) + + + +class LinearJointTrajectory: + """ + Piecewise-linear joint-space trajectory through given waypoints. + Time is distributed proportional to joint-space segment length, + with optional per-joint vmax / amax limiting. + """ + def __init__(self, waypoints, T_max, qdot_max=None, amax=None, eps=1e-9): + self.W = np.asarray(waypoints, dtype=float) # (K, nq) + assert self.W.ndim == 2 and self.W.shape[0] >= 2, "Need at least 2 waypoints" + self.nq = self.W.shape[1] + self.T_max = float(T_max) + self.eps = eps + + # Per-joint limits + if qdot_max is None: + qdot_max = np.full(self.nq, 2.0) # rad/s default + if amax is None: + amax = np.full(self.nq, 5.0) # rad/s^2 default + self.qdot_max = np.asarray(qdot_max, dtype=float) + self.amax = np.asarray(amax, dtype=float) + + # Segment lengths in joint space (∞-norm per joint -> max joint move dominates) + dQ = np.diff(self.W, axis=0) # (K-1, nq) + seg_norms = np.max(np.abs(dQ), axis=1) # (K-1,) + seg_norms = np.maximum(seg_norms, self.eps) + total = float(np.sum(seg_norms)) + + # First pass: proportional time split + self.seg_times = (seg_norms / total) * self.T_max # (K-1,) + + # Enforce vmax per joint: T_i >= max_j |dq_ij| / vmax_j + Tmin_v = np.max(np.abs(dQ) / self.qdot_max, axis=1) + self.seg_times = np.maximum(self.seg_times, Tmin_v + 1e-6) + + # Optional: very light amax check (not a full trapezoid here) + # You can tighten with a true trapezoidal time-scaler if needed. + + # Prefix sums -> piecewise timing + self.t_knots = np.concatenate(([0.0], np.cumsum(self.seg_times))) + # Guard for round-off + self.t_knots[-1] = self.T_max + + # Precompute segment slopes (constant velocity per segment) + self.seg_slopes = np.zeros_like(dQ) + for i, dt in enumerate(self.seg_times): + self.seg_slopes[i] = dQ[i] / max(dt, self.eps) + + def _find_seg(self, t): + if t <= 0.0: return 0, 0.0 + if t >= self.T_max: return len(self.seg_times) - 1, self.seg_times[-1] + # locate segment index i s.t. t in [t_i, t_{i+1}] + i = np.searchsorted(self.t_knots, t, side='right') - 1 + i = min(max(i, 0), len(self.seg_times) - 1) + tau = t - self.t_knots[i] + return i, tau + + def __call__(self, t): + """q(t)""" + t = float(np.clip(t, 0.0, self.T_max)) + i, tau = self._find_seg(t) + return self.W[i] + self.seg_slopes[i] * tau + + def derivative(self, order): + if order == 1: + return lambda t: self.velocity(t) + elif order == 2: + return lambda t: self.acceleration(t) + else: + return lambda t: np.zeros(self.nq) + + def velocity(self, t): + """qdot(t): piecewise constant""" + t = float(np.clip(t, 0.0, self.T_max)) + i, _ = self._find_seg(t) + return self.seg_slopes[i] + + def acceleration(self, t): + """qddot(t): zero inside segments (impulses at knots not modeled)""" + return np.zeros(self.nq) + + + +class GlobalMinJerkLinearJTraj: + """ + Follow a joint-space polyline with a GLOBAL min-jerk time law s(t) in [0, L]. + - q(t) continuous everywhere + - |q̇(t)| ramps from ~0 at start to peak mid-way, back to 0 at end + - q̇ direction still changes at vertices (polyline corners) + """ + def __init__(self, waypoints, T_max, eps=1e-9): + W = np.asarray(waypoints, float) + assert W.ndim == 2 and W.shape[0] >= 2 + self.W = W + self.nq = W.shape[1] + self.T = float(T_max) + self.eps = eps + + # Segment "lengths" in joint space (use ∞-norm so one joint dominates timing) + dQ = np.diff(W, axis=0) # (S, nq) + seg_L = np.max(np.abs(dQ), axis=1) # (S,) + seg_L = np.maximum(seg_L, eps) + self.dQ = dQ + self.seg_L = seg_L + self.cum_L = np.concatenate(([0.0], np.cumsum(seg_L))) + self.Ltot = float(self.cum_L[-1]) + + # Precompute segment directions (unit in ∞-norm sense) + self.seg_dir = np.divide(dQ, seg_L[:, None], where=seg_L[:, None] > 0) + + # ---- Min-jerk scalar profile s(t) (and derivatives) on [0,T] ---- + # τ in [0,1]; s = L*(10τ^3 - 15τ^4 + 6τ^5) + def _s(self, t): + tau = np.clip(t / self.T, 0.0, 1.0) + tau2, tau3, tau4, tau5 = tau*tau, tau*tau*tau, None, None + tau4 = tau2*tau2 + tau5 = tau3*tau2 + s = self.Ltot * (10*tau3 - 15*tau4 + 6*tau5) + sd = self.Ltot * (30*tau*tau - 60*tau2*tau + 30*tau4) / self.T + sdd = self.Ltot * (60*tau - 180*tau2 + 120*tau3) / (self.T**2) + return s, sd, sdd + + def _locate_segment(self, s): + # find i s.t. s in [cum_L[i], cum_L[i+1]] + i = np.searchsorted(self.cum_L, s, side='right') - 1 + i = int(np.clip(i, 0, len(self.seg_L)-1)) + s_local = s - self.cum_L[i] + alpha = s_local / max(self.seg_L[i], self.eps) # in [0,1] + return i, alpha + + def __call__(self, t): + s, sd, sdd = self._s(t) + # Clamp end + if s >= self.Ltot - 1e-12: + return self.W[-1] + i, alpha = self._locate_segment(s) + return self.W[i] + alpha * self.dQ[i] + + def derivative(self, order): + if order == 1: + return lambda t: self.velocity(t) + if order == 2: + return lambda t: self.acceleration(t) + return lambda t: np.zeros(self.nq) + + def velocity(self, t): + s, sd, sdd = self._s(t) + if s >= self.Ltot - 1e-12 or sd <= 0: + return np.zeros(self.nq) + i, _ = self._locate_segment(s) + # q̇ = (dq/ds) * ṡ ; along a linear segment, dq/ds = seg_dir[i] + return self.seg_dir[i] * sd + + def acceleration(self, t): + s, sd, sdd = self._s(t) + if s >= self.Ltot - 1e-12: + return np.zeros(self.nq) + i, _ = self._locate_segment(s) + # q̈ = (dq/ds) * s̈ + (d/ds seg_dir)*ṡ^2 ; second term is zero inside segment + # (impulses at corners not modeled explicitly) + return self.seg_dir[i] * sdd + + + +class LinearPathWithTrapezoidalVel: + """ + Piecewise linear path with smooth velocity profile that slows at corners + """ + def __init__(self, waypoints, T_max, corner_slowdown=0.1): + W = np.asarray(waypoints, float) + self.W = W + self.nq = W.shape[1] + self.T = T_max + + # Segment lengths + dQ = np.diff(W, axis=0) + seg_L = np.linalg.norm(dQ, axis=1) + self.seg_L = seg_L + self.cum_L = np.concatenate(([0.0], np.cumsum(seg_L))) + self.Ltot = self.cum_L[-1] + + # Direction vectors + self.seg_dir = dQ / (seg_L[:, None] + 1e-9) + + # Define "corner zones" - last X% of each segment + first X% of next + self.corner_slowdown = corner_slowdown + self.slow_distance = corner_slowdown * np.min(seg_L[seg_L > 0]) if len(seg_L) > 0 else 0.1 + + def _s(self, t): + """Min-jerk profile on [0, T] but modified near corners""" + tau = np.clip(t / self.T, 0.0, 1.0) + tau2, tau3 = tau*tau, tau*tau*tau + tau4, tau5 = tau2*tau2, tau3*tau2 + + # Base min-jerk + s_base = self.Ltot * (10*tau3 - 15*tau4 + 6*tau5) + sd_base = self.Ltot * (30*tau2 - 60*tau3 + 30*tau4) / self.T + sdd_base = self.Ltot * (60*tau - 180*tau2 + 120*tau3) / (self.T**2) + + # Slow down factor based on proximity to corners + # slowdown = self._corner_slowdown_factor(s_base) + slowdown = 1.0 + + return s_base, sd_base * slowdown, sdd_base * slowdown + + def _corner_slowdown_factor(self, s): + """Returns value in (0, 1] that reduces speed near corners""" + # Find which segment we're in + i = np.searchsorted(self.cum_L, s, side='right') - 1 + i = int(np.clip(i, 0, len(self.seg_L)-1)) + + # Distance from start and end of segment + s_in_seg = s - self.cum_L[i] + dist_to_end = self.seg_L[i] - s_in_seg + + # Slow down if near end of segment (approaching corner) + if dist_to_end < self.slow_distance: + factor = dist_to_end / self.slow_distance + return 0.2 + 0.8 * factor # Reduce to 20% speed at corner + + # Slow down if near start of segment (just left corner) + if s_in_seg < self.slow_distance and i > 0: + factor = s_in_seg / self.slow_distance + return 0.2 + 0.8 * factor + + return 1.0 # Full speed in middle of segments + + def _locate_segment(self, s): + i = np.searchsorted(self.cum_L, s, side='right') - 1 + i = int(np.clip(i, 0, len(self.seg_L)-1)) + s_local = s - self.cum_L[i] + alpha = s_local / max(self.seg_L[i], 1e-9) + return i, alpha + + def __call__(self, t): + s, _, _ = self._s(t) + if s >= self.Ltot - 1e-12: + return self.W[-1] + i, alpha = self._locate_segment(s) + return self.W[i] + alpha * (self.W[i+1] - self.W[i]) + + def velocity(self, t): + s, sd, _ = self._s(t) + if s >= self.Ltot - 1e-12 or sd <= 0: + return np.zeros(self.nq) + i, _ = self._locate_segment(s) + return self.seg_dir[i] * sd + + def acceleration(self, t): + s, sd, sdd = self._s(t) + if s >= self.Ltot - 1e-12: + return np.zeros(self.nq) + i, _ = self._locate_segment(s) + return self.seg_dir[i] * sdd + + def derivative(self, order): + if order == 1: + return self.velocity + elif order == 2: + return self.acceleration + return lambda t: np.zeros(self.nq) \ No newline at end of file diff --git a/inverse_geometry.py b/inverse_geometry.py index b4b7814..ba4810f 100644 --- a/inverse_geometry.py +++ b/inverse_geometry.py @@ -121,8 +121,8 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None): rotation_to_right_hook = norm(rhand_nu_world[:3]) translation_to_right_hook = norm(rhand_nu_world[3:]) - left_hook_close_enough = rotation_to_left_hook <= EPSILON and translation_to_left_hook <= EPSILON - right_hook_close_enough = rotation_to_right_hook <= EPSILON and translation_to_right_hook <= EPSILON + left_hook_close_enough = rotation_to_left_hook <= EPSILON and translation_to_left_hook <= EPSILON / 10 + right_hook_close_enough = rotation_to_right_hook <= EPSILON and translation_to_right_hook <= EPSILON / 10 if left_hook_close_enough and right_hook_close_enough: if not collision(robot, q): From 25ea0509d338c88720cd5ebe9f89a0ac7cbe5b46 Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Sun, 9 Nov 2025 14:49:48 +0000 Subject: [PATCH 11/23] Move presets to constants --- constants.py | 38 ++++++++++++++++++++++++++++++++++++-- path.py | 47 ++++++++++++++++++++++++++++++++++++++++++----- 2 files changed, 78 insertions(+), 7 deletions(-) diff --git a/constants.py b/constants.py index 482f25f..4671d0f 100644 --- a/constants.py +++ b/constants.py @@ -9,8 +9,8 @@ DEFAULT_CHECK_COLLISIONS: bool = True # RRT / planning -DEFAULT_NUM_ITER: int = 200 -DEFAULT_DISCRETISATION_STEPS: int = 100 +DEFAULT_NUM_ITER: int = 500 +DEFAULT_DISCRETISATION_STEPS: int = 8 DEFAULT_CHECK_EDGE_STEPS: int = 1000 EDGE_DISTANCE_TOL: float = 1e-3 @@ -21,3 +21,37 @@ 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.02 + +# 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 = 30.0 + +# 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, + } +} diff --git a/path.py b/path.py index bd3f019..e517368 100644 --- a/path.py +++ b/path.py @@ -7,7 +7,19 @@ from sampling import random_cube_placement import sampling as sampling_module from rrt import Vertex, RRT, construct_rrt -from constants import DEFAULT_NUM_ITER, DEFAULT_DISCRETISATION_STEPS, DEFAULT_VIZ_DELAY, DEFAULT_SAMPLER_SHRINK, DEFAULT_PLANE_HALF_WIDTH, DEFAULT_TABLE_Z_RANGE +from constants import ( + DEFAULT_NUM_ITER, + DEFAULT_DISCRETISATION_STEPS, + DEFAULT_VIZ_DELAY, + DEFAULT_SAMPLER_SHRINK, + DEFAULT_PLANE_HALF_WIDTH, + DEFAULT_TABLE_Z_RANGE, + PRESETS, + DEFAULT_REPAIR_ATTEMPTS, + DEFAULT_GOAL_BIAS, + DEFAULT_MAX_IK_ATTEMPTS, + DEFAULT_MAX_TIME_S, +) from tools import setcubeplacement import time import logging @@ -36,6 +48,7 @@ def computepath(qinit, num_iter: int = DEFAULT_NUM_ITER, discretisation_steps: int = DEFAULT_DISCRETISATION_STEPS, max_ik_attempts: int = None, + max_time_s: float = DEFAULT_MAX_TIME_S, post_path_wait: float = 5.0, random_sampler=None, repair_sampler=None, @@ -131,7 +144,7 @@ def plane_sampler(): max_delta_q=MAX_DELTA_Q, cubeplacementqgoal=cubeplacementqgoal, q_goal=qgoal, - max_time_s=None, + max_time_s=max_time_s, progress_log_every=50, repair_sampler=repair_sampler, repair_max_attempts=repair_max_attempts, @@ -204,6 +217,7 @@ def displaypath(robot, cube, path, cube_placements, dt, viz): parser.add_argument('--smart-global', action='store_true', help='Use the smart sampler as the global sampler instead of repair-only') parser.add_argument('--goal-bias', type=float, default=0.02, help='Probability to sample the goal directly on each iteration (0.0-1.0)') parser.add_argument('--playback', action='store_true', help='Show the final path even if planning ran with --no-viz') + parser.add_argument('--preset', choices=['fast', 'default', 'robust'], default=None, help='Use a preset bundle of planner defaults') args = parser.parse_args() logging.basicConfig(level=logging.INFO) @@ -221,6 +235,24 @@ def displaypath(robot, cube, path, cube_placements, dt, viz): raise SystemExit(1) viz_obj = None if args.no_viz else viz + # Resolve preset values (CLI args override preset). We detect explicit + # CLI overrides by checking sys.argv for the flag name. + import sys + preset_values = PRESETS.get(args.preset) if args.preset is not None else {} + def use_or_preset(flag_name, current_value): + # Return CLI-provided value if flag present, otherwise preset or current_value + if f'--{flag_name}' in sys.argv: + return current_value + if preset_values and flag_name in preset_values: + return preset_values[flag_name] + return current_value + + resolved_num_iter = use_or_preset('num-iter', args.num_iter) + resolved_discretisation = use_or_preset('discretisation', args.discretisation) + resolved_repair_attempts = use_or_preset('repair-attempts', args.repair_attempts) + resolved_goal_bias = use_or_preset('goal-bias', args.goal_bias) + resolved_max_ik_attempts = use_or_preset('max-ik-attempts', args.max_ik_attempts if args.max_ik_attempts is not None else DEFAULT_MAX_IK_ATTEMPTS) + resolved_max_time = use_or_preset('max-time', args.max_time if args.max_time is not None else DEFAULT_MAX_TIME_S) # choose sampler: either let computepath build the plane sampler, or build a smart sampler here RANDOM_SAMPLER = None REPAIR_SAMPLER = None @@ -303,13 +335,18 @@ def plane_sampler(): robot=robot, cube=cube, computeqgrasppose=computeqgrasppose, - num_iter=args.num_iter, - discretisation_steps=args.discretisation, + num_iter=resolved_num_iter, + discretisation_steps=resolved_discretisation, random_sampler=RANDOM_SAMPLER, repair_sampler=REPAIR_SAMPLER, + repair_max_attempts=resolved_repair_attempts, + goal_bias=resolved_goal_bias, viz=viz_obj, viz_delay=args.viz_delay, - max_ik_attempts=args.max_ik_attempts, + max_ik_attempts=resolved_max_ik_attempts, + # forward wall-time + # Note: computepath forwards this into construct_rrt/RRT.run + max_time_s=resolved_max_time, ) # optionally playback the final path. If --playback is set we show the From dcfaec8ed71b08400a9bce59565e7abe6a6296c6 Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Tue, 11 Nov 2025 13:30:21 +0000 Subject: [PATCH 12/23] Expand sampling region --- path.py | 2 +- rrt.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/path.py b/path.py index e517368..1f995bc 100644 --- a/path.py +++ b/path.py @@ -93,7 +93,7 @@ def computepath(qinit, def plane_sampler(): while True: - s = np.random.uniform(0.0, 1.0) + s = np.random.uniform(-1.0, 2.0) t = np.random.uniform(-half_width, half_width) xy = (1.0 - s) * p0_xy + s * p1_xy + perp * t z = np.random.uniform(zmin, zmax) diff --git a/rrt.py b/rrt.py index 8c417b6..762ac2c 100644 --- a/rrt.py +++ b/rrt.py @@ -13,7 +13,7 @@ from pinocchio.utils import rotate from constants import EDGE_DISTANCE_TOL, DEFAULT_DISCRETISATION_STEPS, DEFAULT_CHECK_EDGE_STEPS -from tools import setcubeplacement +from tools import setcubeplacement, distanceToObstacle from constants import DEFAULT_VIZ_DELAY import time import sampling @@ -192,7 +192,7 @@ def get_q_new(self, # update seed for the next interpolation step if IK succeeded if found and grasping_q is not None: seed_grasping_q = grasping_q - if not found: + if not found or distanceToObstacle(self.robot, grasping_q) < 0.001: # cannot progress past the previous valid step if i - 1 == 0: q_new_found = False From abc07bbddedefe46fd07f3f85b7caa65b06b87d3 Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Tue, 11 Nov 2025 13:35:11 +0000 Subject: [PATCH 13/23] Don't expect a table --- control.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control.py b/control.py index 6c3b46a..aa3a3aa 100644 --- a/control.py +++ b/control.py @@ -119,7 +119,7 @@ def controllaw(sim, robot, trajs, tcurrent): from inverse_geometry import computeqgrasppose from path import computepath - robot, sim, table, cube = setupwithpybullet() + robot, sim, cube = setupwithpybullet() q0, successinit = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT, None) qe, successend = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT_TARGET, None) From 7a1378e2293d32e7e320265b051fd9491aead847 Mon Sep 17 00:00:00 2001 From: Monica Sekoyan Date: Tue, 11 Nov 2025 13:42:00 +0000 Subject: [PATCH 14/23] add qp with bezier curves Signed-off-by: Monica Sekoyan --- bezier.py | 2 +- control.py | 43 ++++++---- inverse_geometry.py | 4 +- path.py | 8 +- qp_utils.py | 190 ++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 223 insertions(+), 24 deletions(-) create mode 100644 qp_utils.py diff --git a/bezier.py b/bezier.py index 62375bc..2e392c1 100644 --- a/bezier.py +++ b/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_): diff --git a/control.py b/control.py index 597e42d..f88ae53 100644 --- a/control.py +++ b/control.py @@ -6,6 +6,7 @@ @author: stonneau """ +import pickle import numpy as np from zmq.constants import THREAD_AFFINITY_CPU_REMOVE @@ -15,7 +16,7 @@ from cube_trajectory_wrapper import GlobalMinJerkLinearJTraj from tools import setcubeplacement from config import LEFT_HAND, RIGHT_HAND - +from qp_utils import get_bezier_control_points import pybullet as pyb # in my solution these gains were good enough for all joints but you might want to tune this. @@ -26,7 +27,15 @@ Dx_lin = 5 * np.sqrt(Kx_lin) Kx_rot = 1000 -Dx_rot = 10 +Dx_rot = 10 + +# Whether to use Bezier curve for trajectory generation +USE_BEZIER = False + +if USE_BEZIER: + Kp = 1000 # proportional gain (P of PD) + Kv = 30 * np.sqrt(Kp) # derivative gain (D of PD) + def controllaw(sim, robot, trajs, tcurrent): """Joint trajectory tracking with end-effector force correction""" @@ -125,9 +134,9 @@ def controllaw(sim, robot, trajs, tcurrent): q0, successinit = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT, None) qe, successend = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT_TARGET, None) + if not successinit or not successend: - print('Failed to compute grasp pose') - exit() + raise RuntimeError('Failed to successfully compute grasp pose!') path, cube_placements = computepath( q0, @@ -141,8 +150,7 @@ def controllaw(sim, robot, trajs, tcurrent): sim.setqsim(q0) - def maketraj(q0, q1, path, T): #TODO compute a real trajectory ! - points = [q0, q0] + path + [q1, q1] + def maketraj(points, T): q_of_t = Bezier(points,t_max=T) vq_of_t = q_of_t.derivative(1) vvq_of_t = vq_of_t.derivative(1) @@ -163,17 +171,18 @@ def maketraj_with_joint_space_linear( joint_space_trajectory.derivative(2) ) - total_time=10. - - trajs = maketraj_with_joint_space_linear( - waypoints=path, - T=total_time - ) - - cube_trajs = maketraj_with_joint_space_linear( - waypoints=[p.translation for p in cube_placements], - T=total_time - )[0] + total_time=10.0 + + if USE_BEZIER: + control_points = get_bezier_control_points(path) + if control_points is None: + raise RuntimeError('Failed to successfully compute Bezier control points!') + trajs = maketraj(control_points, total_time) + else: + trajs = maketraj_with_joint_space_linear( + waypoints=path, + T=total_time + ) tcur = 0. diff --git a/inverse_geometry.py b/inverse_geometry.py index ba4810f..1bca68d 100644 --- a/inverse_geometry.py +++ b/inverse_geometry.py @@ -9,7 +9,7 @@ import pinocchio as pin import numpy as np from numpy.linalg import pinv,inv,norm,svd,eig -from tools import collision, getcubeplacement, setcubeplacement, projecttojointlimits +from tools import collision, getcubeplacement, setcubeplacement, jointlimitsviolated from config import LEFT_HOOK, RIGHT_HOOK, LEFT_HAND, RIGHT_HAND, EPSILON from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET @@ -125,7 +125,7 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None): right_hook_close_enough = rotation_to_right_hook <= EPSILON and translation_to_right_hook <= EPSILON / 10 if left_hook_close_enough and right_hook_close_enough: - if not collision(robot, q): + if not collision(robot, q) and not jointlimitsviolated(robot, q): q_opt_found = True break diff --git a/path.py b/path.py index 3129510..31b75e2 100644 --- a/path.py +++ b/path.py @@ -15,7 +15,7 @@ from typing import Callable, Tuple, List from pinocchio.utils import rotate -from tools import setcubeplacement +from tools import setcubeplacement, distanceToObstacle #### Helper Functions for Random Cube Placement Sampling #### @@ -215,7 +215,7 @@ def get_q_new(self, cube=self.cube, cubetarget=q, ) - if not found_grasping_pose: + if not found_grasping_pose or (found_grasping_pose and distanceToObstacle(self.robot, grasping_q) < 2e-3): if i - 1 == 0: q_new_found = False return self.lerp( @@ -423,10 +423,10 @@ def computepath(qinit, table = table or globals().get("table") computeqgrasppose = computeqgrasppose or globals().get("computeqgrasppose") - NUM_ITER = 200 + NUM_ITER = 500 RANDOM_SAMPLER = lambda: random_cube_placement(robot=robot, cube=cube, table=table) MAX_DELTA_Q = None - DISCRETISATION_STEPS = 100 + DISCRETISATION_STEPS = 200 GET_GRAPSING_POSEQ = computeqgrasppose print('Searching for a valid path...(this may take some time)') diff --git a/qp_utils.py b/qp_utils.py new file mode 100644 index 0000000..17193f1 --- /dev/null +++ b/qp_utils.py @@ -0,0 +1,190 @@ +import numpy as np +import quadprog +from typing import List, Dict, Tuple + +def assign_time_to_intermediate_points(path: List[np.array]) -> Dict[int, float]: + # assign time to each point in the path + # so we can use it in QP cost function + # compute time proportional to distance between points + + total_dist = 0 + point_idx_time = {} + + for i in range(1, len(path)-1): + dist = np.linalg.norm(path[i] - path[i-1]) + total_dist += dist + point_idx_time[i] = dist + + total_dist += np.linalg.norm(path[-1] - path[-2]) + norm_dist = np.cumsum(list(point_idx_time.values()) / total_dist) + for point_idx, time in point_idx_time.items(): + point_idx_time[point_idx] = norm_dist[point_idx-1] + + return point_idx_time + + +def quadprog_solve_qp(H, q, G=None, h=None, C=None, d=None, verbose=False): + """ + min (1/2)x' P x + q' x + subject to G x <= h + subject to C x = d + """ + qp_G = 0.5 * (H + H.T) # make sure P is symmetric + qp_a = -q + qp_C = None + qp_b = None + meq = 0 + if C is not None: + if G is not None: + qp_C = -np.vstack([C, G]).T + qp_b = -np.hstack([d, h]) + else: + qp_C = -C.transpose() + qp_b = -d + meq = C.shape[0] + elif G is not None: # no equality constraint + qp_C = -G.T + qp_b = -h + res = quadprog.solve_qp(qp_G, qp_a, qp_C, qp_b, meq) + if verbose: + return res + return res[0] + + +def bernstein_deg4(t: float) -> np.array: + """Quartic Bernstein basis [B0..B4] at scalar t.""" + # as the equation looks like this + # p(t) = (1-t)^4*p0 + 4(1-t)^3*t*p1 + 6(1-t)^2*t^2*p2 + 4(1-t)*t^3*p3 + t^4*p4 + # so we need to compute the coefficients for each term + # B0 = (1-t)^4 + # B1 = 4*(1-t)^3*t + # B2 = 6*(1-t)^2*t^2 + # B3 = 4*(1-t)*t^3 + # B4 = t^4 + tt = t + omt = 1.0 - tt + B0 = omt**4 + B1 = 4 * omt**3 * tt + B2 = 6 * omt**2 * tt**2 + B3 = 4 * omt * tt**3 + B4 = tt**4 + return np.array([B0, B1, B2, B3, B4], dtype=np.float64) + +def build_H_q_quartic_bezier( + q0: np.array, + qe: np.array, + t_samples: List[float], + g_samples: List[np.array], + l2_reg: float = 0.0 +) -> Tuple[np.array, np.array]: + + + """ + t_samples: (N,) in [0,1] + g_samples: (N, 15) targets + Returns H (45x45), q (45,) + """ + + ## p(t)=P0​B0​ + (P1​B1 ​+ P2​B2 ​+ P3​B3 ​) + P4​B4 + ## need to be p(t) = Ax + b + ## where b = P0​B0​ + P4​B4 + + ## variables are [P1_dimensions, P2_dimensions, P3_dimensions] + ## and A = [ + ## [B1, 0, 0, 0, ...., B2, 0, 0, 0, ...., B3, 0, 0, 0,0...], + ## [0, B1, 0, 0, ...., 0, B2, 0, 0, ...., 0, B3, 0, 0, ...., 0], + ## [0, 0, B1, 0, ...., 0, 0, B2, 0, ...., 0, 0, B3, 0, ...., 0], + ## ..... + ## [0, 0, 0, B1, ...., 0, 0, 0, B2, ...., 0, 0, 0, B3, ...., 0], + ## [0, 0, 0, 0, ...., B3, 0, 0, 0, ...., 0, 0, 0, 0, ...., B3] + ## ] + + # we need to compute the coefficients for each term + t_samples = np.asarray(t_samples, dtype=np.float64).ravel() + g_samples = np.asarray(g_samples, dtype=np.float64) + assert g_samples.shape == (t_samples.size, 15) + + d = 15 + n_ctrl = 3 + nvars = d * n_ctrl + + H = np.zeros((nvars, nvars), dtype=np.float64) + q = np.zeros(nvars, dtype=np.float64) + + # samples in the computed trajectory we want to fit + # Should look like this + # ||p(t) - g(t)||^2 + # ||A x + b - g(t)||^2 + # ||A x - (g(t) - b)||^2 + # 1/2 x.T A.T A x + (-A.T (g(t) - b)) x + # 1/2 x.T A.T A x + (A.T (b - g(t))) x + + for ti, gi in zip(t_samples, g_samples): + B = bernstein_deg4(ti) + b = B[0] * q0 + B[-1] * qe + A_i = np.concatenate([np.eye(d) * b for b in B[1:-1]], axis=1)# (15, 45): per-dim applies same B across control blocks + H += A_i.T @ A_i # accumulate quadratic term + q += A_i.T @ (b - gi) # accumulate linear term + + if l2_reg > 0.0: + # Optional small ridge for PD Hessian (helps quadprog) + H += 2.0 * l2_reg * np.eye(nvars, dtype=np.float64) + + return H, q + + +def accel_equalities_known_endpoints( + q0: np.array, + qe: np.array +) -> Tuple[np.array, np.array]: + d = 15 + C_start = np.concatenate([np.eye(d) * i for i in [-2., 1., 0.]], axis=1) # P2 - 2P1 = -P0 + C_end = np.concatenate([np.eye(d) * i for i in [ 0., 1., -2.]], axis=1) # P2 - 2P3 = -P4 + C = np.vstack([C_start, C_end]) # (30,45) + dvec = np.concatenate([-q0, -qe]) # (30,) + return C, dvec + + +def construct_collision_constraints( + discretization_steps, + distance_to_obstacle_function, + distance_to_obstacle_threshold): + + ## we need to construct inequality constraints for the collision avoidance + ## form of inequality constraint is + ## distance_to_obstacle_function(x) <= distance_to_obstacle_threshold + ## distance_function(p(t)) <= distance_to_obstacle_threshold + ## distance_function(Ax+b) <= distance_to_obstacle_threshold + + pass + + +def get_bezier_control_points( + path: List[np.array]) -> List[np.array]: + + # assign specific times to intermediate points + # this is based on the distance between points in the path + point_times = assign_time_to_intermediate_points(path) + + q0 = path[0] + qe = path[-1] + + t_samples = np.array(list(point_times.values())) + g_samples = path[1:-1] + + l2_reg = 1e-6 + + H, q = build_H_q_quartic_bezier(q0, qe, t_samples, g_samples, l2_reg=l2_reg) + C, d = accel_equalities_known_endpoints(q0, qe) + res = quadprog_solve_qp(H, q, C=C, d=d) + + if res is None or res.size == 0: + return + + control_points = [] + for i in range(0, len(res), 15): + control_points.append(res[i:i+15]) + + return [q0] + control_points + [qe] + + From ddf37f9715852f4a7d5963a518188a332cdd680d Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Tue, 11 Nov 2025 13:51:59 +0000 Subject: [PATCH 15/23] Increase discretisation steps --- constants.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/constants.py b/constants.py index 4671d0f..b7b1dbe 100644 --- a/constants.py +++ b/constants.py @@ -10,7 +10,7 @@ # RRT / planning DEFAULT_NUM_ITER: int = 500 -DEFAULT_DISCRETISATION_STEPS: int = 8 +DEFAULT_DISCRETISATION_STEPS: int = 50 DEFAULT_CHECK_EDGE_STEPS: int = 1000 EDGE_DISTANCE_TOL: float = 1e-3 From fe0caa6409db2ca90c9f03e4072fadbd389fb19c Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Tue, 11 Nov 2025 14:07:39 +0000 Subject: [PATCH 16/23] Finish merge --- inverse_geometry.py | 4 - path.py | 398 -------------------------------------------- 2 files changed, 402 deletions(-) diff --git a/inverse_geometry.py b/inverse_geometry.py index a8ebc72..53409ae 100644 --- a/inverse_geometry.py +++ b/inverse_geometry.py @@ -9,12 +9,8 @@ import pinocchio as pin import numpy as np from numpy.linalg import pinv,inv,norm,svd,eig -<<<<<<< HEAD from tools import collision, getcubeplacement, setcubeplacement, jointlimitsviolated -======= import time -from tools import collision, getcubeplacement, setcubeplacement, projecttojointlimits ->>>>>>> main from config import LEFT_HOOK, RIGHT_HOOK, LEFT_HAND, RIGHT_HAND, EPSILON from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET diff --git a/path.py b/path.py index 4e508b8..157caf5 100644 --- a/path.py +++ b/path.py @@ -30,399 +30,10 @@ logger = logging.getLogger(__name__) -<<<<<<< HEAD from typing import Callable, Tuple, List from pinocchio.utils import rotate from tools import setcubeplacement, distanceToObstacle - -#### Helper Functions for Random Cube Placement Sampling #### - -def get_table_borders(table: pin.robot_wrapper.RobotWrapper) -> Tuple[np.array, np.array]: - """ Returns the minimum and maximum x, y, z coordinates of the table. - Args: - table: table object - Returns: - xyz_min: minimum x, y, z coordinates of the table's leftmost and bottommost corner - xyz_max: maximum x, y, z coordinates of the table's rightmost and topmost corner - """ - pin.updateGeometryPlacements(table.model, table.data, table.collision_model, table.collision_data) - table_surface = table.collision_model.geometryObjects[0].geometry - oMtable_surface = table.collision_data.oMg[0] - R, t = oMtable_surface.rotation, oMtable_surface.translation - - - half = table_surface.halfSide - local_corners = np.array([[sx, sy, sz] - for sx in [+1, -1] - for sy in [+1, -1] - for sz in [+1, -1]]) * half - world_corners = (R @ local_corners.T).T + t - - xyz_min = world_corners.min(axis=0) - xyz_max = world_corners.max(axis=0) - - return xyz_min, xyz_max - - - -def random_cube_placement( - robot: pin.robot_wrapper.RobotWrapper, - cube: pin.robot_wrapper.RobotWrapper, - table: pin.robot_wrapper.RobotWrapper, - check_collisions: bool = True - ) -> pin.pinocchio_pywrap_default.SE3: - """ Returns a random cube placement on the table that is collision free. - Args: - robot: robot object - cube: cube object - table: table object - check_collisions: whether to check for collisions - Returns: - valid_placement: random cube placement on the table that is collision free - """ - xyz_min, xyz_max = get_table_borders(table) - - collision = True - valid_placement = None - - while collision: - x = np.random.uniform(xyz_min[0], xyz_max[0]) - y = np.random.uniform(xyz_min[1], xyz_max[1]) - # TODO: change harcoded max z to a more clever bounding - z = np.random.uniform(0.93, 2) - - placement = pin.SE3(rotate('z', 0.), np.array([x, y, z])) - setcubeplacement(robot, cube, placement) - if check_collisions: - collision = pin.computeCollisions(cube.collision_model, cube.collision_data, False) - valid_placement = placement if not collision else valid_placement - else: - valid_placement = placement - collision = False - - return valid_placement - - -#### Helper Functions and Classes for RRT #### - -class Vertex: - def __init__(self, q, grasping_q, parent): - self.q = q - self.grasping_q = grasping_q - self.parent = parent - -class RRT: - def __init__(self, - root_q: pin.pinocchio_pywrap_default.SE3, - root_grasping_q: np.array, - robot: pin.robot_wrapper.RobotWrapper, - cube: pin.robot_wrapper.RobotWrapper, - get_grasping_poseq: Callable): - - self.robot = robot - self.cube = cube - self.get_grasping_poseq = get_grasping_poseq - - # initialize the tree - self.nodes = [ - Vertex(q=root_q, grasping_q=root_grasping_q, parent=None) - ] - - @staticmethod - def calc_distance( - q1: np.array, - q2: np.array) -> float: - '''Return the euclidian distance between two configurations''' - return np.linalg.norm(q2 - q1) - - @staticmethod - def lerp( - q0: np.array, - q1: np.array, - t: float - ) -> np.array: - """ - Performs linear interpolation between the given points and for the step t - """ - return RRT.get_placement_from_translation((1 - t) * q0 + t * q1) - - @staticmethod - def get_placement_from_translation(translation): - return pin.SE3(rotate('z', 0.), np.array(translation)) - - def add_edge(self, - parent_idx: int, - new_q: pin.pinocchio_pywrap_default.SE3, - new_grasping_q: np.array) -> None: - - """ - Constructs and adds a new vertex to the tree. - """ - - self.nodes.append( - Vertex(q=new_q, - grasping_q=new_grasping_q, - parent=self.nodes[parent_idx]) - ) - - def get_nearest_vertex(self, - q: pin.pinocchio_pywrap_default.SE3 - ) -> Tuple[int, Vertex]: - - """ - Args: - q: random configuration of the cube (SE3) - Returns: - nearest_vertex_idx: nearest vertex index in the tree - nearest_vertex: nearest vertex - """ - nearest_vertex_idx = None - smallest_distance = None - - for i, vertex in enumerate(self.nodes): - dist = self.calc_distance(vertex.q.translation, q.translation) - if not smallest_distance or dist <= smallest_distance: - smallest_distance = dist - nearest_vertex_idx = i - - return nearest_vertex_idx, self.nodes[nearest_vertex_idx] - - def get_q_new(self, - q_nearest_vertex: Vertex, - q_rand: pin.pinocchio_pywrap_default.SE3, - discretisation_steps: int, - max_delta_q: int = None, - ) -> Tuple[pin.pinocchio_pywrap_default.SE3, np.array, bool]: - - """ Returns a new configuration that is the closest to the random configuration `q_rand` that can be grasped and is collison free. - Args: - q_nearest_vertex: nearest vertex - q_rand: random configuration - discretisation_steps: number of discretisation steps - max_delta_q: maximum delta q - Returns: - q_new: new configuration - grasping_q: grasping pose for the new configuration - q_new_found: True if the new configuration (other than the `q_nearest_vertex`) is found, False otherwise - """ - - q_new_found = True - q_end = q_rand.copy() - - dist = self.calc_distance( - q_nearest_vertex.q.translation, - q_rand.translation) - - if max_delta_q is not None and dist > max_delta_q: - q_end = self.lerp( - q_nearest_vertex.q.translation, - q_rand.translation, - max_delta_q/dist) - - if self.get_grasping_poseq is not None: - dt = 1 / discretisation_steps - for i in range(1, discretisation_steps): - q = self.lerp( - q_nearest_vertex.q.translation, - q_end.translation, - dt*i) - grasping_q, found_grasping_pose = self.get_grasping_poseq( - robot=self.robot, - qcurrent=q_nearest_vertex.grasping_q, - cube=self.cube, - cubetarget=q, - ) - if not found_grasping_pose or (found_grasping_pose and distanceToObstacle(self.robot, grasping_q) < 2e-3): - if i - 1 == 0: - q_new_found = False - return self.lerp( - q_nearest_vertex.q.translation, - q_end.translation, - dt*(i-1)), grasping_q, q_new_found - else: - grasping_q = None - - return q_end, grasping_q, q_new_found - - def check_edge( - self, - q_latest_new: pin.pinocchio_pywrap_default.SE3, - q_latest_new_grasping_pose: np.array, - q_goal: pin.pinocchio_pywrap_default.SE3, - discretisation_steps: int = 1000, - ) -> bool: - - - """ Checks if there is a valid edge between the latest new configuration and the goal configuration. - Args: - q_latest_new: latest new configuration - q_latest_new_grasping_pose: grasping pose for the latest new configuration - q_goal: goal configuration - discretisation_steps: number of discretisation steps - Returns: - valid_edge: True if there is a valid edge, False otherwise - """ - - valid_edge = False - q_latest_new_vertex = Vertex( - q=q_latest_new, - grasping_q=q_latest_new_grasping_pose, - parent=None - ) - - # check if we can get to the q_goal from the q_new directly - q_new, q_new_grasping_q, q_new_found = self.get_q_new( - q_nearest_vertex=q_latest_new_vertex, - q_rand=q_goal, - discretisation_steps=discretisation_steps, - max_delta_q=None - ) - - if q_new_found: - distance_to_target = self.calc_distance(q_new.translation, q_goal.translation) - if distance_to_target < 1e-3: - valid_edge = True - - return valid_edge - - def get_path_from_rrt(self) -> List[Vertex]: - """ Returns a list of vertices from the root to the latest vertex. - If there was a valid edge between the latest vertex and the goal configuration, the latest vertex is the vertex with the goal configuration. - Returns: - vertices: list of vertices - """ - vertices = [] - last_vertex = self.nodes[-1] - - while last_vertex.parent is not None: - vertices.insert(0, last_vertex) - last_vertex = last_vertex.parent - - vertices.insert(0, last_vertex) - - return vertices - - def get_shortcut_from_rrt_path(self, - discretisation_steps: int = 100, - ) -> List[Vertex]: - - """ Returns a shortcut path from the root to the latest vertex. - Args: - discretisation_steps: number of discretisation steps - Returns: - short_path: list of vertices from the root to the latest vertex that has no redundant intermediate vertices - """ - - path_vertices = self.get_path_from_rrt() - short_path = [] - start_idx = 0 - - while start_idx < len(path_vertices) - 3: - current_vertex = path_vertices[start_idx] - short_path.append(current_vertex) - - # search in the next nodes starting from the furthest one - next_vertex_idx = len(path_vertices) - 1 - while next_vertex_idx > start_idx + 1: - next_vertex = path_vertices[next_vertex_idx] - is_valid_edge = self.check_edge( - q_latest_new=current_vertex.q, - q_latest_new_grasping_pose=current_vertex.grasping_q, - q_goal=next_vertex.q, - discretisation_steps=discretisation_steps - ) - if is_valid_edge: - start_idx = next_vertex_idx - break - next_vertex_idx -= 1 - - if not is_valid_edge: - start_idx += 1 - - short_path.extend(path_vertices[start_idx:]) - - return short_path - - - -#### The main function for constructing the RRT #### - -def construct_rrt(robot: pin.robot_wrapper.RobotWrapper, - cube: pin.robot_wrapper.RobotWrapper, - q_init: np.array, - q_goal: np.array, - cubeplacementq0: pin.pinocchio_pywrap_default.SE3, - cubeplacementqgoal: pin.pinocchio_pywrap_default.SE3, - num_iter: int, - radnom_sampler: Callable, - max_delta_q: float = None, - discretisation_steps: int = 100, - get_grasping_poseq: Callable = None, - ): - - """ Constructs the RRT from the root to the goal configuration. - Args: - robot: robot object - cube: cube object - q_init: initial grasping pose - q_goal: goal grasping pose - cubeplacementq0: initial cube placement - cubeplacementqgoal: goal cube placement - num_iter: number of iterations of sampling random cube placements - radnom_sampler: random cube placement sampler - max_delta_q: maximum delta q between the current and the end configuration used to get q_new configuration - discretisation_steps: number of discretisation steps used in the get_q_new and check_edge function - get_grasping_poseq: grasping pose calculator from the cube placement (inverse geometry). - Returns: - rrt: RRT object - """ - rrt = RRT( - root_q=cubeplacementq0, - root_grasping_q=q_init, - robot=robot, - cube=cube, - get_grasping_poseq=get_grasping_poseq - ) - - for _ in range(num_iter): - random_cp_q = radnom_sampler() - cp_q_nearest_idx, cp_q_nearest_vertex = rrt.get_nearest_vertex(random_cp_q) - - cp_q_new, cp_q_new_grasping_pose, cp_q_new_found = rrt.get_q_new( - q_nearest_vertex=cp_q_nearest_vertex, - q_rand=random_cp_q, - discretisation_steps=discretisation_steps, - max_delta_q=max_delta_q) - - if cp_q_new_found: - rrt.add_edge( - parent_idx=cp_q_nearest_idx, - new_q=cp_q_new, - new_grasping_q=cp_q_new_grasping_pose, - ) - - is_valid_edge = rrt.check_edge( - q_latest_new=cp_q_new, - q_latest_new_grasping_pose=cp_q_new_grasping_pose, - q_goal=cubeplacementqgoal, - discretisation_steps=discretisation_steps) - - if is_valid_edge: - rrt.add_edge(parent_idx=len(rrt.nodes) - 1, - new_q=cubeplacementqgoal, - new_grasping_q=q_goal) - - return rrt - - return None - -#returns a collision free path from qinit to qgoal under grasping constraints -#the path is expressed as a list of configurations -def computepath(qinit, - qgoal, - cubeplacementq0, -======= __all__ = [ "random_cube_placement", "Vertex", @@ -430,11 +41,9 @@ def computepath(qinit, "construct_rrt", ] - def computepath(qinit, qgoal, cubeplacementq0, ->>>>>>> main cubeplacementqgoal, robot=None, cube=None, @@ -463,12 +72,6 @@ def computepath(qinit, cube = cube or globals().get("cube") computeqgrasppose = computeqgrasppose or globals().get("computeqgrasppose") -<<<<<<< HEAD - NUM_ITER = 500 - RANDOM_SAMPLER = lambda: random_cube_placement(robot=robot, cube=cube, table=table) - MAX_DELTA_Q = None - DISCRETISATION_STEPS = 200 -======= # If no sampler supplied, build a constrained sampler on the vertical plane # that intersects the initial and final cube placements. sampling_box_ranges = None @@ -524,7 +127,6 @@ def plane_sampler(): else: RANDOM_SAMPLER = random_sampler MAX_DELTA_Q = None ->>>>>>> main GET_GRAPSING_POSEQ = computeqgrasppose print('Searching for a valid path...(this may take some time)') From 930dfa39eb7b9e8818236597159b08192210e8eb Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Tue, 11 Nov 2025 15:56:26 +0000 Subject: [PATCH 17/23] Actually finish merge --- path.py | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/path.py b/path.py index fe445f2..7e9c09b 100644 --- a/path.py +++ b/path.py @@ -30,13 +30,10 @@ logger = logging.getLogger(__name__) -<<<<<<< HEAD -======= from typing import Callable, Tuple, List from pinocchio.utils import rotate from tools import setcubeplacement, distanceToObstacle ->>>>>>> monica-branch __all__ = [ "random_cube_placement", "Vertex", @@ -75,10 +72,7 @@ def computepath(qinit, cube = cube or globals().get("cube") computeqgrasppose = computeqgrasppose or globals().get("computeqgrasppose") -<<<<<<< HEAD -======= ->>>>>>> monica-branch # If no sampler supplied, build a constrained sampler on the vertical plane # that intersects the initial and final cube placements. sampling_box_ranges = None @@ -134,11 +128,7 @@ def plane_sampler(): else: RANDOM_SAMPLER = random_sampler MAX_DELTA_Q = None -<<<<<<< HEAD GET_GRASPPING_POSEQ = computeqgrasppose -======= - GET_GRAPSING_POSEQ = computeqgrasppose ->>>>>>> monica-branch print('Searching for a valid path...(this may take some time)') # (no sampling-box visualization) From 386ca93e33759dc24e5b04c493063182ab5a32eb Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Thu, 13 Nov 2025 14:46:04 +0000 Subject: [PATCH 18/23] Remove smart sampler --- constants.py | 2 +- cube_trajectory_wrapper.py | 56 ++++++++++++------------- path.py | 83 +------------------------------------- rrt.py | 50 ----------------------- 4 files changed, 31 insertions(+), 160 deletions(-) diff --git a/constants.py b/constants.py index b7b1dbe..e8825f2 100644 --- a/constants.py +++ b/constants.py @@ -10,7 +10,7 @@ # RRT / planning DEFAULT_NUM_ITER: int = 500 -DEFAULT_DISCRETISATION_STEPS: int = 50 +DEFAULT_DISCRETISATION_STEPS: int = 100 DEFAULT_CHECK_EDGE_STEPS: int = 1000 EDGE_DISTANCE_TOL: float = 1e-3 diff --git a/cube_trajectory_wrapper.py b/cube_trajectory_wrapper.py index 77e2400..d536112 100644 --- a/cube_trajectory_wrapper.py +++ b/cube_trajectory_wrapper.py @@ -7,9 +7,9 @@ class RobotTrajectoryWrapper: def __init__( - self, - q_init, - q_goal, + self, + q_init, + q_goal, T_max, T_min=0.0, dt: float = DT @@ -72,8 +72,8 @@ def derivative(self, order, t): class CubeBezierTrajectory: - def __init__(self, - cube_placements, + def __init__(self, + cube_placements, robot, cube, q_init, @@ -108,7 +108,7 @@ def _register_robot_q_t(self, q, t): def __call__(self, t): cube_pos = self.q_of_t_cube(t) cube_placement = pin.SE3(rotate('z', 0.), cube_pos) - + q, success = computeqgrasppose( robot=self.robot, qcurrent=self.robot_q_trajectory.get_closest_q(t - self.dt), @@ -140,8 +140,8 @@ def derivative(self, order): class CubeLinearTrajectory: - def __init__(self, - cube_placements, + def __init__(self, + cube_placements, robot, cube, q_init, @@ -174,7 +174,7 @@ def __init__(self, self.cumulative_lengths = np.concatenate([[0], np.cumsum(self.lengths)]) self.total_length = self.cumulative_lengths[-1] - # constant speed + # constant speed self.v = self.total_length / T_max def _register_robot_q_t(self, q, t): @@ -237,7 +237,7 @@ class LinearJointTrajectory: """ def __init__(self, waypoints, T_max, qdot_max=None, amax=None, eps=1e-9): self.W = np.asarray(waypoints, dtype=float) # (K, nq) - assert self.W.ndim == 2 and self.W.shape[0] >= 2, "Need at least 2 waypoints" + # assert self.W.ndim == 2 and self.W.shape[0] >= 2, "Need at least 2 waypoints" self.nq = self.W.shape[1] self.T_max = float(T_max) self.eps = eps @@ -401,91 +401,91 @@ def __init__(self, waypoints, T_max, corner_slowdown=0.1): self.W = W self.nq = W.shape[1] self.T = T_max - + # Segment lengths dQ = np.diff(W, axis=0) seg_L = np.linalg.norm(dQ, axis=1) self.seg_L = seg_L self.cum_L = np.concatenate(([0.0], np.cumsum(seg_L))) self.Ltot = self.cum_L[-1] - + # Direction vectors self.seg_dir = dQ / (seg_L[:, None] + 1e-9) - + # Define "corner zones" - last X% of each segment + first X% of next self.corner_slowdown = corner_slowdown self.slow_distance = corner_slowdown * np.min(seg_L[seg_L > 0]) if len(seg_L) > 0 else 0.1 - + def _s(self, t): """Min-jerk profile on [0, T] but modified near corners""" tau = np.clip(t / self.T, 0.0, 1.0) tau2, tau3 = tau*tau, tau*tau*tau tau4, tau5 = tau2*tau2, tau3*tau2 - + # Base min-jerk s_base = self.Ltot * (10*tau3 - 15*tau4 + 6*tau5) sd_base = self.Ltot * (30*tau2 - 60*tau3 + 30*tau4) / self.T sdd_base = self.Ltot * (60*tau - 180*tau2 + 120*tau3) / (self.T**2) - + # Slow down factor based on proximity to corners # slowdown = self._corner_slowdown_factor(s_base) slowdown = 1.0 - + return s_base, sd_base * slowdown, sdd_base * slowdown - + def _corner_slowdown_factor(self, s): """Returns value in (0, 1] that reduces speed near corners""" # Find which segment we're in i = np.searchsorted(self.cum_L, s, side='right') - 1 i = int(np.clip(i, 0, len(self.seg_L)-1)) - + # Distance from start and end of segment s_in_seg = s - self.cum_L[i] dist_to_end = self.seg_L[i] - s_in_seg - + # Slow down if near end of segment (approaching corner) if dist_to_end < self.slow_distance: factor = dist_to_end / self.slow_distance return 0.2 + 0.8 * factor # Reduce to 20% speed at corner - + # Slow down if near start of segment (just left corner) if s_in_seg < self.slow_distance and i > 0: factor = s_in_seg / self.slow_distance return 0.2 + 0.8 * factor - + return 1.0 # Full speed in middle of segments - + def _locate_segment(self, s): i = np.searchsorted(self.cum_L, s, side='right') - 1 i = int(np.clip(i, 0, len(self.seg_L)-1)) s_local = s - self.cum_L[i] alpha = s_local / max(self.seg_L[i], 1e-9) return i, alpha - + def __call__(self, t): s, _, _ = self._s(t) if s >= self.Ltot - 1e-12: return self.W[-1] i, alpha = self._locate_segment(s) return self.W[i] + alpha * (self.W[i+1] - self.W[i]) - + def velocity(self, t): s, sd, _ = self._s(t) if s >= self.Ltot - 1e-12 or sd <= 0: return np.zeros(self.nq) i, _ = self._locate_segment(s) return self.seg_dir[i] * sd - + def acceleration(self, t): s, sd, sdd = self._s(t) if s >= self.Ltot - 1e-12: return np.zeros(self.nq) i, _ = self._locate_segment(s) return self.seg_dir[i] * sdd - + def derivative(self, order): if order == 1: return self.velocity elif order == 2: return self.acceleration - return lambda t: np.zeros(self.nq) \ No newline at end of file + return lambda t: np.zeros(self.nq) diff --git a/path.py b/path.py index 7e9c09b..8344d60 100644 --- a/path.py +++ b/path.py @@ -150,15 +150,13 @@ def plane_sampler(): q_goal=qgoal, max_time_s=max_time_s, progress_log_every=50, - repair_sampler=repair_sampler, - repair_max_attempts=repair_max_attempts, goal_bias=goal_bias) rrt = rrt.run() if not rrt: print( - """Valid Path was not found! + """Valid Path was not found! Try to increase discretisation steps or try with different start and end configurations.""") return [], [] @@ -206,7 +204,6 @@ def displaypath(robot, cube, path, cube_placements, dt, viz): parser.add_argument('--no-viz', action='store_true', help='Disable visualization during planning') parser.add_argument('--discretisation', type=int, default=DEFAULT_DISCRETISATION_STEPS, help='Discretisation steps for interpolation') parser.add_argument('--num-iter', type=int, default=DEFAULT_NUM_ITER, help='Maximum RRT iterations/samples') - parser.add_argument('--sampler', choices=['plane', 'smart'], default='plane', help='Which placement sampler to use') # smart sampler params parser.add_argument('--smart-init-sigma', type=float, default=0.02, help='Initial sigma (m) for smart midpoint sampler') parser.add_argument('--smart-scale', type=float, default=1.5, help='Sigma inflation factor for smart sampler') @@ -253,86 +250,12 @@ def use_or_preset(flag_name, current_value): resolved_num_iter = use_or_preset('num-iter', args.num_iter) resolved_discretisation = use_or_preset('discretisation', args.discretisation) - resolved_repair_attempts = use_or_preset('repair-attempts', args.repair_attempts) resolved_goal_bias = use_or_preset('goal-bias', args.goal_bias) resolved_max_ik_attempts = use_or_preset('max-ik-attempts', args.max_ik_attempts if args.max_ik_attempts is not None else DEFAULT_MAX_IK_ATTEMPTS) resolved_max_time = use_or_preset('max-time', args.max_time if args.max_time is not None else DEFAULT_MAX_TIME_S) # choose sampler: either let computepath build the plane sampler, or build a smart sampler here RANDOM_SAMPLER = None REPAIR_SAMPLER = None - if args.sampler == 'smart': - # build a plane sampler to use as fallback - p0 = np.array(CUBE_PLACEMENT.translation) - p1 = np.array(CUBE_PLACEMENT_TARGET.translation) - - p0_xy = p0[:2] - p1_xy = p1[:2] - seg = p1_xy - p0_xy - L = np.linalg.norm(seg) - if L < 1e-6: - u = np.array([1.0, 0.0]) - else: - u = seg / L - perp = np.array([-u[1], u[0]]) - - half_width = DEFAULT_PLANE_HALF_WIDTH - zmin, zmax = DEFAULT_TABLE_Z_RANGE - - def plane_sampler(): - while True: - s = np.random.uniform(0.0, 1.0) - t = np.random.uniform(-half_width, half_width) - xy = (1.0 - s) * p0_xy + s * p1_xy + perp * t - z = np.random.uniform(zmin, zmax) - placement = pin.SE3(np.eye(3), np.array([xy[0], xy[1], z])) - setcubeplacement(robot, cube, placement) - has_collision = pin.computeCollisions(cube.collision_model, cube.collision_data, False) - if not has_collision: - return placement - - # If --smart-global selected, use the smart sampler as the global - # sampler; otherwise keep the plane sampler global and use smart as a - # local repair strategy. This keeps exploration simple by default but - # lets the user switch behaviour. - if args.smart_global: - RANDOM_SAMPLER = sampling_module.make_smart_midpoint_sampler( - robot=robot, - cube=cube, - p0=CUBE_PLACEMENT, - p1=CUBE_PLACEMENT_TARGET, - initial_sigma=args.smart_init_sigma, - sigma_scale=args.smart_scale, - max_sigma=args.smart_max_sigma, - max_attempts=args.smart_max_attempts, - fallback_sampler=plane_sampler, - anisotropic=True, - z_range=DEFAULT_TABLE_Z_RANGE, - viz=(None if args.no_viz else viz), - viz_delay=args.viz_delay, - ) - REPAIR_SAMPLER = None - else: - # use the plane sampler as the global sampler and the smart sampler as a - # local repair strategy. This keeps global exploration simple while the - # smart sampler is used to attempt local fixes when an extension fails. - RANDOM_SAMPLER = plane_sampler - - # create the smart sampler closure (used as repair_sampler) - REPAIR_SAMPLER = sampling_module.make_smart_midpoint_sampler( - robot=robot, - cube=cube, - p0=CUBE_PLACEMENT, - p1=CUBE_PLACEMENT_TARGET, - initial_sigma=args.smart_init_sigma, - sigma_scale=args.smart_scale, - max_sigma=args.smart_max_sigma, - max_attempts=args.smart_max_attempts, - fallback_sampler=plane_sampler, - anisotropic=True, - z_range=DEFAULT_TABLE_Z_RANGE, - viz=(None if args.no_viz else viz), - viz_delay=args.viz_delay, - ) path, cube_placements = computepath( q0, qe, CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET, @@ -342,8 +265,6 @@ def plane_sampler(): num_iter=resolved_num_iter, discretisation_steps=resolved_discretisation, random_sampler=RANDOM_SAMPLER, - repair_sampler=REPAIR_SAMPLER, - repair_max_attempts=resolved_repair_attempts, goal_bias=resolved_goal_bias, viz=viz_obj, viz_delay=args.viz_delay, @@ -377,4 +298,4 @@ def plane_sampler(): except Exception: pass - print('Done') \ No newline at end of file + print('Done') diff --git a/rrt.py b/rrt.py index 762ac2c..d0d609a 100644 --- a/rrt.py +++ b/rrt.py @@ -56,8 +56,6 @@ def __init__(self, q_goal: Optional[np.ndarray] = None, max_time_s: Optional[float] = None, progress_log_every: int = 50, - repair_sampler: Optional[Callable] = None, - repair_max_attempts: int = 5, goal_bias: float = 0.02): self.robot = robot @@ -79,8 +77,6 @@ def __init__(self, self.q_goal = q_goal self.max_time_s = max_time_s self.progress_log_every = progress_log_every - self.repair_sampler = repair_sampler - self.repair_max_attempts = repair_max_attempts self.goal_bias = goal_bias # profiling counters for IK, viz and edge checks self._profile = { @@ -330,52 +326,6 @@ def run(self) -> Optional['RRT']: # If the extension succeeded use it. Otherwise, optionally try a # local repair sampler (smart sampler) a few times before continuing. - if not cp_q_new_found and self.repair_sampler is not None: - for r_attempt in range(self.repair_max_attempts): - total_samples += 1 - try: - repair_target = self.repair_sampler() - except Exception: - # if the repair sampler misbehaves, stop trying repairs - break - nearest_idx, nearest_vertex = self.get_nearest_vertex(repair_target) - cp_q_new, cp_q_new_grasping_pose, cp_q_new_found = self.get_q_new( - q_nearest_vertex=nearest_vertex, - q_rand=repair_target, - discretisation_steps=(self.discretisation_steps if self.discretisation_steps is not None else self.discretisation_steps), - max_delta_q=self.max_delta_q, - ) - if cp_q_new_found: - successful_extensions += 1 - self.add_edge(parent_idx=nearest_idx, new_q=cp_q_new, new_grasping_q=cp_q_new_grasping_pose) - # check connection to goal and return if found - if self.check_edge(cp_q_new, cp_q_new_grasping_pose, self.cubeplacementqgoal, - discretisation_steps=(self.discretisation_steps if self.discretisation_steps is not None else self.discretisation_steps)): - self.add_edge(parent_idx=len(self.nodes) - 1, new_q=self.cubeplacementqgoal, new_grasping_q=self.q_goal) - elapsed = time.perf_counter() - start_time - # attach metrics to self for later inspection - self.metrics = { - 'total_samples': total_samples, - 'successful_extensions': successful_extensions, - 'iterations': i + 1, - 'time_s': elapsed, - 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), - 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, - } - try: - self.metrics['profiling'] = self._profile.copy() - except Exception: - self.metrics['profiling'] = {} - try: - self.metrics['sampling'] = sampling.metrics.copy() - except Exception: - self.metrics['sampling'] = {} - logger.info("RRT found a path after %d iterations (%.3fs)", i + 1, elapsed) - logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", - total_samples, successful_extensions, self.metrics['success_rate'], self.metrics['samples_per_s']) - return self - break - if cp_q_new_found: successful_extensions += 1 self.add_edge(parent_idx=nearest_idx, new_q=cp_q_new, new_grasping_q=cp_q_new_grasping_pose) From e75caafff4ce27ebcc89e5526f3dbd294f165b20 Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Thu, 13 Nov 2025 15:49:23 +0000 Subject: [PATCH 19/23] Clean up some --- constants.py | 2 +- cube_trajectory_wrapper.py | 3 +- path.py | 32 +---- rrt.py | 177 ++----------------------- sampling.py | 263 ------------------------------------- 5 files changed, 16 insertions(+), 461 deletions(-) delete mode 100644 sampling.py diff --git a/constants.py b/constants.py index e8825f2..87873b3 100644 --- a/constants.py +++ b/constants.py @@ -26,7 +26,7 @@ DEFAULT_REPAIR_ATTEMPTS: int = 5 DEFAULT_GOAL_BIAS: float = 0.03 DEFAULT_MAX_IK_ATTEMPTS: int = 3 -DEFAULT_MAX_TIME_S: float = 30.0 +DEFAULT_MAX_TIME_S: float = 300 # Preset bundles for convenience: 'fast', 'default', 'robust' PRESETS = { diff --git a/cube_trajectory_wrapper.py b/cube_trajectory_wrapper.py index d536112..30d70ec 100644 --- a/cube_trajectory_wrapper.py +++ b/cube_trajectory_wrapper.py @@ -320,7 +320,8 @@ class GlobalMinJerkLinearJTraj: """ def __init__(self, waypoints, T_max, eps=1e-9): W = np.asarray(waypoints, float) - assert W.ndim == 2 and W.shape[0] >= 2 + # assert W.ndim == 2 and W.shape[0] >= 2 + print(W) self.W = W self.nq = W.shape[1] self.T = float(T_max) diff --git a/path.py b/path.py index 8344d60..1dc7dda 100644 --- a/path.py +++ b/path.py @@ -4,9 +4,7 @@ backwards compatibility for imports that expect `path.py`, it re-exports the cleaner implementations from `sampling.py` and `rrt.py`. """ -from sampling import random_cube_placement -import sampling as sampling_module -from rrt import Vertex, RRT, construct_rrt +from rrt import Vertex, RRT from constants import ( DEFAULT_NUM_ITER, DEFAULT_DISCRETISATION_STEPS, @@ -34,13 +32,6 @@ from pinocchio.utils import rotate from tools import setcubeplacement, distanceToObstacle -__all__ = [ - "random_cube_placement", - "Vertex", - "RRT", - "construct_rrt", -] - def computepath(qinit, qgoal, cubeplacementq0, @@ -54,8 +45,6 @@ def computepath(qinit, max_time_s: float = DEFAULT_MAX_TIME_S, post_path_wait: float = 5.0, random_sampler=None, - repair_sampler=None, - repair_max_attempts: int = 5, goal_bias: float = 0.02, viz=None, viz_delay: float = DEFAULT_VIZ_DELAY): @@ -73,9 +62,6 @@ def computepath(qinit, computeqgrasppose = computeqgrasppose or globals().get("computeqgrasppose") - # If no sampler supplied, build a constrained sampler on the vertical plane - # that intersects the initial and final cube placements. - sampling_box_ranges = None if random_sampler is None: p0 = np.array(cubeplacementq0.translation) p1 = np.array(cubeplacementqgoal.translation) @@ -148,11 +134,9 @@ def plane_sampler(): max_delta_q=MAX_DELTA_Q, cubeplacementqgoal=cubeplacementqgoal, q_goal=qgoal, - max_time_s=max_time_s, - progress_log_every=50, - goal_bias=goal_bias) + max_time_s=max_time_s) - rrt = rrt.run() + rrt.run() if not rrt: print( @@ -204,18 +188,11 @@ def displaypath(robot, cube, path, cube_placements, dt, viz): parser.add_argument('--no-viz', action='store_true', help='Disable visualization during planning') parser.add_argument('--discretisation', type=int, default=DEFAULT_DISCRETISATION_STEPS, help='Discretisation steps for interpolation') parser.add_argument('--num-iter', type=int, default=DEFAULT_NUM_ITER, help='Maximum RRT iterations/samples') - # smart sampler params - parser.add_argument('--smart-init-sigma', type=float, default=0.02, help='Initial sigma (m) for smart midpoint sampler') - parser.add_argument('--smart-scale', type=float, default=1.5, help='Sigma inflation factor for smart sampler') - parser.add_argument('--smart-max-sigma', type=float, default=0.2, help='Maximum sigma (m) for smart sampler') - parser.add_argument('--smart-max-attempts', type=int, default=30, help='Max attempts for smart sampler before fallback') parser.add_argument('--max-time', type=float, default=None, help='Maximum wall time (s) for planner (abort early)') parser.add_argument('--max-ik-attempts', type=int, default=None, help='Maximum IK iterations per interpolation point') parser.add_argument('--max-delta-q', type=float, default=None, help='Maximum delta (translation) per extension (meters)') parser.add_argument('--viz-delay', type=float, default=DEFAULT_VIZ_DELAY, help='Visualization delay in seconds') parser.add_argument('--progress-every', type=int, default=50, help='How many iterations between progress logs') - parser.add_argument('--repair-attempts', type=int, default=5, help='How many local repair attempts to try when an extension fails') - parser.add_argument('--smart-global', action='store_true', help='Use the smart sampler as the global sampler instead of repair-only') parser.add_argument('--goal-bias', type=float, default=0.02, help='Probability to sample the goal directly on each iteration (0.0-1.0)') parser.add_argument('--playback', action='store_true', help='Show the final path even if planning ran with --no-viz') parser.add_argument('--preset', choices=['fast', 'default', 'robust'], default=None, help='Use a preset bundle of planner defaults') @@ -254,8 +231,6 @@ def use_or_preset(flag_name, current_value): resolved_max_ik_attempts = use_or_preset('max-ik-attempts', args.max_ik_attempts if args.max_ik_attempts is not None else DEFAULT_MAX_IK_ATTEMPTS) resolved_max_time = use_or_preset('max-time', args.max_time if args.max_time is not None else DEFAULT_MAX_TIME_S) # choose sampler: either let computepath build the plane sampler, or build a smart sampler here - RANDOM_SAMPLER = None - REPAIR_SAMPLER = None path, cube_placements = computepath( q0, qe, CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET, @@ -264,7 +239,6 @@ def use_or_preset(flag_name, current_value): computeqgrasppose=computeqgrasppose, num_iter=resolved_num_iter, discretisation_steps=resolved_discretisation, - random_sampler=RANDOM_SAMPLER, goal_bias=resolved_goal_bias, viz=viz_obj, viz_delay=args.viz_delay, diff --git a/rrt.py b/rrt.py index d0d609a..1913420 100644 --- a/rrt.py +++ b/rrt.py @@ -16,7 +16,6 @@ from tools import setcubeplacement, distanceToObstacle from constants import DEFAULT_VIZ_DELAY import time -import sampling logger = logging.getLogger(__name__) @@ -54,9 +53,7 @@ def __init__(self, max_delta_q: Optional[float] = None, cubeplacementqgoal: Optional[pin.SE3] = None, q_goal: Optional[np.ndarray] = None, - max_time_s: Optional[float] = None, - progress_log_every: int = 50, - goal_bias: float = 0.02): + max_time_s: Optional[float] = None): self.robot = robot self.cube = cube @@ -76,17 +73,7 @@ def __init__(self, self.cubeplacementqgoal = cubeplacementqgoal self.q_goal = q_goal self.max_time_s = max_time_s - self.progress_log_every = progress_log_every - self.goal_bias = goal_bias # profiling counters for IK, viz and edge checks - self._profile = { - 'ik_calls': 0, - 'time_ik': 0.0, - 'viz_calls': 0, - 'time_viz': 0.0, - 'edge_checks': 0, - 'time_edge_checks': 0.0, - } @staticmethod def calc_distance(q1, q2) -> float: @@ -135,7 +122,6 @@ def get_nearest_vertex(self, q: pin.SE3) -> Tuple[int, Vertex]: def get_q_new(self, q_nearest_vertex: Vertex, q_rand: pin.SE3, - discretisation_steps: Optional[int] = None, max_delta_q: Optional[float] = None) -> Tuple[pin.SE3, Optional[np.ndarray], bool]: q_new_found = True @@ -148,11 +134,10 @@ def get_q_new(self, grasping_q = None if self.get_grasping_poseq is not None: # allow caller to override discretisation for this call; otherwise use instance default - discretisation_steps = discretisation_steps if discretisation_steps is not None else self.discretisation_steps - dt = 1.0 / discretisation_steps + dt = 1.0 / self.discretisation_steps # seed IK with the nearest vertex grasping pose and then update the seed seed_grasping_q = q_nearest_vertex.grasping_q - for i in range(1, discretisation_steps): + for i in range(1, self.discretisation_steps): q = self.lerp(q_nearest_vertex.q, q_end, dt * i) # optionally update visualization so user sees the robot attempt if self.viz is not None: @@ -165,8 +150,6 @@ def get_q_new(self, if hasattr(self.viz, 'render'): self.viz.render() dt_v = time.perf_counter() - t_v - self._profile['viz_calls'] += 1 - self._profile['time_viz'] += dt_v time.sleep(self.viz_delay) # Use the IK wrapper to call the IK callback in a @@ -183,8 +166,6 @@ def get_q_new(self, viz_sleep=False, ) (grasping_q, found), dt_ik = call_ik(self.get_grasping_poseq, **ik_kwargs) - self._profile['ik_calls'] += 1 - self._profile['time_ik'] += dt_ik # update seed for the next interpolation step if IK succeeded if found and grasping_q is not None: seed_grasping_q = grasping_q @@ -197,19 +178,16 @@ def get_q_new(self, return q_end, grasping_q, q_new_found def check_edge(self, q_latest_new: pin.SE3, q_latest_new_grasping_pose: Optional[np.ndarray], - q_goal: pin.SE3, discretisation_steps: Optional[int] = None) -> bool: + q_goal: pin.SE3) -> bool: # Try to progress from q_latest_new to q_goal using get_q_new semantics dummy_vertex = Vertex(q=q_latest_new, grasping_q=q_latest_new_grasping_pose, parent=None) t_edge = time.perf_counter() q_new, q_new_grasping_q, q_new_found = self.get_q_new( q_nearest_vertex=dummy_vertex, q_rand=q_goal, - discretisation_steps=discretisation_steps, max_delta_q=None, ) dt_edge = time.perf_counter() - t_edge - self._profile['edge_checks'] += 1 - self._profile['time_edge_checks'] += dt_edge if not q_new_found: return False distance_to_target = self.calc_distance(q_new, q_goal) @@ -223,8 +201,7 @@ def get_path_from_rrt(self) -> List[Vertex]: last = last.parent return path - def get_shortcut_from_rrt_path(self, discretisation_steps: int = DEFAULT_DISCRETISATION_STEPS, - max_ik_attempts: Optional[int] = None) -> List[Vertex]: + def get_shortcut_from_rrt_path(self) -> List[Vertex]: path_vertices = self.get_path_from_rrt() if len(path_vertices) <= 2: return path_vertices @@ -233,11 +210,9 @@ def get_shortcut_from_rrt_path(self, discretisation_steps: int = DEFAULT_DISCRET start_idx = 0 while start_idx < len(path_vertices) - 1: current_vertex = path_vertices[start_idx] - # Try to find the furthest vertex we can directly connect to found_idx = None for next_idx in range(len(path_vertices) - 1, start_idx, -1): - if self.check_edge(current_vertex.q, current_vertex.grasping_q, path_vertices[next_idx].q, - discretisation_steps=discretisation_steps): + if self.check_edge(current_vertex.q, current_vertex.grasping_q, path_vertices[next_idx].q): found_idx = next_idx break if found_idx is None: @@ -265,162 +240,30 @@ def run(self) -> Optional['RRT']: successful_extensions = 0 start_time = time.perf_counter() - # reset sampling module metrics for this run - try: - sampling.metrics = { - 'samples': 0, - 'geom_updates': 0, - 'collision_checks': 0, - 'time_collision_checks': 0.0, - 'time_viz': 0.0, - 'time_sampling_loop': 0.0, - } - except Exception: - pass - for i in range(self.num_iter): - # Periodic progress logging so long runs show activity - if self.progress_log_every and i % self.progress_log_every == 0 and i > 0: - elapsed_now = time.perf_counter() - start_time - samples_per_s_now = total_samples / elapsed_now if elapsed_now > 0 else float('inf') - logger.info("RRT progress: %d/%d iterations, samples: %d, elapsed: %.2fs, samples/s: %.2f", - i, self.num_iter, total_samples, elapsed_now, samples_per_s_now) - # Abort the run if we've exceeded a user-specified wall-time budget if self.max_time_s is not None: elapsed_now = time.perf_counter() - start_time if elapsed_now > self.max_time_s: - logger.warning("RRT aborted after %.2fs (max_time_s=%.2fs) — returning no path", elapsed_now, self.max_time_s) - # include profiling into self.metrics before returning - try: - self.metrics = { - 'total_samples': total_samples, - 'successful_extensions': successful_extensions, - 'iterations': i, - 'time_s': elapsed_now, - 'samples_per_s': total_samples / elapsed_now if elapsed_now > 0 else float('inf'), - 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, - } - self.metrics['profiling'] = self._profile.copy() - self.metrics['sampling'] = sampling.metrics.copy() - except Exception: - pass + print("Stopped because of time") return None - # With small probability sample the goal directly (goal bias) - import random as _random - if self.goal_bias and _random.random() < self.goal_bias: - random_cp_q = self.cubeplacementqgoal - else: - total_samples += 1 - random_cp_q = self.random_sampler() + total_samples += 1 + random_cp_q = self.random_sampler() nearest_idx, nearest_vertex = self.get_nearest_vertex(random_cp_q) cp_q_new, cp_q_new_grasping_pose, cp_q_new_found = self.get_q_new( q_nearest_vertex=nearest_vertex, q_rand=random_cp_q, - discretisation_steps=(self.discretisation_steps if self.discretisation_steps is not None else self.discretisation_steps), max_delta_q=self.max_delta_q, ) - # If the extension succeeded use it. Otherwise, optionally try a - # local repair sampler (smart sampler) a few times before continuing. if cp_q_new_found: - successful_extensions += 1 self.add_edge(parent_idx=nearest_idx, new_q=cp_q_new, new_grasping_q=cp_q_new_grasping_pose) - if self.check_edge(cp_q_new, cp_q_new_grasping_pose, self.cubeplacementqgoal, discretisation_steps=(self.discretisation_steps if self.discretisation_steps is not None else self.discretisation_steps)): + if self.check_edge(cp_q_new, cp_q_new_grasping_pose, self.cubeplacementqgoal): self.add_edge(parent_idx=len(self.nodes) - 1, new_q=self.cubeplacementqgoal, new_grasping_q=self.q_goal) - elapsed = time.perf_counter() - start_time - # attach metrics to self for later inspection - self.metrics = { - 'total_samples': total_samples, - 'successful_extensions': successful_extensions, - 'iterations': i + 1, - 'time_s': elapsed, - 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), - 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, - } - # attach profiling collected by the RRT and sampling modules - try: - self.metrics['profiling'] = self._profile.copy() - except Exception: - self.metrics['profiling'] = {} - try: - self.metrics['sampling'] = sampling.metrics.copy() - except Exception: - self.metrics['sampling'] = {} - logger.info("RRT found a path after %d iterations (%.3fs)", i + 1, elapsed) - logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", - total_samples, successful_extensions, self.metrics['success_rate'], self.metrics['samples_per_s']) return self - elapsed = time.perf_counter() - start_time - # include profiling even on failure - try: - self.metrics = { - 'total_samples': total_samples, - 'successful_extensions': successful_extensions, - 'iterations': self.num_iter, - 'time_s': elapsed, - 'samples_per_s': total_samples / elapsed if elapsed > 0 else float('inf'), - 'success_rate': successful_extensions / total_samples if total_samples > 0 else 0.0, - } - self.metrics['profiling'] = self._profile.copy() - self.metrics['sampling'] = sampling.metrics.copy() - except Exception: - pass - logger.info("RRT completed %d iterations without connecting to goal (%.3fs)", self.num_iter, elapsed) - logger.info("Samples: %d, successful extensions: %d, success rate: %.3f, samples/s: %.1f", - total_samples, successful_extensions, (successful_extensions / total_samples) if total_samples > 0 else 0.0, - total_samples / elapsed if elapsed > 0 else float('inf')) return None - - -def construct_rrt(robot: pin.robot_wrapper.RobotWrapper, - cube: pin.robot_wrapper.RobotWrapper, - q_init: np.ndarray, - q_goal: np.ndarray, - cubeplacementq0: pin.SE3, - cubeplacementqgoal: pin.SE3, - num_iter: int, - random_sampler: Callable, - max_delta_q: Optional[float] = None, - discretisation_steps: int = DEFAULT_DISCRETISATION_STEPS, - get_grasping_poseq: Optional[Callable] = None, - viz=None, - viz_delay: float = DEFAULT_VIZ_DELAY, - max_ik_attempts: Optional[int] = None, - max_time_s: Optional[float] = None, - progress_log_every: int = 50, - repair_sampler: Optional[Callable] = None, - repair_max_attempts: int = 5, - goal_bias: float = 0.02) -> Optional[RRT]: - """Construct a simple RRT that samples cube placements using `random_sampler`. - - This is intentionally straightforward: we sample, extend toward the sample, - and stop when we can connect to the goal. - """ - # instantiate with run-time parameters and delegate to instance.run() - rrt = RRT(root_q=cubeplacementq0, - root_grasping_q=q_init, - robot=robot, - cube=cube, - get_grasping_poseq=get_grasping_poseq, - viz=viz, - viz_delay=viz_delay, - discretisation_steps=discretisation_steps, - max_ik_attempts=max_ik_attempts, - num_iter=num_iter, - random_sampler=random_sampler, - max_delta_q=max_delta_q, - cubeplacementqgoal=cubeplacementqgoal, - q_goal=q_goal, - max_time_s=max_time_s, - progress_log_every=progress_log_every, - repair_sampler=repair_sampler, - repair_max_attempts=repair_max_attempts, - goal_bias=goal_bias) - - return rrt.run() diff --git a/sampling.py b/sampling.py deleted file mode 100644 index fcd8a8e..0000000 --- a/sampling.py +++ /dev/null @@ -1,263 +0,0 @@ -#!/usr/bin/env python3 -"""Sampling helpers for cube placement on a table. - -This module contains pure sampling logic (no RRT) so it can be read, -tested, and understood independently. -""" -from typing import Tuple, Callable - -import logging -import numpy as np -import pinocchio as pin -from pinocchio.utils import rotate - -from tools import setcubeplacement -from constants import DEFAULT_TABLE_Z_RANGE, DEFAULT_CHECK_COLLISIONS, DEFAULT_VIZ_DELAY, SAMPLE_DISPLAY_EVERY -import time - -logger = logging.getLogger(__name__) - -# Lightweight profiling counters populated during sampling calls. -# Accessible as sampling.metrics after running the sampler. -metrics = { - 'samples': 0, - 'geom_updates': 0, - 'collision_checks': 0, - 'time_collision_checks': 0.0, - 'time_viz': 0.0, - 'time_sampling_loop': 0.0, -} - - -def get_table_borders(table: pin.robot_wrapper.RobotWrapper) -> Tuple[np.array, np.array]: - """Return the axis-aligned bounding box (min, max) of the table collision surface. - - Args: - table: pinocchio RobotWrapper for the table which contains geometry information. - - Returns: - (xyz_min, xyz_max): two length-3 numpy arrays with world-frame corner coordinates. - """ - # ensure geometry placements are up-to-date - pin.updateGeometryPlacements(table.model, table.data, table.collision_model, table.collision_data) - table_surface = table.collision_model.geometryObjects[0].geometry - oMtable_surface = table.collision_data.oMg[0] - R, t = oMtable_surface.rotation, oMtable_surface.translation - - half = table_surface.halfSide - local_corners = np.array([[sx, sy, sz] - for sx in [+1, -1] - for sy in [+1, -1] - for sz in [+1, -1]]) * half - world_corners = (R @ local_corners.T).T + t - - xyz_min = world_corners.min(axis=0) - xyz_max = world_corners.max(axis=0) - - logger.debug("Table borders: min=%s max=%s", xyz_min, xyz_max) - return xyz_min, xyz_max - - -def random_cube_placement( - robot: pin.robot_wrapper.RobotWrapper, - cube: pin.robot_wrapper.RobotWrapper, - x_range: Tuple[float, float], - y_range: Tuple[float, float], - z_range: Tuple[float, float] = DEFAULT_TABLE_Z_RANGE, - check_collisions: bool = DEFAULT_CHECK_COLLISIONS, - sampler: Callable[[], float] = None, - viz=None, - viz_delay: float = DEFAULT_VIZ_DELAY, - display_every: int = SAMPLE_DISPLAY_EVERY, - collision_check_every: int = 1, -) -> pin.SE3: - """Sample a random placement (SE3) for the cube on the table surface. - - The function places the cube at a random (x,y) inside the table AABB and a - z sampled from `z_range`. It sets the cube placement on the robot via - `setcubeplacement` and optionally checks for collisions. - - Args: - robot, cube, table: pinocchio wrappers used for updating placements. - check_collisions: if True, verify the placement is collision-free. - z_range: tuple (z_min, z_max) for sampling height above world origin. - sampler: optional callable returning a float in [0,1] used to sample x/y. - - Returns: - A pin.SE3 placement that is collision-free (if check_collisions) or the - last sampled placement if checks are disabled. - """ - sampler = sampler or np.random.random - - sample_count = 0 - last_placement = None - while True: - x = np.random.uniform(x_range[0], x_range[1]) - y = np.random.uniform(y_range[0], y_range[1]) - z = np.random.uniform(z_range[0], z_range[1]) - - t_loop = time.perf_counter() - placement = pin.SE3(rotate('z', 0.0), np.array([x, y, z])) - last_placement = placement - - sample_count += 1 - metrics['samples'] += 1 - - # only update the visualiser occasionally to avoid blocking - if viz is not None and (sample_count % display_every) == 0: - t_v = time.perf_counter() - q_disp = getattr(robot, 'q', None) - if q_disp is not None: - viz.display(q_disp) - else: - if hasattr(viz, 'render'): - viz.render() - dt_v = time.perf_counter() - t_v - metrics['time_viz'] += dt_v - time.sleep(viz_delay) - - if not check_collisions: - metrics['time_sampling_loop'] += time.perf_counter() - t_loop - return last_placement - - # perform the (more expensive) geometry update and collision check only every N samples - if (sample_count % max(1, collision_check_every)) != 0: - # don't call geometry updates for this sample, keep sampling - metrics['time_sampling_loop'] += time.perf_counter() - t_loop - continue - - # now set placement on the scene and do collision checking - from tools import set_and_check_cube_placement - metrics['geom_updates'] += 1 - has_collision, dt_coll = set_and_check_cube_placement(robot, cube, last_placement) - metrics['collision_checks'] += 1 - metrics['time_collision_checks'] += dt_coll - metrics['time_sampling_loop'] += time.perf_counter() - t_loop - if not has_collision: - logger.debug("Found collision-free placement after %d samples", sample_count) - return last_placement - # otherwise keep sampling - logger.debug("Sampled placement in collision at sample %d, retrying", sample_count) - - -def make_smart_midpoint_sampler(robot: pin.robot_wrapper.RobotWrapper, - cube: pin.robot_wrapper.RobotWrapper, - p0: pin.SE3, - p1: pin.SE3, - initial_sigma: float = 0.02, - sigma_scale: float = 1.5, - max_sigma: float = 0.2, - max_attempts: int = 30, - fallback_sampler: Callable[[], pin.SE3] = None, - anisotropic: bool = True, - z_range: Tuple[float, float] = DEFAULT_TABLE_Z_RANGE, - viz=None, - viz_delay: float = DEFAULT_VIZ_DELAY) -> Callable[[], pin.SE3]: - """Create a sampler that prefers the straight-line midpoint between p0 and p1. - - The returned callable takes no arguments and returns a collision-free - pin.SE3 placement. It first tries the exact midpoint, then samples from a - Gaussian around the midpoint with adaptive variance inflation until a - collision-free placement is found or max_attempts is reached. If nothing - found, calls fallback_sampler if provided. - """ - # Precompute useful values - p0_xy = np.array(p0.translation)[:2] - p1_xy = np.array(p1.translation)[:2] - mid_xy = (p0_xy + p1_xy) / 2.0 - mid_z = (np.array(p0.translation)[2] + np.array(p1.translation)[2]) / 2.0 - - seg = p1_xy - p0_xy - L = np.linalg.norm(seg) - if L < 1e-6: - u = np.array([1.0, 0.0]) - else: - u = seg / L - perp = np.array([-u[1], u[0]]) - - def sampler() -> pin.SE3: - # try exact midpoint first - placement = pin.SE3(np.eye(3), np.array([mid_xy[0], mid_xy[1], mid_z])) - # do a timed collision check for the midpoint - from tools import set_and_check_cube_placement - metrics['geom_updates'] += 1 - has_collision, dt = set_and_check_cube_placement(robot, cube, placement) - metrics['collision_checks'] += 1 - metrics['time_collision_checks'] += dt - if not has_collision: - return placement - - # deterministic-offset-first: try a small set of deterministic offsets - # (along the segment and perpendicular) before doing stochastic sampling. - attempts_total = 1 - # choose a small grid of offsets based on initial_sigma - base_eps = max(1e-4, initial_sigma) - eps_factors = [0.5, 1.0, 2.0] - det_offsets = [] - for f in eps_factors: - eps = base_eps * f - # along, perp, and diagonal combinations - det_offsets.extend([ - (eps, 0.0), (-eps, 0.0), - (0.0, eps), (0.0, -eps), - (eps, eps), (eps, -eps), (-eps, eps), (-eps, -eps) - ]) - - for along_eps, perp_eps in det_offsets: - if attempts_total >= max_attempts: - break - xy = mid_xy + u * along_eps + perp * perp_eps - # bias z toward midpoint - zmin, zmax = z_range - z = np.clip(np.random.normal(mid_z, initial_sigma * 0.5), zmin, zmax) - placement = pin.SE3(np.eye(3), np.array([xy[0], xy[1], z])) - from tools import set_and_check_cube_placement - metrics['geom_updates'] += 1 - has_collision, dt = set_and_check_cube_placement(robot, cube, placement) - metrics['collision_checks'] += 1 - metrics['time_collision_checks'] += dt - attempts_total += 1 - if not has_collision: - logger.debug("Smart sampler found placement on deterministic offset after %d attempts", attempts_total) - return placement - - sigma = initial_sigma - attempts = attempts_total - while attempts < max_attempts: - attempts += 1 - # anisotropic sampling in the xy-plane: smaller along segment, larger perpendicular - if anisotropic: - sigma_along = max(1e-6, sigma * 0.5) - sigma_perp = sigma - r_along = np.random.normal(0.0, sigma_along) - r_perp = np.random.normal(0.0, sigma_perp) - xy = mid_xy + u * r_along + perp * r_perp - else: - xy = mid_xy + np.random.normal(0.0, sigma, size=2) - - # sample z from the provided z_range, but bias toward midpoint z - zmin, zmax = z_range - z = np.clip(np.random.normal(mid_z, sigma), zmin, zmax) - - placement = pin.SE3(np.eye(3), np.array([xy[0], xy[1], z])) - from tools import set_and_check_cube_placement - metrics['geom_updates'] += 1 - has_collision, dt = set_and_check_cube_placement(robot, cube, placement) - metrics['collision_checks'] += 1 - metrics['time_collision_checks'] += dt - if not has_collision: - logger.debug("Smart sampler found placement after %d attempts (sigma=%.4f)", attempts, sigma) - return placement - - # increase variance and try again - sigma = min(max_sigma, sigma * sigma_scale) - - # fallback if provided - if fallback_sampler is not None: - logger.debug("Smart sampler failing after %d attempts; using fallback sampler", attempts) - return fallback_sampler() - - # as a last resort return the midpoint (may be colliding) - return placement - - return sampler From 7c4e3ce7db976f168bde7c8e209933d19c695a22 Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Thu, 13 Nov 2025 16:57:48 +0000 Subject: [PATCH 20/23] Format main files --- control.py | 48 ++++++++++++++++++++++----------------------- inverse_geometry.py | 21 +++++++++----------- tools.py | 40 ++++++++++++++++++------------------- 3 files changed, 52 insertions(+), 57 deletions(-) diff --git a/control.py b/control.py index b0a9ff6..3ea58bf 100644 --- a/control.py +++ b/control.py @@ -18,7 +18,7 @@ from config import LEFT_HAND, RIGHT_HAND from qp_utils import get_bezier_control_points import pybullet as pyb - + # in my solution these gains were good enough for all joints but you might want to tune this. Kp = 1000 # proportional gain (P of PD) Kv = 30 * np.sqrt(Kp) # derivative gain (D of PD) @@ -27,7 +27,7 @@ Dx_lin = 5 * np.sqrt(Kx_lin) Kx_rot = 1000 -Dx_rot = 10 +Dx_rot = 10 # Whether to use Bezier curve for trajectory generation USE_BEZIER = False @@ -44,7 +44,7 @@ def controllaw(sim, robot, trajs, tcurrent): q_target = trajs[0](tcurrent) vq_target = trajs[1](tcurrent) vvq_target = trajs[2](tcurrent) - + vvq_des = vvq_target + Kp * (q_target - q) + Kv * (vq_target - vq) M = robot.mass(q) h = robot.nle(q, vq) @@ -68,22 +68,22 @@ def controllaw(sim, robot, trajs, tcurrent): oMLhand_desired = data.oMf[left_id] # From joint trajectory oMRhand_desired = data.oMf[right_id] - + pin.forwardKinematics(model, data, q, vq) pin.updateFramePlacements(model, data) pin.computeJointJacobians(model, data, q) - + # Jacobians - J_l = pin.computeFrameJacobian(model, data, q, left_id, + J_l = pin.computeFrameJacobian(model, data, q, left_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) J_r = pin.computeFrameJacobian(model, data, q, right_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) - + oMLhand_actual = data.oMf[left_id] oMRhand_actual = data.oMf[right_id] error_pos_hands = oMLhand_actual.translation - oMRhand_actual.translation - + # Orientation errors R_err_l = oMLhand_desired.rotation @ oMLhand_actual.rotation.T err_rot_l = pin.log3(R_err_l) @@ -92,13 +92,13 @@ def controllaw(sim, robot, trajs, tcurrent): err_rot_r = pin.log3(R_err_r) # Velocities - v_l = pin.getFrameVelocity(model, data, left_id, + v_l = pin.getFrameVelocity(model, data, left_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) vel_l = np.hstack([v_l.linear, v_l.angular]) v_r = pin.getFrameVelocity(model, data, right_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) vel_r = np.hstack([v_r.linear, v_r.angular]) - + # Compute correction forces F_l = np.hstack([ -Kx_lin * error_pos_hands - Dx_lin * vel_l[:3], @@ -108,7 +108,7 @@ def controllaw(sim, robot, trajs, tcurrent): Kx_lin * error_pos_hands - Dx_lin * vel_r[:3], Kx_rot * err_rot_r - Dx_rot * vel_r[3:] ]) - + # Map to joint torques tau_l = J_l[:, :].T @ F_l tau_r = J_r[:, :].T @ F_r @@ -119,17 +119,17 @@ def controllaw(sim, robot, trajs, tcurrent): # Combine: base tracking + end-effector correction tau_total = tau_base + tau_r + tau_l sim.step(tau_total) - + if __name__ == "__main__": - + from tools import setupwithpybullet, setupwithpybulletandmeshcat, rununtil from config import DT - from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET + from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET from inverse_geometry import computeqgrasppose from path import computepath - + robot, sim, cube = setupwithpybullet() - + q0, successinit = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT, None) qe, successend = computeqgrasppose(robot, robot.q0, cube, CUBE_PLACEMENT_TARGET, None) @@ -168,7 +168,7 @@ def maketraj_with_joint_space_linear( joint_space_trajectory.derivative(1), joint_space_trajectory.derivative(2) ) - + total_time=10.0 if USE_BEZIER: @@ -183,15 +183,13 @@ def maketraj_with_joint_space_linear( ) tcur = 0. - + while tcur < total_time: rununtil( - controllaw, - DT, - sim, - robot, - trajs, + controllaw, + DT, + sim, + robot, + trajs, tcur) tcur += DT - - \ No newline at end of file diff --git a/inverse_geometry.py b/inverse_geometry.py index 0cd20f4..0cbcb55 100644 --- a/inverse_geometry.py +++ b/inverse_geometry.py @@ -6,7 +6,7 @@ @author: stonneau """ -import pinocchio as pin +import pinocchio as pin import numpy as np from numpy.linalg import pinv,inv,norm,svd,eig from tools import collision, getcubeplacement, setcubeplacement, jointlimitsviolated @@ -34,7 +34,7 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None, max_attempts: lhand_id = robot.model.getFrameId(LEFT_HAND) rhand_id = robot.model.getFrameId(RIGHT_HAND) - + oMlhook = getcubeplacement(cube, LEFT_HOOK) oMrhook = getcubeplacement(cube, RIGHT_HOOK) @@ -49,7 +49,7 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None, max_attempts: lhandMlhook = oMlhand.inverse()*oMlhook rhandMrhook = oMrhand.inverse()*oMrhook - + nu_L = pin.log(lhandMlhook).vector nu_R = pin.log(rhandMrhook).vector @@ -82,25 +82,22 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None, max_attempts: viz.display(q) if viz_sleep: time.sleep(0.1) - + if lerror < EPSILON and rerror < EPSILON and not collision(robot, q) and not jointlimitsviolated(robot, q): success = True break - + return q, success - + if __name__ == "__main__": from tools import setupwithmeshcat from setup_meshcat import updatevisuals robot, cube, viz = setupwithmeshcat() - + q = robot.q0.copy() - + q0,successinit = computeqgrasppose(robot, q, cube, CUBE_PLACEMENT, viz) qe,successend = computeqgrasppose(robot, q, cube, CUBE_PLACEMENT_TARGET, viz) - + updatevisuals(viz, robot, cube, q0) - - - diff --git a/tools.py b/tools.py index 9ad021c..6c94e79 100644 --- a/tools.py +++ b/tools.py @@ -26,14 +26,14 @@ def collision(robot, q): '''Return true if in collision, false otherwise.''' pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q) # if pin.computeCollisions(robot.collision_model,robot.collision_data,False): - # for k in range(len(robot.collision_model.collisionPairs)): + # for k in range(len(robot.collision_model.collisionPairs)): # cr = robot.collision_data.collisionResults[k] # cp = robot.collision_model.collisionPairs[k] # if cr.isCollision(): # print("collision pair:",robot.collision_model.geometryObjects[cp.first].name,",",robot.collision_model.geometryObjects[cp.second].name,"- collision:","Yes" if cr.isCollision() else "No") - + return pin.computeCollisions(robot.collision_model,robot.collision_data,False) - + def distanceToObstacle(robot, q): '''Return the shortest distance between robot and the obstacle. ''' geomidobs = robot.collision_model.getGeometryId('obstaclebase_0') @@ -41,8 +41,8 @@ def distanceToObstacle(robot, q): pairs = [i for i, pair in enumerate(robot.collision_model.collisionPairs) if pair.second == geomidobs or pair.second == geomidtable] pin.framesForwardKinematics(robot.model,robot.data,q) pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q) - dists = [pin.computeDistance(robot.collision_model, robot.collision_data, idx).min_distance for idx in pairs] - + dists = [pin.computeDistance(robot.collision_model, robot.collision_data, idx).min_distance for idx in pairs] + # pairsId = [pair.first for i, pair in enumerate(robot.collision_model.collisionPairs) if pair.second == geomidobs or pair.second == geomidtable] # names = [robot.collision_model.geometryObjects[idx].name for idx in pairsId ] # for name, dist in zip(names,dists): @@ -50,27 +50,27 @@ def distanceToObstacle(robot, q): # print(min (dists)) return min(dists) - + def getcubeplacement(cube, hookname = None): oMf = cube.collision_model.geometryObjects[0].placement if hookname is not None: frameid = cube.model.getFrameId(hookname) - oMf *= cube.data.oMf[frameid] + oMf *= cube.data.oMf[frameid] return oMf - + def setcubeplacement(robot, cube, oMf): q = cube.q0 robot.visual_model.geometryObjects[-1].placement = oMf robot.collision_model.geometryObjects[-1].placement = oMf cube.visual_model.geometryObjects[-1].placement = oMf - cube.collision_model.geometryObjects[0].placement = oMf + cube.collision_model.geometryObjects[0].placement = oMf pin.updateGeometryPlacements(cube.model,cube.data,cube.collision_model,cube.collision_data,q) - - + + from setup_pinocchio import setuppinocchio -from setup_meshcat import setupmeshcat +from setup_meshcat import setupmeshcat from config import MESHCAT_URL def setupwithmeshcat(url=MESHCAT_URL): @@ -78,27 +78,27 @@ def setupwithmeshcat(url=MESHCAT_URL): robot, table, obstacle, cube = setuppinocchio() viz = setupmeshcat(robot, url) return robot, cube, viz - + from setup_pybullet import setuppybullet def setupwithpybullet(): '''setups everything to work with the robot and pybullet''' - robot, table, obstacle, cube = setuppinocchio() + robot, table, obstacle, cube = setuppinocchio() sim = setuppybullet(robot) sim.setTorqueControlMode() return robot, sim, cube - - + + def setupwithpybulletandmeshcat(url=MESHCAT_URL): '''setups everything to work with the robot, pybullet AND meshcat''' - robot, table, obstacle, cube = setuppinocchio() + robot, table, obstacle, cube = setuppinocchio() viz = setupmeshcat(robot) sim = setuppybullet(robot) sim.setTorqueControlMode() return robot, sim, cube, viz - + import time - + def rununtil(f, t, *args, **kwargs): '''starts a timer, runs a function f then waits until t seconds have passed since timer started''' t = time.perf_counter() @@ -107,4 +107,4 @@ def rununtil(f, t, *args, **kwargs): t+=0.001 while(time.perf_counter()-t<0.001): time.sleep(0.0001) # Weird loop for parallel execution (should not be needed in this project) - return result \ No newline at end of file + return result From fb80f33b8a0bcc7bab15747caa84aaa8c36b8469 Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Thu, 13 Nov 2025 18:21:16 +0000 Subject: [PATCH 21/23] Add duct tape --- config.py | 8 ++++---- control.py | 2 +- rrt.py | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/config.py b/config.py index b3e51ee..c9f1d54 100644 --- a/config.py +++ b/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' diff --git a/control.py b/control.py index 3ea58bf..003df0b 100644 --- a/control.py +++ b/control.py @@ -157,7 +157,7 @@ def maketraj(points, T): def maketraj_with_joint_space_linear( waypoints: List[np.array], - T: int): + T: float): joint_space_trajectory = GlobalMinJerkLinearJTraj( waypoints=waypoints, T_max=T, diff --git a/rrt.py b/rrt.py index 1913420..201b1d6 100644 --- a/rrt.py +++ b/rrt.py @@ -8,6 +8,7 @@ from typing import Callable, Tuple, List, Optional import logging +from inverse_geometry import computeqgrasppose import numpy as np import pinocchio as pin from pinocchio.utils import rotate @@ -156,7 +157,7 @@ def get_q_new(self, # backwards-compatible way and time the call. We set # viz_sleep=False so display calls (if any) don't sleep. from ik_utils import call_ik - ik_kwargs = dict( + grasping_q, found = computeqgrasppose( robot=self.robot, qcurrent=seed_grasping_q, cube=self.cube, @@ -165,7 +166,6 @@ def get_q_new(self, max_attempts=(self.max_ik_attempts if self.max_ik_attempts is not None else 1000), viz_sleep=False, ) - (grasping_q, found), dt_ik = call_ik(self.get_grasping_poseq, **ik_kwargs) # update seed for the next interpolation step if IK succeeded if found and grasping_q is not None: seed_grasping_q = grasping_q From 5016eb90d5c3216537a03ef29ee3b6316724e8a4 Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Thu, 13 Nov 2025 20:38:42 +0000 Subject: [PATCH 22/23] Can't remember --- constants.py | 6 ++++-- ik_utils.py | 30 ------------------------------ qp_utils.py | 46 ++++++++++++++++++++++------------------------ rrt.py | 32 +++++++++++++++++++++----------- 4 files changed, 47 insertions(+), 67 deletions(-) delete mode 100644 ik_utils.py diff --git a/constants.py b/constants.py index 87873b3..c59eab8 100644 --- a/constants.py +++ b/constants.py @@ -5,7 +5,7 @@ from typing import Tuple # Sampling (table / cube placement) -DEFAULT_TABLE_Z_RANGE: Tuple[float, float] = (0.93, 1.3) +DEFAULT_TABLE_Z_RANGE: Tuple[float, float] = (0.93, 2) DEFAULT_CHECK_COLLISIONS: bool = True # RRT / planning @@ -20,7 +20,7 @@ # 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.02 +DEFAULT_PLANE_HALF_WIDTH: float = 0.3 # Planner-specific defaults (tunable) DEFAULT_REPAIR_ATTEMPTS: int = 5 @@ -28,6 +28,8 @@ 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': { diff --git a/ik_utils.py b/ik_utils.py deleted file mode 100644 index 9a2fb16..0000000 --- a/ik_utils.py +++ /dev/null @@ -1,30 +0,0 @@ -"""IK helper utilities. - -Provides a thin wrapper `call_ik` that calls an IK callback with a -backwards-compatible set of kwargs, times the call, and returns -((q, success), dt). The wrapper will inspect the callback signature and -only pass supported keyword arguments so older callbacks remain compatible. -""" -from typing import Callable, Any, Tuple -import time -import inspect - - -def call_ik(callback: Callable[..., Any], /, **kwargs) -> Tuple[Any, float]: - """Call an IK callback while timing it and keeping backwards compatibility. - - The callback is expected to return a tuple like (q, success). `kwargs` - may include: robot, qcurrent, cube, cubetarget, viz, max_attempts, viz_sleep. - - Returns: - (result, dt) where `result` is whatever the callback returned and dt is - the wall-time seconds spent in the call. - """ - sig = inspect.signature(callback) - # Filter kwargs to what the callback actually accepts - call_kwargs = {k: v for k, v in kwargs.items() if k in sig.parameters} - - t0 = time.perf_counter() - res = callback(**call_kwargs) - dt = time.perf_counter() - t0 - return res, dt diff --git a/qp_utils.py b/qp_utils.py index 17193f1..7098b7f 100644 --- a/qp_utils.py +++ b/qp_utils.py @@ -3,9 +3,9 @@ from typing import List, Dict, Tuple def assign_time_to_intermediate_points(path: List[np.array]) -> Dict[int, float]: - # assign time to each point in the path + # assign time to each point in the path # so we can use it in QP cost function - # compute time proportional to distance between points + # compute time proportional to distance between points total_dist = 0 point_idx_time = {} @@ -14,11 +14,11 @@ def assign_time_to_intermediate_points(path: List[np.array]) -> Dict[int, float] dist = np.linalg.norm(path[i] - path[i-1]) total_dist += dist point_idx_time[i] = dist - + total_dist += np.linalg.norm(path[-1] - path[-2]) norm_dist = np.cumsum(list(point_idx_time.values()) / total_dist) for point_idx, time in point_idx_time.items(): - point_idx_time[point_idx] = norm_dist[point_idx-1] + point_idx_time[point_idx] = norm_dist[point_idx-1] return point_idx_time @@ -53,9 +53,9 @@ def quadprog_solve_qp(H, q, G=None, h=None, C=None, d=None, verbose=False): def bernstein_deg4(t: float) -> np.array: """Quartic Bernstein basis [B0..B4] at scalar t.""" - # as the equation looks like this + # as the equation looks like this # p(t) = (1-t)^4*p0 + 4(1-t)^3*t*p1 + 6(1-t)^2*t^2*p2 + 4(1-t)*t^3*p3 + t^4*p4 - # so we need to compute the coefficients for each term + # so we need to compute the coefficients for each term # B0 = (1-t)^4 # B1 = 4*(1-t)^3*t # B2 = 6*(1-t)^2*t^2 @@ -71,10 +71,10 @@ def bernstein_deg4(t: float) -> np.array: return np.array([B0, B1, B2, B3, B4], dtype=np.float64) def build_H_q_quartic_bezier( - q0: np.array, - qe: np.array, - t_samples: List[float], - g_samples: List[np.array], + q0: np.array, + qe: np.array, + t_samples: np.array, + g_samples: np.array, l2_reg: float = 0.0 ) -> Tuple[np.array, np.array]: @@ -86,7 +86,7 @@ def build_H_q_quartic_bezier( """ ## p(t)=P0​B0​ + (P1​B1 ​+ P2​B2 ​+ P3​B3 ​) + P4​B4 - ## need to be p(t) = Ax + b + ## need to be p(t) = Ax + b ## where b = P0​B0​ + P4​B4 ## variables are [P1_dimensions, P2_dimensions, P3_dimensions] @@ -99,7 +99,7 @@ def build_H_q_quartic_bezier( ## [0, 0, 0, 0, ...., B3, 0, 0, 0, ...., 0, 0, 0, 0, ...., B3] ## ] - # we need to compute the coefficients for each term + # we need to compute the coefficients for each term t_samples = np.asarray(t_samples, dtype=np.float64).ravel() g_samples = np.asarray(g_samples, dtype=np.float64) assert g_samples.shape == (t_samples.size, 15) @@ -111,8 +111,8 @@ def build_H_q_quartic_bezier( H = np.zeros((nvars, nvars), dtype=np.float64) q = np.zeros(nvars, dtype=np.float64) - # samples in the computed trajectory we want to fit - # Should look like this + # samples in the computed trajectory we want to fit + # Should look like this # ||p(t) - g(t)||^2 # ||A x + b - g(t)||^2 # ||A x - (g(t) - b)||^2 @@ -120,8 +120,8 @@ def build_H_q_quartic_bezier( # 1/2 x.T A.T A x + (A.T (b - g(t))) x for ti, gi in zip(t_samples, g_samples): - B = bernstein_deg4(ti) - b = B[0] * q0 + B[-1] * qe + B = bernstein_deg4(ti) + b = B[0] * q0 + B[-1] * qe A_i = np.concatenate([np.eye(d) * b for b in B[1:-1]], axis=1)# (15, 45): per-dim applies same B across control blocks H += A_i.T @ A_i # accumulate quadratic term q += A_i.T @ (b - gi) # accumulate linear term @@ -134,7 +134,7 @@ def build_H_q_quartic_bezier( def accel_equalities_known_endpoints( - q0: np.array, + q0: np.array, qe: np.array ) -> Tuple[np.array, np.array]: d = 15 @@ -146,12 +146,12 @@ def accel_equalities_known_endpoints( def construct_collision_constraints( - discretization_steps, + discretization_steps, distance_to_obstacle_function, distance_to_obstacle_threshold): - ## we need to construct inequality constraints for the collision avoidance - ## form of inequality constraint is + ## we need to construct inequality constraints for the collision avoidance + ## form of inequality constraint is ## distance_to_obstacle_function(x) <= distance_to_obstacle_threshold ## distance_function(p(t)) <= distance_to_obstacle_threshold ## distance_function(Ax+b) <= distance_to_obstacle_threshold @@ -162,8 +162,8 @@ def construct_collision_constraints( def get_bezier_control_points( path: List[np.array]) -> List[np.array]: - # assign specific times to intermediate points - # this is based on the distance between points in the path + # assign specific times to intermediate points + # this is based on the distance between points in the path point_times = assign_time_to_intermediate_points(path) q0 = path[0] @@ -186,5 +186,3 @@ def get_bezier_control_points( control_points.append(res[i:i+15]) return [q0] + control_points + [qe] - - diff --git a/rrt.py b/rrt.py index 201b1d6..8a61406 100644 --- a/rrt.py +++ b/rrt.py @@ -5,6 +5,7 @@ placements (pin.SE3) and optional grasping-pose callbacks. The implementation is intentionally minimal and readable. """ +from turtle import distance from typing import Callable, Tuple, List, Optional import logging @@ -13,7 +14,7 @@ import pinocchio as pin from pinocchio.utils import rotate -from constants import EDGE_DISTANCE_TOL, DEFAULT_DISCRETISATION_STEPS, DEFAULT_CHECK_EDGE_STEPS +from constants import EDGE_DISTANCE_TOL, DEFAULT_DISCRETISATION_STEPS, MIN_DIST_TO_OBS from tools import setcubeplacement, distanceToObstacle from constants import DEFAULT_VIZ_DELAY import time @@ -24,10 +25,21 @@ from dataclasses import dataclass +def distance_to_obstacle_only(robot, cube, q): + '''Return the shortest distance between robot and the obstacle. ''' + geomidobs = robot.collision_model.getGeometryId('baseLink_0') + pairs = [i for i, pair in enumerate(robot.collision_model.collisionPairs) if pair.second == geomidobs] + pin.framesForwardKinematics(robot.model,robot.data,q) + pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q) + dists = [pin.computeDistance(robot.collision_model, robot.collision_data, idx).min_distance for idx in pairs] + + return min(dists) + + @dataclass class Vertex: q: pin.SE3 - grasping_q: Optional[np.ndarray] + grasping_q: np.ndarray parent: Optional['Vertex'] = None @@ -156,7 +168,6 @@ def get_q_new(self, # Use the IK wrapper to call the IK callback in a # backwards-compatible way and time the call. We set # viz_sleep=False so display calls (if any) don't sleep. - from ik_utils import call_ik grasping_q, found = computeqgrasppose( robot=self.robot, qcurrent=seed_grasping_q, @@ -166,28 +177,27 @@ def get_q_new(self, max_attempts=(self.max_ik_attempts if self.max_ik_attempts is not None else 1000), viz_sleep=False, ) - # update seed for the next interpolation step if IK succeeded if found and grasping_q is not None: seed_grasping_q = grasping_q - if not found or distanceToObstacle(self.robot, grasping_q) < 0.001: - # cannot progress past the previous valid step + if not found or distance_to_obstacle_only(self.robot, self.cube, grasping_q) < MIN_DIST_TO_OBS: if i - 1 == 0: q_new_found = False return q_nearest_vertex.q, q_nearest_vertex.grasping_q, False return self.lerp(q_nearest_vertex.q, q_end, dt * (i - 1)), grasping_q, True return q_end, grasping_q, q_new_found - def check_edge(self, q_latest_new: pin.SE3, q_latest_new_grasping_pose: Optional[np.ndarray], - q_goal: pin.SE3) -> bool: - # Try to progress from q_latest_new to q_goal using get_q_new semantics + def check_edge( + self, + q_latest_new: pin.SE3, + q_latest_new_grasping_pose: Optional[np.ndarray], + q_goal: pin.SE3 + ) -> bool: dummy_vertex = Vertex(q=q_latest_new, grasping_q=q_latest_new_grasping_pose, parent=None) - t_edge = time.perf_counter() q_new, q_new_grasping_q, q_new_found = self.get_q_new( q_nearest_vertex=dummy_vertex, q_rand=q_goal, max_delta_q=None, ) - dt_edge = time.perf_counter() - t_edge if not q_new_found: return False distance_to_target = self.calc_distance(q_new, q_goal) From a058a71a40d97c6ca6dc7a83b1967724904c3a30 Mon Sep 17 00:00:00 2001 From: Rory Collins Date: Fri, 14 Nov 2025 09:51:48 +0000 Subject: [PATCH 23/23] Remove turtle module --- rrt.py | 1 - 1 file changed, 1 deletion(-) diff --git a/rrt.py b/rrt.py index 8a61406..58f2a3b 100644 --- a/rrt.py +++ b/rrt.py @@ -5,7 +5,6 @@ placements (pin.SE3) and optional grasping-pose callbacks. The implementation is intentionally minimal and readable. """ -from turtle import distance from typing import Callable, Tuple, List, Optional import logging