diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index 5687c6fe08ff5ad0c6bb92f2ddbe38f8b8b54e8a..0480c90f7723d3c4b934a55b59c7d3e9b4aa13e0 100644
--- a/python/examples/cart_pulling.py
+++ b/python/examples/cart_pulling.py
@@ -87,8 +87,8 @@ def areDualGrippersRelativeToBaseOK(args, goal_transform, robot):
     #grasp_pose = T_w_base.act(rotate.act(translate))
     grasp_pose = T_w_base.act(translate.act(rotate))
 
-    grasp_pose_left = grasp_pose.act(goal_transform)
-    grasp_pose_right = grasp_pose.act(goal_transform.inverse())
+    grasp_pose_left = goal_transform.act(grasp_pose)
+    grasp_pose_right = goal_transform.inverse().act(grasp_pose)
 
     T_w_e_left, T_w_e_right = robot.getT_w_e()
     SEerror_left = T_w_e_left.actInv(grasp_pose_left)
@@ -100,9 +100,9 @@ def areDualGrippersRelativeToBaseOK(args, goal_transform, robot):
     #if np.linalg.norm(err_vector) < robot.args.goal_error:
     if (np.linalg.norm(err_vector_left) < 2*1e-1) and (np.linalg.norm(err_vector_right) < 2*1e-1):
         isOK = True
-    return isOK, grasp_pose
+    return isOK, grasp_pose, grasp_pose_left, grasp_pose_right
 
-def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solver_pulling,
+def cartPullingControlLoop(args, robot : RobotManager, goal, goal_transform, solver_grasp, solver_pulling,
                            path_planner : ProcessManager, i : int, past_data):
     """
     cartPulling
@@ -128,8 +128,7 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
     if robot.robot_name != "yumi":
         graspOK, grasp_pose = isGripperRelativeToBaseOK(args, robot)
     else:
-        goal_transform = pin.SE3.Identity()
-        graspOK, grasp_pose = areDualGrippersRelativeToBaseOK(args, goal_transform, robot)
+        graspOK, grasp_pose, grasp_pose_left, grasp_pose_right = areDualGrippersRelativeToBaseOK(args, goal_transform, robot)
     # NOTE: this keeps getting reset after initial grasp has been completed.
     # and we want to let mpc cook
     priority_register[1] = str(int(not graspOK)) # set if not ok
@@ -161,14 +160,13 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
                 if robot.robot_name != "yumi":
                     runningModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
                 else:
-                    # MASSIVE TODO: CREATE A DIFFERENT REFERENCE FOR EACH ARM
-                    runningModel.differential.costs.costs['gripperPose_l'].cost.residual.reference = grasp_pose
-                    runningModel.differential.costs.costs['gripperPose_r'].cost.residual.reference = grasp_pose
+                    runningModel.differential.costs.costs['gripperPose_l'].cost.residual.reference = grasp_pose_left
+                    runningModel.differential.costs.costs['gripperPose_r'].cost.residual.reference = grasp_pose_right
             if robot.robot_name != "yumi":
                 solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
             else:
-                solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose_l'].cost.residual.reference = grasp_pose
-                solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose_r'].cost.residual.reference = grasp_pose
+                solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose_l'].cost.residual.reference = grasp_pose_left
+                solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose_r'].cost.residual.reference = grasp_pose_right
 
             robot.Mgoal = grasp_pose
             #breakFlag, save_past_item, log_item = CrocoIKMPCControlLoop(args, robot, solver_grasp, i, past_data)
@@ -181,7 +179,6 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
                 controlLoopClikArmOnly(robot, clikController, i, past_data)
             else:
                 # TODO: DEFINE SENSIBLE TRANSFOR
-                goal_transform = pin.SE3.Identity()
                 controlLoopClikDualArmsOnly(robot, clikController, goal_transform, i, past_data)
 
     # case 2)
@@ -205,6 +202,8 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
         # 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
+# ----------------> should be doable by just hitting the path with the goal_transform for dual.
+#                   in the whole task there are no differences in left and right arm
             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)
@@ -218,7 +217,7 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
 
     if priority_int < 2:
         # TODO make this one work
-        breakFlag, save_past_item, log_item = BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, grasp_pose, i, past_data)
+        breakFlag, save_past_item, log_item = BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, grasp_pose, goal_transform, i, past_data)
         #BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, i, past_data)
 
     p = q[:2]
@@ -234,6 +233,13 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
 
 
 def cartPulling(args, robot : RobotManager, goal, path_planner):
+    # transfer single arm to dual arm reference
+    # (instead of gripping the middle grip slightly away from middle)
+    goal_transform = pin.SE3.Identity()
+# TODO: it's cursed and it shouldn't be
+#    goal_transform.rotation = pin.rpy.rpyToMatrix(0.0, np.pi/2, 0.0)
+# TODO: it's cursed and it shouldn't be
+#    goal_transform.translation[1] = -0.1
     ############################
     #  setup cart-pulling mpc  #
     ############################
@@ -261,7 +267,7 @@ def cartPulling(args, robot : RobotManager, goal, path_planner):
     us_init = solver_grasp.problem.quasiStatic([x0] * solver_grasp.problem.T)
     solver_grasp.solve(xs_init, us_init, args.max_solver_iter)
     
-    controlLoop = partial(cartPullingControlLoop, args, robot, goal, solver_grasp, solver_pulling, path_planner)
+    controlLoop = partial(cartPullingControlLoop, args, robot, goal, goal_transform, solver_grasp, solver_pulling, path_planner)
 
     log_item = {}
     q = robot.getQ()
diff --git a/python/examples/clik.py b/python/examples/clik.py
index 1c3a4d7faf5ce88f54eaf4e0ae1a401b2f95688c..e58580b571226ccefe078663ce9ea6a6ba59de04 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -30,12 +30,13 @@ if __name__ == "__main__":
                     {"Mgoal" : Mgoal})
     else:
         Mgoal = robot.defineGoalPointCLI()
+        Mgoal.rotation = np.eye(3)
     #compliantMoveL(args, robot, Mgoal)
     if robot.robot_name != "yumi":
         moveL(args, robot, Mgoal)
     else:
         goal_transform = pin.SE3.Identity()
-        #goal_transform.translation[1] = 0.1
+        goal_transform.translation[1] = 0.1
         moveLDualArm(args, robot, Mgoal, goal_transform)
     robot.closeGripper()
     robot.openGripper()
diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py
index d29008e8b02e99e69ae4171ede21d462252ef8c4..f5f8e14ef2be8fe14c4986eff85da3b837c71940 100644
--- a/python/examples/crocoddyl_mpc.py
+++ b/python/examples/crocoddyl_mpc.py
@@ -35,6 +35,13 @@ if __name__ == "__main__":
     # TODO: put this back for nicer demos
     #Mgoal = robot.defineGoalPointCLI()
     Mgoal = pin.SE3.Random()
+    if robot.robot_name == "yumi":
+        Mgoal.rotation = np.eye(3)
+        Mgoal_transform = pin.SE3.Identity()
+        Mgoal_transform.translation[1] = 0.1
+        Mgoal_left = Mgoal_transform.act(Mgoal)
+        Mgoal_right = Mgoal_transform.inverse().act(Mgoal)
+        Mgoal = (Mgoal_left, Mgoal_right)
 
     robot.Mgoal = Mgoal.copy()
     if args.visualize_manipulator:
diff --git a/python/examples/dummy_cmds_pub.py b/python/examples/dummy_cmds_pub.py
new file mode 100755
index 0000000000000000000000000000000000000000..a3aa1517a82a9f95a16170b63e3e5cf5c52feec1
--- /dev/null
+++ b/python/examples/dummy_cmds_pub.py
@@ -0,0 +1,76 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2024, ABB Schweiz AG
+# All rights reserved.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
+# IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+# FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+# CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+# WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
+# WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+import numpy as np
+
+import rclpy
+from sensor_msgs.msg import JointState
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+
+from abb_python_utilities.names import get_rosified_name
+
+
+class DummyNode(Node):
+    def __init__(self):
+        super().__init__("dummy_cmds_pub_node")
+
+        self._cb = ReentrantCallbackGroup()
+
+        self._ns = get_rosified_name(self.get_namespace())
+        # test
+        self._ns = "maskinn"
+
+        self._cmd_pub = self.create_publisher(JointState, f"{self._ns}/joints_cmd", 1)
+
+        self.get_logger().info(f"### Starting dummy example under namespace {self._ns}")
+
+        self.empty_msg = JointState()
+        for i in range(29):
+            self.empty_msg.velocity.append(0.0)
+
+        self._T = 1.0
+        self._A = 0.2
+        self._t = 0.0
+        self._dt = 0.004
+
+        self._pub_timer = self.create_timer(self._dt, self.send_cmd)
+
+    def send_cmd(self):
+        msg = self.empty_msg
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.velocity[0] = 1.0
+        msg.velocity[1] = 1.0
+        msg.velocity[2] = 1.0
+        msg.velocity[3] = 1.0
+        msg.velocity[4] = 1.0
+        msg.velocity[5] = 1.0
+        msg.velocity[21] = self._A * np.sin(2 * np.pi / self._T * self._t)
+        msg.velocity[28] = self._A * np.sin(2 * np.pi / self._T * self._t)
+        self._cmd_pub.publish(msg)
+        #print("published")
+        self._t += self._dt
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    executor = MultiThreadedExecutor()
+    node = DummyNode()
+    executor.add_node(node)
+    executor.spin()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/python/examples/smc_yumi_challenge.py b/python/examples/smc_yumi_challenge.py
index 83fe5d3b169afb94ca477a6aa6dd7c2f0bd3f4b9..2af6a7c911be229f85c081adbf7beb389e99b317 100644
--- a/python/examples/smc_yumi_challenge.py
+++ b/python/examples/smc_yumi_challenge.py
@@ -10,9 +10,17 @@ from abb_python_utilities.names import get_rosified_name
 import numpy as np
 import argcomplete, argparse
 from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, controlLoopCompliantClik, invKinmQP
+from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, controlLoopCompliantClik, invKinmQP, moveLDualArm
 from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
 from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoIKMPC
+import pinocchio as pin
+
+
+###########################################################
+# this is how to get the positions  ros2 topic echo /maskinn/auxiliary/joint_states_merged
+# this is how to send commands: /maskinn/auxiliary/robot_description
+#self._cmd_pub = self.create_publisher(JointState, f"{self._ns}/joints_cmd", 1)
+#############################################33
 
 
 def get_args():
@@ -21,6 +29,7 @@ def get_args():
     of various kinds. Make sure you know what the goal is before you run!'
     parser = getClikArgs(parser)
     parser = get_OCP_args(parser)
+    parser.add_argument('--ros-namespace', type=str, default="maskinn", help="you MUST put in ros namespace you're using")
     argcomplete.autocomplete(parser)
     args = parser.parse_args()
     return args
@@ -28,32 +37,40 @@ def get_args():
 class ROSCommHandlerMYumi(Node):
 
     def __init__(self, args, robot_manager : RobotManager, loop_manager : ControlLoopManager):
-        super().__init__('ROSCommHandlerHeron')
+        super().__init__('ROSCommHandlerMYumi')
         
-        self._ns = get_rosified_name(self.get_namespace())
+        # does not work
+        #self._ns = get_rosified_name(self.get_namespace())
+        # so we pass it as an argument
+        self._ns = args.ros_namespace + "/"
         self.get_logger().info(f"### Starting dummy example under namespace {self._ns}")
         
-        self.pub_vel_base = self.create_publisher(Twist, f"{self._ns}/platform/base_command", 1)
-        self.pub_vel_left = self.create_publisher(JointState, f"{self._ns}/platform/left_arm_command", 1)
-        self.pub_vel_right = self.create_publisher(JointState, f"{self._ns}/platform/right_arm_command", 1)
+        #self.pub_vel_base = self.create_publisher(Twist, f"{self._ns}platform/base_command", 1)
+        #self.pub_vel_base = self.create_publisher(Twist, f"{self._ns}platform/command_limiter_node/base/cmd_vel_in_navigation", 1)
+        #self.pub_vel_left = self.create_publisher(JointState, f"{self._ns}platform/left_arm_command", 1)
+        #self.pub_vel_right = self.create_publisher(JointState, f"{self._ns}platform/right_arm_command", 1)
+
         robot_manager.set_publisher_vel_base(self.pub_vel_base)
-        robot_manager.set_publiser_vel_left(self.pub_vel_left)
-        robot_manager.set_publiser_vel_right(self.pub_vel_right)
+        robot_manager.set_publisher_vel_left(self.pub_vel_left)
+        robot_manager.set_publisher_vel_right(self.pub_vel_right)
         
         self.robot_manager = robot_manager
         self.loop_manager = loop_manager
         self.args = args
         
+        # give me the latest thing even if there wasn't an update
         qos_prof = rclpy.qos.QoSProfile(
             reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE,
             durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL,
             history = rclpy.qos.HistoryPolicy.KEEP_LAST,
             depth = 1)
+
         # MASSIVE TODO: you want to be subsribed to a proper localization thing,
         # specifically to output from robot_localization
-        self.sub_base_odom = self.create_subscription(Odometry, f"{self._ns}/platform/odometry", self.callback_base_odom, qos_prof)
+        self.sub_base_odom = self.create_subscription(Odometry, f"{self._ns}platform/odometry", self.callback_base_odom, qos_prof)
         # self.sub_amcl = self.create_subscription(PoseWithCovarianceStamped, '/amcl_pose', self.pose_callback, qos_prof)
-        self.sub_arms_state = self.create_subscription(JointState, f"{self._ns}/platform/joint_states", self.callback_arms_state, qos_prof)
+        self.sub_arms_state = self.create_subscription(JointState, f"{self._ns}platform/joint_states", self.callback_arms_state, qos_prof)
+
         self.names_left = {f"{self._ns}_yumi_robl_joint_{i+1}": i for i in range(7)}
         self.state_left = JointState(name=list(sorted(self.names_left.keys())),
                                      position=[0] * 7,
@@ -143,7 +160,15 @@ def main(args=None):
     args = get_args()
     robot = RobotManager(args)
     goal = robot.defineGoalPointCLI()
-    loop_manager = CrocoIKMPC(args, robot, goal, run=False)
+    if robot.robot_name == "yumi":
+        goal.rotation = np.eye(3)
+        goal_transform = pin.SE3.Identity()
+        goal_transform.translation[1] = 0.1
+        goal_left = goal_transform.act(goal)
+        goal_left = goal_transform.inverse().act(goal)
+#        goal = (goal_left, goal_right)
+    #loop_manager = CrocoIKMPC(args, robot, goal, run=False)
+    loop_manager = moveLDualArm(args, robot, goal, goal_transform, run=False)
     
     executor = MultiThreadedExecutor()
     ros_comm_handler = ROSCommHandlerMYumi(args, robot, loop_manager)
diff --git a/python/ur_simple_control.egg-info/SOURCES.txt b/python/ur_simple_control.egg-info/SOURCES.txt
index bf3ed08977ae3f5773625f56e8e8db1824a507a8..3780070e45f466b3f46fda3c2e6c4d8217f70215 100644
--- a/python/ur_simple_control.egg-info/SOURCES.txt
+++ b/python/ur_simple_control.egg-info/SOURCES.txt
@@ -22,7 +22,6 @@ examples/comparing_logs_example.py
 examples/crocoddyl_mpc.py
 examples/crocoddyl_ocp_clik.py
 examples/drawing_from_input_drawing.py
-examples/heron_pls.py
 examples/joint_trajectory.csv
 examples/path_following_mpc.py
 examples/path_in_pixels.csv
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..d0587e1010aa996ade5bcbe92c010aa75a4c6838
Binary files /dev/null and b/python/ur_simple_control/.managers.py.swp differ
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc
index 013bf4c3c931ca6dca9274fbad730bf76f14dc35..b346ab48b3eb5b0bdef663970c9cca16de1e2b0f 100644
Binary files a/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index 3b476c272538d9a1f12c522c5ae7d32bfa054cde..8344427e05627676833671090b0d83c03dc1c605 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc differ
diff --git a/python/ur_simple_control/basics/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/basics/__pycache__/__init__.cpython-310.pyc
index 8edaa2f577ddfd74d815dee53f8265d38c0f2fca..bc4e2818b028c9a713a54e1a7e4d188e056df7b9 100644
Binary files a/python/ur_simple_control/basics/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/basics/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc
index ecf89b920431584195c255ccc99637ca09fd2338..bd525f88ef2485c470b94afc01afe489688494ed 100644
Binary files a/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc and b/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc differ
diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
deleted file mode 100644
index 4f63782936cd661ef1ccba5b4d9e81eb3873b0a1..0000000000000000000000000000000000000000
Binary files a/python/ur_simple_control/clik/.clik.py.swp and /dev/null differ
diff --git a/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc
index c14ed132923a1fd4fe37a2eea6179541d994dc02..46908832b061d8500ffd3ddedf0c04ef12d83254 100644
Binary files a/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index c643261a19bc6f3276416a7d846fc0f4c9fbfd94..2c0434e5bec720deb4fe6e7709e436607ccbe6f2 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -205,8 +205,11 @@ def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform
     q = robot.getQ()
     T_w_e_left, T_w_e_right = robot.getT_w_e()
     # 
-    Mgoal_left = robot.Mgoal.act(goal_transform)
-    Mgoal_right = robot.Mgoal.act(goal_transform.inverse())
+    Mgoal_left = goal_transform.act(robot.Mgoal)
+    Mgoal_right = goal_transform.inverse().act(robot.Mgoal)
+    #print("robot.Mgoal", robot.Mgoal)
+    #print("Mgoal_left", Mgoal_left)
+    #print("Mgoal_right", Mgoal_right)
 
     SEerror_left = T_w_e_left.actInv(Mgoal_left)
     SEerror_right = T_w_e_right.actInv(Mgoal_right)
@@ -219,6 +222,9 @@ def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform
     J_left = pin.computeFrameJacobian(robot.model, robot.data, q, robot.l_ee_frame_id)
     J_right = pin.computeFrameJacobian(robot.model, robot.data, q, robot.r_ee_frame_id)
     # compute the joint velocities based on controller you passed
+    # this works exactly the way you think it does:
+    # the velocities for the other arm are 0
+    # what happens to the base hasn't been investigated but it seems ok 
     qd_left = clik_controller(J_left, err_vector_left)
     qd_right = clik_controller(J_right, err_vector_right)
     qd = qd_left + qd_right
@@ -231,6 +237,27 @@ def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform
     # hence the empty dict
     return breakFlag, {}, log_item
 
+#def clikDualArm(args, robot, goal, goal_transform, run=True):
+#    """
+#    clikDualArm
+#    -----------
+#    """
+#    robot.Mgoal = copy.deepcopy(goal)
+#    clik_controller = getClikController(args)
+#    controlLoop = partial(controlLoopClikDualArm, robot, clik_controller, goal_transform)
+#    # we're not using any past data or logging, hence the empty arguments
+#    log_item = {
+#            'qs' : np.zeros(robot.model.nq),
+#            'dqs' : np.zeros(robot.model.nv),
+#            'dqs_cmd' : np.zeros(robot.model.nv),
+#        }
+#    save_past_dict = {}
+#    loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+#    if run:
+#        loop_manager.run()
+#    else:
+#        return loop_manager
+
 def controlLoopClikArmOnly(robot, clik_controller, i, past_data):
     breakFlag = False
     log_item = {}
@@ -335,7 +362,7 @@ def moveL(args, robot : RobotManager, goal_point):
     loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
     loop_manager.run()
 
-def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform):
+def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform, run=True):
     """
     moveL
     -----
@@ -354,7 +381,10 @@ def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform):
             'dqs_cmd' : np.zeros(robot.model.nv),
         }
     loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
-    loop_manager.run()
+    if run:
+        loop_manager.run()
+    else:
+        return loop_manager
 
 # TODO: implement
 def moveLFollowingLine(args, robot, goal_point):
diff --git a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc
index 8f623ffcc78243df2a5ee3e73e7c0e561efecc48..e2215d22c3eadbb5fecdbc20c727397dd704569b 100644
Binary files a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc
index e81144824ae642b3e14a08a19d1224855ba643bb..e35fbc28222a465baaca0356405c91375c026a23 100644
Binary files a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc and b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc differ
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 3b5c8bf58f00147ff3ab8a80a1c1262b7e5e1655..48467bbdf32b98a0fbce034211a61f277ba5f70f 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -936,7 +936,16 @@ class RobotManager:
         if self.robot_name == "yumi":
             qd_cmd = np.clip(qd, -1 * self.model.velocityLimit, self.model.velocityLimit)
             self.v_q = qd_cmd
-            self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
+            if self.args.pinocchio_only:
+                self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
+            else:
+                qd_base = qd[:3]
+                qd_left = qd[3:10]
+                qd_right = qd[10:]
+                self.publisher_vel_base(qd_base)
+                self.publisher_vel_left(qd_left)
+                self.publisher_vel_right(qd_right)
+
             
 
 
@@ -1073,7 +1082,16 @@ class RobotManager:
 
     def set_publisher_vel_base(self, publisher_vel_base):
         self.publisher_vel_base = publisher_vel_base
-        print("set publisher into robotmanager")
+        print("set vel_base_publisher into robotmanager")
+
+    def set_publisher_vel_left(self, publisher_vel_left):
+        self.publisher_vel_left = publisher_vel_left
+        print("set vel_left_publisher into robotmanager")
+
+    def set_publisher_vel_right(self, publisher_vel_right):
+        self.publisher_vel_right = publisher_vel_right
+        print("set vel_right_publisher into robotmanager")
+
 
 class ProcessManager:
     """
diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp
deleted file mode 100644
index 007e01f6afc624b2a6efd9ea7c340e95ac661700..0000000000000000000000000000000000000000
Binary files a/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp and /dev/null differ
diff --git a/python/ur_simple_control/optimal_control/__init__.py b/python/ur_simple_control/optimal_control/__init__.py
index adbd050942478d9957c0384029c66b8e7ac3dd66..1feaec8e968dc9d22865791a63b24db8077af2d7 100644
--- a/python/ur_simple_control/optimal_control/__init__.py
+++ b/python/ur_simple_control/optimal_control/__init__.py
@@ -1,10 +1,10 @@
 import importlib.util
-if importlib.util.find_spec('casadi'):
-    import pinocchio as pin
-    if int(pin.__version__[0]) < 3:
-        print("you need to have pinocchio version 3.0.0 or greater to use pinocchio.casadi!")
-        exit()
-    from .create_pinocchio_casadi_ocp import *
+#if importlib.util.find_spec('casadi'):
+#    import pinocchio as pin
+#    if int(pin.__version__[0]) < 3:
+#        print("you need to have pinocchio version 3.0.0 or greater to use pinocchio.casadi!")
+#        exit()
+#    from .create_pinocchio_casadi_ocp import *
 from .crocoddyl_mpc import *
 from .crocoddyl_optimal_control import *
 from .get_ocp_args import *
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index d20301eb404c1f8456b6ef53cca7ce6de5a7b078..f056d26c6ef76efcbaae5bdccc0ee827770594b6 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -29,12 +29,14 @@ if importlib.util.find_spec('shapely'):
 # and probably better. 
 # i'm pretty sure that's how croco devs & mim_people do mpc
 
-def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, i, past_data): 
+def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, goal, i, past_data): 
     breakFlag = False
     log_item = {}
     save_past_dict = {}
 
     # check for goal
+    if robot.robot_name == "yumi":
+        goal_left, goal_right = goal
     SEerror = robot.getT_w_e().actInv(robot.Mgoal)
     err_vector = pin.log6(SEerror).vector 
     if np.linalg.norm(err_vector) < robot.args.goal_error:
@@ -214,7 +216,7 @@ def CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner):
 
 
 def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, 
-                                         grasp_pose, iter_n, past_data):
+                                         grasp_pose, goal_transform, iter_n, past_data):
     """
     CrocoPathFollowingMPCControlLoop
     -----------------------------
@@ -312,6 +314,11 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
         np.interp(s, time_past, path2D_handlebar_1_untimed[:,1]).reshape((-1,1))))
 
     pathSE3_handlebar = path2D_to_SE3(path2D_handlebar_1, args.handlebar_height)
+    pathSE3_handlebar_left = []
+    pathSE3_handlebar_right = []
+    for pathSE3 in pathSE3_handlebar:
+        pathSE3_handlebar_left.append(goal_transform.act(pathSE3))
+        pathSE3_handlebar_right.append(goal_transform.inverse().act(pathSE3))
 
     if args.visualize_manipulator:
         if iter_n % 20 == 0:
@@ -331,10 +338,9 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
         runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference = path_base[i]
         if robot.robot_name != "yumi":
             runningModel.differential.costs.costs['ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
-        # MASSIVE TODO: actually have different references for left and right arm
         else:
-            runningModel.differential.costs.costs['l_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
-            runningModel.differential.costs.costs['r_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
+            runningModel.differential.costs.costs['l_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar_left[i]
+            runningModel.differential.costs.costs['r_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar_right[i]
 
     # idk if that's necessary
     solver.problem.terminalModel.differential.costs.costs['base_translation'+str(args.n_knots)].cost.residual.reference = path_base[-1]
diff --git a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc
index 654427aae8dcbedf493f3a356f4a6e5732f76014..04a1ccef3f6b4dfc6021b9488af0d53b8e27cf34 100644
Binary files a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc
index 49efa1685621d359f4209efe2cdfd6fac8a86bfa..addfb91192daa6a5085310358c6bcd33c1802a29 100644
Binary files a/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc
index 7d915c240c73fce908dbaf592187296265279892..48c5c7d75670106ce36c3db1cfd7fcc17c5eed69 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-310.pyc
index f3e4e64d35092a39fd51bda1a2b81e6f0f0f1341..4771e5aa53a3f3ac42dd177e6b27f00d8ec1cb6f 100644
Binary files a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-310.pyc and b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-310.pyc differ
diff --git a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc
index 66a8209bfd54f39391e1003fc8c1a3e7a66d382b..12f837a1d098d83ea0eba85ab02537512288f3d1 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc
index 86c35e0c7f317e7d72788853f7280a7d2570a6c3..d71d90faa69cc38279f576660076700113dcd9ef 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc differ