diff --git a/python/examples/.cart_pulling.py.swp b/python/examples/.cart_pulling.py.swp index 1e3db6e2e2ea153f62b9d8f4e40c73cfd5e6ea9b..b7c97396aebcf2267653da4c60596a8c43fb31f2 100644 Binary files a/python/examples/.cart_pulling.py.swp and b/python/examples/.cart_pulling.py.swp differ diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py index 86dca387084348ab27f3434fe4dc061fc6fe11fd..66b33e798ae32937abc7fea661ea359407526c69 100644 --- a/python/examples/cart_pulling.py +++ b/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] - p_ee = T_w_e.translation[:2] - straigh_line_path = np.linspace(p_ee, p_cart, args.past_window_size) + 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) diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp index 3dd35d54e6b1f3bcee9bb7b2e5175e414d2190fc..4f63782936cd661ef1ccba5b4d9e81eb3873b0a1 100644 Binary files a/python/ur_simple_control/clik/.clik.py.swp and b/python/ur_simple_control/clik/.clik.py.swp differ