cm

python/examples/cart_pulling.py

@@ -194,8 +194,18 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
     if past_data['priority_register'][-1][1] == '1' and priority_register[1] == '0': 
         # create straight line path from ee to base
         p_cart = q[:2]
+        if robot.robot_name != "yumi":
             p_ee = T_w_e.translation[:2]
             straigh_line_path = np.linspace(p_ee, p_cart, args.past_window_size)
+        else:
+            p_ee_l = T_w_e_left.translation[:2]
+            p_ee_r = T_w_e_right.translation[:2]
+        # MASSIVE TODO: have two different REFERENCES FOR TWO ARMS
+        # EVILLLLLLLLLLLLLLLLLLLLLLLLLLll
+        # MASSIVE TODO: have two different REFERENCES FOR TWO ARMS
+        # MASSIVE TODO: have two different REFERENCES FOR TWO ARMS
+        # MASSIVE TODO: have two different REFERENCES FOR TWO ARMS
+            straigh_line_path = np.linspace(p_ee_l, p_cart, args.past_window_size)
         # time it the same way the base path is timed 
         time_past = np.linspace(0.0, args.past_window_size * robot.dt, args.past_window_size)
         s = np.linspace(0.0, args.n_knots * args.ocp_dt, args.past_window_size)