diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py
index e992cb3b079b218d5b9f3c42d5356270fccf8ad7..90cd8498bcdc42ac3ab90468d6e3de634bb8ad86 100644
--- a/python/examples/crocoddyl_mpc.py
+++ b/python/examples/crocoddyl_mpc.py
@@ -1,13 +1,76 @@
-"""
-TODO: this should be as generic as possible
-a function that continuously solves an ocp and executes
-trajectory following on it in the meantime
-(that's faster than mpc rate obv).
-
-now it can't be completely general as it's calling 
-on different solvers,
-but this can probably be tackled with some ifs.
-
-although now that i think of it having (abstract) classes
-makes things more readable ?
-"""
+import numpy as np
+import time
+import argparse
+from functools import partial
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import get_OCP_args, createCrocoIKOCP, solveCrocoOCP
+from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoIKMPC
+from ur_simple_control.basics.basics import followKinematicJointTrajP
+from ur_simple_control.util.logging_utils import LogManager
+from ur_simple_control.visualize.visualize import plotFromDict
+from ur_simple_control.clik.clik import getClikArgs
+import pinocchio as pin
+import crocoddyl
+
+
+def get_args():
+    parser = getMinimalArgParser()
+    parser = get_OCP_args(parser)
+    parser = getClikArgs(parser) # literally just for goal error
+    args = parser.parse_args()
+    return args
+
+
+if __name__ == "__main__": 
+    args = get_args()
+    robot = RobotManager(args)
+    # TODO: put this back for nicer demos
+    #Mgoal = robot.defineGoalPointCLI()
+    Mgoal = pin.SE3.Random()
+
+    robot.Mgoal = Mgoal.copy()
+    if args.visualize_manipulator:
+        # TODO document this somewhere
+        robot.manipulator_visualizer_queue.put(
+                {"Mgoal" : Mgoal})
+    # create and solve the optimal control problem of
+    # getting from current to goal end-effector position.
+    # reference is position and velocity reference (as a dictionary),
+    # while solver is a crocoddyl object containing a lot more information
+    # starting state
+    x0 = np.concatenate([robot.getQ(), robot.getQd()])
+    problem = createCrocoIKOCP(args, robot, x0, Mgoal)
+
+    # NOTE: this might be useful if we solve with a large time horizon,
+    # lower frequency, and then follow the predicted trajectory with velocity p-control
+    # this shouldn't really depend on x0 but i can't be bothered
+    #reference, solver = solveCrocoOCP(args, robot, problem, x0)
+    #if args.solver == "boxfddp":
+    #    log = solver.getCallbacks()[1]
+    #    crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
+    #if args.solver == "csqp":
+    #    log = solver.getCallbacks()[1]
+    #    mim_solvers.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
+
+    # we need a way to follow the reference trajectory,
+    # both because there can be disturbances,
+    # and because it is sampled at a much lower frequency
+    #followKinematicJointTrajP(args, robot, reference)
+
+    CrocoIKMPC(args, robot, x0, Mgoal)
+
+    print("final position:")
+    print(robot.getT_w_e())
+
+    robot.log_manager.plotAllControlLoops()
+
+    if not args.pinocchio_only:
+        robot.stopRobot()
+
+    if args.visualize_manipulator:
+        robot.killManipulatorVisualizer()
+    
+    if args.save_log:
+        robot.log_manager.saveLog()
+    #loop_manager.stopHandler(None, None)
+
diff --git a/python/examples/data/latest_run b/python/examples/data/latest_run
index 297d01f2c8a2e1b65b1584915dfdc4b840769aa3..ff992b84184343c3d9e4f6270f795b27a195a77f 100644
Binary files a/python/examples/data/latest_run and b/python/examples/data/latest_run differ
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index d3c5d1ae9a74a1df098821fca9469e72b11175e9..833a629e83be093317c49121020419c044232609 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -1,6 +1,7 @@
 from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
 from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP
 import pinocchio as pin
+import crocoddyl
 import numpy as np
 from functools import partial
 
@@ -22,25 +23,69 @@ from functools import partial
 # i'm pretty sure that's how croco devs & mim_people do mpc
 
 # TODO: FINISH
-def CrocoIKMPCControlLoop(solver, robot : RobotManager, i, past_data): 
+def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, i, past_data): 
+    breakFlag = False
+    log_item = {}
+    save_past_dict = {}
+
+    # check for 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:
+#      print("Convergence achieved, reached destionation!")
+        breakFlag = True
+
+    # set initial state from sensor 
     x0 = np.concatenate([robot.getQ(), robot.getQd()])
     solver.problem.x0 = x0
+    # warmstart solver with previous solution
     xs_init = list(solver.xs[1:]) + [solver.xs[-1]]
     xs_init[0] = x0
     us_init = list(solver.us[1:]) + [solver.us[-1]]
+
     solver.solve(xs_init, us_init, args.max_solver_iter)
-    pass
+    xs = np.array(solver.xs)
+    us = np.array(solver.us)
+    vel_cmds = xs[1, robot.model.nq:]
+    robot.sendQd(vel_cmds)
 
-# TODO: FINISH
-def IKMPC(args, robot, x0, goal):
-    problem = createCrocoIKOCP(args, robot, problem, x0)
+    log_item['qs'] = x0[:robot.model.nq]
+    log_item['dqs'] = x0[robot.model.nq:]
+    log_item['dqs_cmd'] = vel_cmds
+    log_item['u_tau'] = us[0]
+    
+    return breakFlag, save_past_dict, log_item
+
+def CrocoIKMPC(args, robot, x0, goal):
+    """
+    IKMPC
+    -----
+    run mpc for a point-to-point inverse kinematics.
+    note that the actual problem is solved on
+    a dynamics level, and velocities we command
+    are actually extracted from the state x(q,dq)
+    """
+    problem = createCrocoIKOCP(args, robot, x0, goal)
     if args.solver == "boxfddp":
         solver = crocoddyl.SolverBoxFDDP(problem)
     if args.solver == "csqp":
         solver = mim_solvers.SolverCSQP(problem)
 
+    # technically should be done in controlloop because now
+    # it's solved 2 times before the first command,
+    # but we don't have time for details rn
     x0 = np.concatenate([robot.getQ(), robot.getQd()])
-    xs = [x0] * (solver.problem.T + 1)
-    us = solver.problem.quasiStatic([x0] * solver.problem.T)
-    CrocoMPCControlLoop(...)
-    CrocoMPCControlLoop.run()
+    xs_init = [x0] * (solver.problem.T + 1)
+    us_init = solver.problem.quasiStatic([x0] * solver.problem.T)
+    solver.solve(xs_init, us_init, args.max_solver_iter)
+
+    controlLoop = partial(CrocoIKMPCControlLoop, args, robot, solver)
+    log_item = {
+            'qs' : np.zeros(robot.model.nq),
+            'dqs' : np.zeros(robot.model.nq),
+            'dqs_cmd' : np.zeros(robot.model.nv), # we're assuming full actuation here
+            'u_tau' : np.zeros(robot.model.nv),
+        }
+    save_past_dict = {}
+    loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+    loop_manager.run()
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 7d3b7cb9f3a598828e5a91f66e2a6263d57d6fc7..850c8452b1ec28f7bdebc604521b0c61316c1878 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -20,6 +20,8 @@ def get_OCP_args(parser : argparse.ArgumentParser):
                              if this argument is set to true, the final velocity will be set to 0 (there will be overshoot).")
     parser.add_argument("--solver", type=str, default="boxfddp", \
                         help="optimal control problem solver you want", choices=["boxfddp", "csqp"])
+    parser.add_argument("--max-solver-iter", type=int, default=200, \
+                        help="number of iterations the ocp solver takes. ~100-500 for just ocp (it can converge before ofc), ~10 for mpc")
     return parser
                         
 def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):