Integrate monicas code

Author: rrwwee

constants.py

@@ -3,14 +3,15 @@
 Place all tunable "magic numbers" here so they are easy to discover and tweak.
 """
 from typing import Tuple
+import numpy as np
 
 # Sampling (table / cube placement)
-DEFAULT_TABLE_Z_RANGE: Tuple[float, float] = (0.93, 1.3)
+DEFAULT_TABLE_Z_RANGE: Tuple[float, float] = (0.93, 1.4)
 DEFAULT_CHECK_COLLISIONS: bool = True
 
 # RRT / planning
 DEFAULT_NUM_ITER: int = 500
-DEFAULT_DISCRETISATION_STEPS: int = 8
+DEFAULT_DISCRETISATION_STEPS: int = 100
 DEFAULT_CHECK_EDGE_STEPS: int = 1000
 EDGE_DISTANCE_TOL: float = 1e-3
 
@@ -28,27 +29,37 @@
 DEFAULT_MAX_IK_ATTEMPTS: int = 3
 DEFAULT_MAX_TIME_S: float = 30.0
 
+# Control gains
+Kp = 1000               # proportional gain (P of PD)
+Kv = 30 * np.sqrt(Kp)   # derivative gain (D of PD)
+
+Kx_lin = 1000  # Very stiff position control
+Dx_lin = 5 * np.sqrt(Kx_lin)
+
+Kx_rot = 1000
+Dx_rot = 1 * np.sqrt(Kx_rot)
+
 # Preset bundles for convenience: 'fast', 'default', 'robust'
 PRESETS = {
 	'fast': {
-		'num_iter': 200,
-		'discretisation': 8,
+		'num_iter': 500,
+		'discretisation': 100,
 		'repair_attempts': 3,
 		'goal_bias': 0.02,
 		'max_ik_attempts': 3,
 		'max_time_s': 15.0,
 	},
 	'default': {
-		'num_iter': 500,
-		'discretisation': 8,
+		'num_iter': 1000,
+		'discretisation': 100,
 		'repair_attempts': 5,
 		'goal_bias': 0.03,
 		'max_ik_attempts': 3,
 		'max_time_s': 30.0,
 	},
 	'robust': {
 		'num_iter': 1000,
-		'discretisation': 10,
+		'discretisation': 100,
 		'repair_attempts': 8,
 		'goal_bias': 0.05,
 		'max_ik_attempts': 5,

control.py

@@ -15,18 +15,9 @@
 from cube_trajectory_wrapper import GlobalMinJerkLinearJTraj
 from tools import setcubeplacement
 from config import LEFT_HAND, RIGHT_HAND
+from constants import Kp, Kv, Kx_lin, Dx_lin, Kx_rot, Dx_rot
 
 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)
-
-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):
     """Joint trajectory tracking with end-effector force correction"""
@@ -118,7 +109,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)
@@ -162,6 +153,8 @@ def maketraj_with_joint_space_linear(
 
     total_time=10.
 
+    print(path, len(path))
+
     trajs = maketraj_with_joint_space_linear(
         waypoints=path,
         T=total_time

inverse_geometry.py

@@ -10,7 +10,7 @@
 import numpy as np
 from numpy.linalg import pinv,inv,norm,svd,eig
 import time
-from tools import collision, getcubeplacement, setcubeplacement, projecttojointlimits
+from tools import collision, getcubeplacement, setcubeplacement, projecttojointlimits, jointlimitsviolated
 from config import LEFT_HOOK, RIGHT_HOOK, LEFT_HAND, RIGHT_HAND, EPSILON
 from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET
 
@@ -82,7 +82,7 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None, max_attempts:
             if viz_sleep:
                 time.sleep(0.1)
 
-        if lerror < EPSILON and rerror < EPSILON and not collision(robot, q):
+        if lerror < EPSILON and rerror < EPSILON and not collision(robot, q) and not jointlimitsviolated(robot, q):
             success = True
             break