add qp with bezier curves

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_):

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"""
@@ -124,9 +133,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,
@@ -139,8 +148,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)
@@ -161,17 +169,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.
 

inverse_geometry.py

@@ -9,8 +9,8 @@
 import pinocchio as pin 
 import numpy as np
 from numpy.linalg import pinv,inv,norm,svd,eig
+from tools import collision, getcubeplacement, setcubeplacement, jointlimitsviolated
 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
 
@@ -72,8 +72,8 @@ def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None, max_attempts:
 
         q = pin.integrate(robot.model,q, vq)
 
-        lerror = norm(oMlhand.translation - oMlhook.translation)
-        rerror = norm(oMrhand.translation - oMrhook.translation)
+        lerror = norm(oMlhand.translation - oMlhook.translation) + norm(oMlhand.rotation - oMlhook.rotation)
+        rerror = norm(oMrhand.translation - oMrhook.translation) + norm(oMrhand.rotation - oMrhook.rotation)
 
         if viz:
             # allow callers to request that we display without sleeping so the
@@ -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
 

path.py

@@ -30,14 +30,17 @@
 
 logger = logging.getLogger(__name__)
 
+from typing import Callable, Tuple, List
+from pinocchio.utils import rotate
+from tools import setcubeplacement, distanceToObstacle
+
 __all__ = [
     "random_cube_placement",
     "Vertex",
     "RRT",
     "construct_rrt",
 ]
 
-
 def computepath(qinit,
                 qgoal,
                 cubeplacementq0,

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]
+
+