diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5b97d91e..09caabca 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -41,7 +41,7 @@ if(NOT INSTALL_ROS_INTERFACE_ONLY)
   set(pkg3 ${PROJECT_NAME}/main/panda main_hpp_mpc_buffer.py main_hpp_mpc.py
            main_meshcat_display.py main_optim_traj.py main_scenes.py)
   set(pkg4 ${PROJECT_NAME}/main/ur3 main_hpp_mpc.py)
-  set(pkg5 ${PROJECT_NAME}/ocps ocp_croco_hpp.py ocp.py)
+  set(pkg5 ${PROJECT_NAME}/ocps ocp_croco_hpp.py ocp_pose_ref.py ocp.py)
   set(pkg6 ${PROJECT_NAME}/robot_model obstacle_params_parser.py panda_model.py
            robot_model.py ur3_model.py)
   set(pkg7 ${PROJECT_NAME}/utils iostream.py ocp_analyzer.py pin_utils.py
@@ -84,7 +84,8 @@ if(BUILD_ROS_INTERFACE)
                  ${MY_PYTHON_DEPENDENCIES})
   catkin_install_python(
     PROGRAMS node/agimus_controller_node node/hpp_agimus_controller_node
-    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+    node/reaching_goal_controller_node DESTINATION
+    ${CATKIN_PACKAGE_BIN_DESTINATION})
   install(PROGRAMS node/hpp_corbaserver
           DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/node)
   install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/agimus_controller/main/panda/main_reaching_goal.py b/agimus_controller/main/panda/main_reaching_goal.py
new file mode 100644
index 00000000..f20311cc
--- /dev/null
+++ b/agimus_controller/main/panda/main_reaching_goal.py
@@ -0,0 +1,83 @@
+import time
+import numpy as np
+from agimus_controller.hpp_interface import HppInterface
+from agimus_controller.mpc import MPC
+from agimus_controller.utils.path_finder import get_mpc_params_dict
+from agimus_controller.visualization.plots import MPCPlots
+from agimus_controller.ocps.ocp_pose_ref import OCPPoseRef
+from agimus_controller.robot_model.panda_model import (
+    get_task_models,
+    get_robot_constructor,
+)
+from agimus_controller.ocps.parameters import OCPParameters
+from agimus_controller.main.servers import Servers
+from agimus_controller.utils.ocp_analyzer import (
+    return_cost_vectors,
+    return_constraint_vector,
+    plot_costs_from_dic,
+    plot_constraints_from_dic,
+)
+
+
+class APP(object):
+    def main(self, use_gui=False, spawn_servers=False):
+        if spawn_servers:
+            self.servers = Servers()
+            self.servers.spawn_servers(use_gui)
+
+        rmodel, cmodel, vmodel = get_task_models(task_name="reaching_goal")
+        self.robot_constructor = get_robot_constructor(task_name="reaching_goal")
+        mpc_params_dict = get_mpc_params_dict(task_name="reaching_goal")
+        ocp_params = OCPParameters()
+        ocp_params.set_parameters_from_dict(mpc_params_dict["ocp"])
+
+        hpp_interface = HppInterface()
+        q_init, q_goal = hpp_interface.get_panda_q_init_q_goal()
+        hpp_interface.set_panda_planning(q_init, q_goal, use_gepetto_gui=use_gui)
+        viewer = hpp_interface.get_viewer()
+        x0 = q_init + [0] * 7
+        length = 500
+        x_plan = np.array(x0 * length)
+        x_plan = np.reshape(x_plan, (length, 14))
+        a_plan = np.zeros((length, 7))
+        ocp = OCPPoseRef(rmodel, cmodel, ocp_params, np.array(q_goal))
+
+        self.mpc = MPC(ocp, x_plan, a_plan, rmodel, cmodel)
+        start = time.time()
+        self.mpc.simulate_mpc(save_predictions=True)
+        solver = self.mpc.ocp.solver
+        costs = return_cost_vectors(solver, weighted=True)
+        constraint = return_constraint_vector(solver)
+        plot_costs_from_dic(costs)
+        plot_constraints_from_dic(constraint)
+        max_kkt = max(self.mpc.mpc_data["kkt_norm"])
+        index = self.mpc.mpc_data["kkt_norm"].index(max_kkt)
+        print(f"max kkt {max_kkt} index {index}")
+        end = time.time()
+        print("Time of solving: ", end - start)
+        ee_frame_name = ocp_params.effector_frame_name
+        self.mpc_plots = MPCPlots(
+            croco_xs=self.mpc.croco_xs,
+            croco_us=self.mpc.croco_us,
+            whole_x_plan=x_plan,
+            whole_u_plan=np.zeros((length - 1, 7)),
+            rmodel=rmodel,
+            vmodel=vmodel,
+            cmodel=cmodel,
+            DT=self.mpc.ocp.params.dt,
+            ee_frame_name=ee_frame_name,
+            viewer=viewer,
+        )
+
+        if use_gui:
+            self.mpc_plots.display_path_gepetto_gui()
+        return True
+
+
+def main():
+    return APP().main(use_gui=False, spawn_servers=False)
+
+
+if __name__ == "__main__":
+    app = APP()
+    app.main(use_gui=True, spawn_servers=True)
diff --git a/agimus_controller/ocps/ocp_pose_ref.py b/agimus_controller/ocps/ocp_pose_ref.py
new file mode 100644
index 00000000..0a8fca45
--- /dev/null
+++ b/agimus_controller/ocps/ocp_pose_ref.py
@@ -0,0 +1,324 @@
+import crocoddyl
+import pinocchio as pin
+import numpy as np
+import mim_solvers
+from colmpc import ResidualDistanceCollision
+
+from agimus_controller.utils.pin_utils import get_ee_pose_from_configuration
+from agimus_controller_ros.parameters import OCPParameters
+
+
+class OCPPoseRef:
+    def __init__(
+        self,
+        rmodel: pin.Model,
+        cmodel: pin.GeometryModel,
+        params: OCPParameters,
+        q_goal: np.array,
+    ) -> None:
+        """Class to define the OCP linked witha HPP generated trajectory.
+
+        Args:
+            rmodel (pin.Model): Pinocchio model of the robot.
+            cmodel (pin.GeometryModel): Pinocchio geometry model of the robot. Must have been convexified for the collisions to work.
+            params (OCPParameters) : parameters of the ocp.
+        """
+
+        self._rmodel = rmodel
+        self._cmodel = cmodel
+        self.params = params
+        self._rdata = self._rmodel.createData()
+
+        self._effector_frame_id = self._rmodel.getFrameId(params.effector_frame_name)
+        self._weight_u_reg = params.control_weight
+        self._weight_ee_placement = None
+        self._weight_vel_reg = None
+        self._weight_x_reg = params.state_weight
+        self._collision_margin = 2e-2
+        self.state = crocoddyl.StateMultibody(self._rmodel)
+        self.actuation = crocoddyl.ActuationModelFull(self.state)
+        self.nq = self._rmodel.nq  # Number of joints of the robot
+        self.nv = self._rmodel.nv  # Dimension of the joint's speed vector of the robot
+        self.x_plan = None
+        self.a_plan = None
+        self.u_plan = None
+        self.running_models = None
+        self.terminal_model = None
+        self.solver = None
+        self.next_node_time = None
+        self.q_goal = q_goal
+        self.x_goal = np.concatenate([q_goal, np.array([0.0] * self.nq)])
+        self.des_pose = get_ee_pose_from_configuration(
+            self._rmodel,
+            self._rdata,
+            self._effector_frame_id,
+            np.array(q_goal),
+        )
+
+    def get_grav_compensation(
+        self,
+        q: np.ndarray,
+    ) -> np.ndarray:
+        """Return the reference of control u_plan that compensates gravity."""
+        pin.computeGeneralizedGravity(
+            self._rmodel,
+            self._rdata,
+            q,
+        )
+        return self._rdata.g.copy()
+
+    def get_inverse_dynamic_control(self, x, a):
+        """Return inverse dynamic control for a given state and acceleration."""
+        return pin.rnea(self._rmodel, self._rdata, x[: self.nq], x[self.nq :], a).copy()
+
+    def set_weights(
+        self,
+        weight_ee_placement: float,
+        weight_x_reg: float,
+        weight_u_reg: float,
+        weight_vel_reg: float,
+    ):
+        """Set weights of the ocp.
+
+        Args:
+            weight_ee_placement (float): Weight of the placement of the end effector with regards to the target.
+            weight_x_reg (float): Weight of the state regularization.
+            weight_u_reg (float): Weight of the control regularization.
+            weight_vel_reg (float): Weight of the velocity regularization.
+        """
+        self._weight_ee_placement = weight_ee_placement
+        self._weight_x_reg = weight_x_reg
+        self._weight_u_reg = weight_u_reg
+        self._weight_vel_reg = weight_vel_reg
+
+    def set_weight_ee_placement(self, weight_ee_placement: float):
+        """Set end effector weight of the ocp.
+
+        Args:
+            weight_ee_placement (float): Weight of the placement of the end effector with regards to the target.
+        """
+        self._weight_ee_placement = weight_ee_placement
+
+    def set_vel_weight(self, weight_vel_reg):
+        self._weight_vel_reg = weight_vel_reg
+
+    def set_control_weight(self, weight_u_reg):
+        self._weight_u_reg = weight_u_reg
+
+    def get_increasing_weight(self, time, max_weight):
+        return max_weight * np.tanh(
+            max(0.0, time)
+            * np.arctanh(self.params.increasing_weights["percent"])
+            / self.params.increasing_weights["time_reach_percent"]
+        )
+
+    def get_model(self, x_ref, u_ref, des_pose):
+        running_cost_model = crocoddyl.CostModelSum(self.state)
+        u_reg_cost = self.get_control_residual(u_ref)
+        ugrav_reg_cost = self.get_control_grav_residual()
+        x_reg_weights = np.array([1.0] * self.nq + [0.0] * self.nv)
+        x_reg_cost = self.get_state_residual(x_ref, x_reg_weights)
+        vel_reg_cost = self.get_velocity_residual()
+        placement_reg_cost = self.get_placement_residual(des_pose)
+        running_cost_model.addCost("uReg", u_reg_cost, 0)
+        running_cost_model.addCost("ugravReg", ugrav_reg_cost, self._weight_u_reg)
+        running_cost_model.addCost(
+            "gripperPose", placement_reg_cost, self._weight_ee_placement
+        )
+        running_cost_model.addCost("velReg", vel_reg_cost, self._weight_vel_reg)
+        running_cost_model.addCost("xReg", x_reg_cost, self._weight_x_reg)
+
+        constraints = self.get_constraints()
+        running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics(
+            self.state, self.actuation, running_cost_model, constraints
+        )
+        running_DAM.armature = self.params.armature
+        return crocoddyl.IntegratedActionModelEuler(running_DAM, self.params.dt)
+
+    def get_terminal_model(self, x_ref, u_ref, des_pose):
+        cost_model = crocoddyl.CostModelSum(self.state)
+        u_reg_cost = self.get_control_residual(u_ref)
+        ugrav_reg_cost = self.get_control_grav_residual()
+        x_reg_cost = self.get_state_residual(x_ref)
+        vel_reg_cost = self.get_velocity_residual()
+        placement_reg_cost = self.get_placement_residual(des_pose)
+        cost_model.addCost("uReg", u_reg_cost, 0)
+        cost_model.addCost("ugravReg", ugrav_reg_cost, 0)
+        cost_model.addCost("gripperPose", placement_reg_cost, 0)
+        cost_model.addCost("velReg", vel_reg_cost, 0)
+        cost_model.addCost("xReg", x_reg_cost, 0)
+        running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics(
+            self.state, self.actuation, cost_model
+        )
+        running_DAM.armature = self.params.armature
+        return crocoddyl.IntegratedActionModelEuler(running_DAM, self.params.dt)
+
+    def get_constraints(self):
+        constraint_model_manager = crocoddyl.ConstraintModelManager(self.state, self.nq)
+        if len(self._cmodel.collisionPairs) != 0:
+            for col_idx in range(len(self._cmodel.collisionPairs)):
+                collision_constraint = self._get_collision_constraint(
+                    col_idx, self._collision_margin
+                )
+                # Adding the constraint to the constraint manager
+                constraint_model_manager.addConstraint(
+                    "col_term_" + str(col_idx), collision_constraint
+                )
+        return constraint_model_manager
+
+    def _get_collision_constraint(
+        self, col_idx: int, collision_margin: float
+    ) -> "crocoddyl.ConstraintModelResidual":
+        """Returns the collision constraint that will be in the constraint model manager.
+
+        Args:
+            col_idx (int): index of the collision pair.
+            collision_margin (float): Lower bound of the constraint, ie the collision margin.
+
+        Returns:
+            _type_: _description_
+        """
+        obstacleDistanceResidual = ResidualDistanceCollision(
+            self.state, 7, self._cmodel, col_idx
+        )
+
+        # Creating the inequality constraint
+        constraint = crocoddyl.ConstraintModelResidual(
+            self.state,
+            obstacleDistanceResidual,
+            np.array([collision_margin]),
+            np.array([np.inf]),
+        )
+        return constraint
+
+    def get_placement_residual(self, placement_ref):
+        """Return placement residual with desired reference for end effector placement."""
+        return crocoddyl.CostModelResidual(
+            self.state,
+            crocoddyl.ResidualModelFramePlacement(
+                self.state, self._effector_frame_id, placement_ref
+            ),
+        )
+
+    def get_velocity_residual(self):
+        """Return velocity residual of desired joint."""
+        vref = pin.Motion.Zero()
+        return crocoddyl.CostModelResidual(
+            self.state,
+            crocoddyl.ResidualModelFrameVelocity(
+                self.state,
+                self._effector_frame_id,
+                vref,
+                pin.WORLD,
+            ),
+        )
+
+    def get_control_residual(self, uref):
+        """Return control residual with uref the control reference."""
+        return crocoddyl.CostModelResidual(
+            self.state, crocoddyl.ResidualModelControl(self.state, uref)
+        )
+
+    def get_control_grav_residual(self):
+        """Return control residual with uref the control reference."""
+        return crocoddyl.CostModelResidual(
+            self.state, crocoddyl.ResidualModelControlGrav(self.state)
+        )
+
+    def get_state_residual(self, xref, x_reg_weights=None):
+        """Return state residual with xref the state reference."""
+        if x_reg_weights is None:
+            return crocoddyl.CostModelResidual(
+                self.state,
+                crocoddyl.ResidualModelState(self.state, xref, self.actuation.nu),
+            )
+        else:
+            return crocoddyl.CostModelResidual(
+                self.state,
+                crocoddyl.ActivationModelWeightedQuad(x_reg_weights),
+                crocoddyl.ResidualModelState(self.state, xref, self.actuation.nu),
+            )
+
+    def update_cost(self, model, new_model, cost_name, update_weight=True):
+        """Update model's cost reference and weight by copying new_model's cost."""
+        model.differential.costs.costs[
+            cost_name
+        ].cost.residual.reference = new_model.differential.costs.costs[
+            cost_name
+        ].cost.residual.reference.copy()
+        if update_weight:
+            new_weight = new_model.differential.costs.costs[cost_name].weight
+            model.differential.costs.costs[cost_name].weight = new_weight
+
+    def update_weight(self, model, new_model, cost_name):
+        new_weight = new_model.differential.costs.costs[cost_name].weight
+        model.differential.costs.costs[cost_name].weight = new_weight
+
+    def update_model(self, model, new_model, update_weight):
+        """update model's costs by copying new_model's costs."""
+        self.update_cost(model, new_model, "gripperPose", update_weight)
+        self.update_cost(model, new_model, "velReg", update_weight)
+        self.update_cost(model, new_model, "xReg", update_weight)
+        self.update_weight(model, new_model, "ugravReg")
+
+    def reset_ocp(self, x0, x_ref: np.ndarray, u_plan: np.ndarray, placement_ref):
+        """Reset ocp problem using next reference in state and control."""
+        self.solver.problem.x0 = x0
+        u_grav = self.get_grav_compensation(x0[: self.nq])
+        runningModels = list(self.solver.problem.runningModels)
+        for node_idx in range(len(runningModels) - 1):
+            self.update_model(
+                runningModels[node_idx], runningModels[node_idx + 1], True
+            )
+        weight = self.get_increasing_weight(
+            self.next_node_time - self.params.dt,
+            self.params.increasing_weights["max"] / self.params.dt,
+        )
+        self.set_weight_ee_placement(weight)
+        self.set_vel_weight(weight / 10)
+        last_running_model = self.get_model(self.x_goal, u_grav, self.des_pose)
+        self.update_model(runningModels[-1], last_running_model, True)
+        terminal_model = self.get_terminal_model(x0, u_grav, self.des_pose)
+        self.next_node_time += self.params.dt
+        self.update_model(self.solver.problem.terminalModel, terminal_model, True)
+
+    def set_planning_variables(self, x_plan: np.ndarray, a_plan: np.ndarray):
+        self.x_plan = x_plan
+        self.a_plan = a_plan
+        u_grav = self.get_grav_compensation(x_plan[0, : self.nq])
+        self.u_plan = np.array(list(u_grav) * (self.params.horizon_size - 1))
+        self.u_plan = np.reshape(self.u_plan, (self.params.horizon_size - 1, 7))
+
+    def build_ocp_from_plannif(self, x0):
+        u_grav = self.u_plan[0, :]
+        models = []
+        for idx in range(self.params.horizon_size - 1):
+            time = idx * self.params.dt - 0.2
+            weight = self.get_increasing_weight(
+                time, self.params.increasing_weights["max"] / self.params.dt
+            )
+            self.set_weight_ee_placement(weight)
+            self.set_vel_weight(weight / 10)
+            models.append(self.get_model(self.x_goal, u_grav, self.des_pose))
+        terminal_model = self.get_terminal_model(x0, u_grav, self.des_pose)
+        self.next_node_time = self.params.horizon_size * self.params.dt - 0.2
+
+        return crocoddyl.ShootingProblem(x0, models, terminal_model)
+
+    def run_solver(self, problem, xs_init, us_init):
+        """
+        Run FDDP or CSQP solver
+        problem : crocoddyl ocp problem.
+        xs_init : xs warm start.
+        us_init : us warm start.
+        max_iter : max number of iteration for the solver
+        set_callback : activate solver callback
+        """
+        # Creating the solver for this OC problem, defining a logger
+        solver = mim_solvers.SolverCSQP(problem)
+        solver.use_filter_line_search = True
+        solver.termination_tolerance = 1e-4
+        solver.max_qp_iters = self.params.max_qp_iter
+        solver.with_callbacks = self.params.activate_callback
+        solver.solve(xs_init, us_init, self.params.max_iter)
+        self.solver = solver
diff --git a/agimus_controller_ros/reaching_goal_controller.py b/agimus_controller_ros/reaching_goal_controller.py
new file mode 100644
index 00000000..da3d398f
--- /dev/null
+++ b/agimus_controller_ros/reaching_goal_controller.py
@@ -0,0 +1,170 @@
+#!/usr/bin/env python3
+import rospy
+import numpy as np
+from copy import deepcopy
+import time
+from threading import Lock
+from std_msgs.msg import Duration, Header
+from linear_feedback_controller_msgs.msg import Control, Sensor
+import atexit
+
+from agimus_controller.hpp_interface import HppInterface
+from agimus_controller_ros.ros_np_multiarray import to_multiarray_f64
+from agimus_controller.robot_model.panda_model import get_task_models
+from agimus_controller.utils.pin_utils import get_ee_pose_from_configuration
+from agimus_controller.mpc import MPC
+from agimus_controller.ocps.ocp_pose_ref import OCPPoseRef
+from agimus_controller_ros.sim_utils import convert_float_to_ros_duration_msg
+from agimus_controller_ros.parameters import AgimusControllerNodeParameters
+
+
+class ReachingGoalController:
+    def __init__(self, params: AgimusControllerNodeParameters) -> None:
+        self.params = params
+        self.rmodel, self.cmodel = get_task_models(task_name="goal_reaching")
+        self.rdata = self.rmodel.createData()
+        self.effector_frame_id = self.rmodel.getFrameId(
+            self.params.ocp.effector_frame_name
+        )
+        self.hpp_interface = HppInterface()
+        self.nq = self.rmodel.nq
+        self.nv = self.rmodel.nv
+        self.nx = self.nq + self.nv
+
+        q_init, q_goal = self.hpp_interface.get_panda_q_init_q_goal()
+        self.ocp = OCPPoseRef(self.rmodel, self.cmodel, params.ocp, np.array(q_goal))
+
+        if self.params.use_ros_params:
+            self.initialize_ros_attributes()
+
+    def initialize_ros_attributes(self):
+        self.rate = rospy.Rate(self.params.rate, reset=True)
+        self.mutex = Lock()
+        self.sensor_msg = Sensor()
+        self.control_msg = Control()
+        self.state_subscriber = rospy.Subscriber(
+            "robot_sensors", Sensor, self.sensor_callback
+        )
+        self.control_publisher = rospy.Publisher(
+            "motion_server_control", Control, queue_size=1, tcp_nodelay=True
+        )
+        self.ocp_solve_time_pub = rospy.Publisher(
+            "ocp_solve_time", Duration, queue_size=1, tcp_nodelay=True
+        )
+        self.ocp_x0_pub = rospy.Publisher(
+            "ocp_x0", Sensor, queue_size=1, tcp_nodelay=True
+        )
+        self.first_robot_sensor_msg_received = False
+        self.first_pose_ref_msg_received = True
+
+    def sensor_callback(self, sensor_msg):
+        with self.mutex:
+            self.sensor_msg = deepcopy(sensor_msg)
+            if not self.first_robot_sensor_msg_received:
+                self.first_robot_sensor_msg_received = True
+
+    def wait_first_sensor_msg(self):
+        wait_for_input = True
+        while not rospy.is_shutdown() and wait_for_input:
+            wait_for_input = (
+                not self.first_robot_sensor_msg_received
+                or not self.first_pose_ref_msg_received
+            )
+            if wait_for_input:
+                rospy.loginfo_throttle(3, "Waiting until we receive a sensor message.")
+            rospy.loginfo_once("Start controller")
+            self.rate.sleep()
+        return wait_for_input
+
+    def first_solve(self, x0):
+        # retrieve horizon state and acc references
+        # x0 = list(x0)
+        x_plan = np.array(list(x0) * self.params.ocp.horizon_size)
+        x_plan = np.reshape(x_plan, (self.params.ocp.horizon_size, 14))
+        a_plan = np.zeros((self.params.ocp.horizon_size, 7))
+
+        # x0 = np.array(x0)
+
+        # First solve
+        self.mpc = MPC(self.ocp, x_plan, a_plan, self.rmodel, self.cmodel)
+        self.mpc.ocp.set_planning_variables(x_plan, a_plan)
+        self.mpc.mpc_first_step(
+            x_plan, self.ocp.u_plan[: self.params.ocp.horizon_size - 1], x0
+        )
+        self.next_node_idx = self.params.ocp.horizon_size
+        if self.params.save_predictions_and_refs:
+            self.mpc.create_mpc_data()
+        _, u, k = self.mpc.get_mpc_output()
+        return u, k
+
+    def solve(self, x0):
+        new_x_ref = np.array(x0)
+        new_a_ref = np.zeros((7))
+        self.in_world_M_effector = get_ee_pose_from_configuration(
+            self.rmodel,
+            self.rdata,
+            self.effector_frame_id,
+            new_x_ref[: self.rmodel.nq],
+        )
+        # if last point of the pick trajectory is in horizon and we wanna use vision pose
+        self.mpc.mpc_step(x0, new_x_ref, new_a_ref, self.in_world_M_effector)
+
+        if self.params.save_predictions_and_refs:
+            self.mpc.fill_predictions_and_refs_arrays()
+        _, u, k = self.mpc.get_mpc_output()
+        return u, k
+
+    def get_sensor_msg(self):
+        with self.mutex:
+            sensor_msg = deepcopy(self.sensor_msg)
+        return sensor_msg
+
+    def send(self, sensor_msg, u, k):
+        self.control_msg.header = Header()
+        self.control_msg.header.stamp = rospy.Time.now()
+        self.control_msg.feedback_gain = to_multiarray_f64(k)
+        self.control_msg.feedforward = to_multiarray_f64(u)
+        self.control_msg.initial_state = sensor_msg
+        self.control_publisher.publish(self.control_msg)
+
+    def exit_handler(self):
+        print("saving data")
+        np.save("mpc_params.npy", self.params.get_dict())
+        if self.params.save_predictions_and_refs:
+            np.save("mpc_data.npy", self.mpc.mpc_data)
+
+    def get_x0_from_sensor_msg(self, sensor_msg):
+        return np.concatenate(
+            [sensor_msg.joint_state.position, sensor_msg.joint_state.velocity]
+        )
+
+    def publish_ocp_solve_time(self, ocp_solve_time):
+        self.ocp_solve_time_pub.publish(
+            convert_float_to_ros_duration_msg(ocp_solve_time)
+        )
+
+    def run(self):
+        self.wait_first_sensor_msg()
+        sensor_msg = self.get_sensor_msg()
+        time.sleep(2.0)
+        start_compute_time = time.time()
+        u, k = self.first_solve(self.get_x0_from_sensor_msg(sensor_msg))
+        compute_time = time.time() - start_compute_time
+        self.send(sensor_msg, u, k)
+        self.publish_ocp_solve_time(compute_time)
+        self.ocp_x0_pub.publish(sensor_msg)
+        self.rate.sleep()
+        atexit.register(self.exit_handler)
+        self.run_timer = rospy.Timer(
+            rospy.Duration(self.params.ocp.dt), self.run_callback
+        )
+        rospy.spin()
+
+    def run_callback(self, *args):
+        start_compute_time = time.time()
+        sensor_msg = self.get_sensor_msg()
+        u, k = self.solve(self.get_x0_from_sensor_msg(sensor_msg))
+        self.send(sensor_msg, u, k)
+        compute_time = time.time() - start_compute_time
+        self.publish_ocp_solve_time(compute_time)
+        self.ocp_x0_pub.publish(sensor_msg)
diff --git a/launch/reaching_goal_controller.launch b/launch/reaching_goal_controller.launch
new file mode 100644
index 00000000..3c8d89a7
--- /dev/null
+++ b/launch/reaching_goal_controller.launch
@@ -0,0 +1,11 @@
+<launch>
+
+    <arg name="ctrl_ns" default="/ctrl_mpc_linearized"/>
+    <group ns="$(arg ctrl_ns)">
+        <node pkg="agimus_controller" name="reaching_goal_controller_node"
+        type="reaching_goal_controller_node" output="screen"/>
+        <include file="$(find agimus_controller)/launch/record_rosbag.launch"/>
+        <rosparam command="load" file="$(find agimus_demos_description)/reaching_goal/mpc_params.yaml"
+        subst_value="true" />
+    </group>
+</launch>
diff --git a/node/reaching_goal_controller_node b/node/reaching_goal_controller_node
new file mode 100644
index 00000000..1ca6a0e9
--- /dev/null
+++ b/node/reaching_goal_controller_node
@@ -0,0 +1,20 @@
+#!/usr/bin/env python3
+
+import rospy
+from agimus_controller_ros.reaching_goal_controller import ReachingGoalController
+from agimus_controller_ros.parameters import AgimusControllerNodeParameters
+
+
+def run():
+    rospy.init_node("talker", anonymous=True)
+    params = AgimusControllerNodeParameters()
+    params.set_parameters_from_ros()
+    node = ReachingGoalController(params)
+    node.run()
+
+
+if __name__ == "__main__":
+    try:
+        run()
+    except rospy.ROSInterruptException:
+        pass