diff --git a/controllers.py b/controllers.py
index 7188191..c25b328 100644
--- a/controllers.py
+++ b/controllers.py
@@ -2,19 +2,24 @@ import numpy as np
 
 
 class CartesianPoseController:
-    def __init__(self, model):
+    def __init__(self, model, x0):
         self.model = model
-        # self.x_d = x0
+        self.x_d = x0
+        self.kp = np.ones(6) * 5.0
 
-    def set_target(self, pose):
-        self.x_d = pose.translation
+    def set_target(self, x_d):
+        self.x_d = x_d
 
     def update(self, q, dq):
-        t = self.model.pose(q).translation
-        J = self.model.jacobian(q)[:3, :]
-        J_pinv = np.linalg.pinv(J)
+        x = self.model.pose(q)
+        x_d = self.x_d
 
-        err = 2.0 * (self.x_d - t)
-        cmd = np.dot(J_pinv, err)
+        err = np.zeros(6)
+        err[:3] = x_d.translation - x.translation
+        err[3:] = (x_d.rotation * x.rotation.inv()).as_rotvec()
+
+        J = self.model.jacobian(q)
+        J_pinv = np.linalg.pinv(J)
+        cmd = np.dot(J_pinv, self.kp * err)
 
         return cmd
diff --git a/robot_sim.py b/robot_sim.py
index ac23ab2..0d09f01 100644
--- a/robot_sim.py
+++ b/robot_sim.py
@@ -11,15 +11,13 @@ from utils import *
 
 class PandaArm(object):
     def __init__(self):
-        self.num_dof = 7
-        self.ee_link_id = 7
-        self.ee_frame_id = "panda_link7"
+        self.nr_dof = 7
 
         self.named_joint_values = {"ready": [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79]}
 
-        self.upper_limits = [-7] * self.num_dof
-        self.lower_limits = [7] * self.num_dof
-        self.ranges = [7] * self.num_dof
+        self.upper_limits = [-7] * self.nr_dof
+        self.lower_limits = [7] * self.nr_dof
+        self.ranges = [7] * self.nr_dof
 
     def load(self, pose):
         self.T_world_base = pose
@@ -29,8 +27,8 @@ class PandaArm(object):
             baseOrientation=pose.rotation.as_quat(),
             useFixedBase=True,
         )
-        for i in range(self.num_dof):
-            p.resetJointState(self.uid, i, self.named_joint_values["ready"][i])
+        for i, q_i in enumerate(self.named_joint_values["ready"]):
+            p.resetJointState(self.uid, i, q_i)
 
     def get_state(self):
         joint_states = p.getJointStates(self.uid, range(p.getNumJoints(self.uid)))[:7]
diff --git a/run_demo.py b/run_demo.py
index 6ab38f4..587723d 100644
--- a/run_demo.py
+++ b/run_demo.py
@@ -14,10 +14,13 @@ rospy.init_node("demo")
 
 env = SimPandaEnv(gui)
 model = Model("panda_link0", "panda_link8")
-controller = CartesianPoseController(model)
 
 q, dq = env.arm.get_state()
-marker = InteractiveMarkerWrapper("target", "panda_link0", model.pose(q))
+x0 = model.pose(q)
+
+controller = CartesianPoseController(model, x0)
+
+marker = InteractiveMarkerWrapper("target", "panda_link0", x0)
 
 # run the control loop
 while True:
diff --git a/utils.py b/utils.py
index 1418dd7..93dcf46 100644
--- a/utils.py
+++ b/utils.py
@@ -2,22 +2,23 @@ import numpy as np
 from scipy.spatial.transform import Rotation
 import PyKDL as kdl
 
+import geometry_msgs.msg
 from interactive_markers.interactive_marker_server import *
 from interactive_markers.menu_handler import *
 from visualization_msgs.msg import *
 
 
 class InteractiveMarkerWrapper(object):
-    def __init__(self, name, frame_id, pose):
+    def __init__(self, name, frame_id, x0):
+        self.pose = x0
+
         server = InteractiveMarkerServer(topic_ns=name)
 
-        target_marker = InteractiveMarker()
-        target_marker.header.frame_id = frame_id
-        target_marker.name = name
-        target_marker.scale = 0.2
-        target_marker.pose.position.x = pose.translation[0]
-        target_marker.pose.position.y = pose.translation[1]
-        target_marker.pose.position.z = pose.translation[2]
+        int_marker = InteractiveMarker()
+        int_marker.header.frame_id = frame_id
+        int_marker.name = name
+        int_marker.scale = 0.2
+        int_marker.pose = to_pose_msg(x0)
 
         marker = Marker()
         marker.type = Marker.SPHERE
@@ -28,40 +29,65 @@ class InteractiveMarkerWrapper(object):
         marker.color.g = 0.5
         marker.color.b = 0.5
         marker.color.a = 0.6
+
         ctrl = InteractiveMarkerControl()
         ctrl.always_visible = True
         ctrl.markers.append(marker)
-        target_marker.controls.append(ctrl)
+        int_marker.controls.append(ctrl)
+
+        ctrl = InteractiveMarkerControl()
+        ctrl.orientation.w = 1
+        ctrl.orientation.x = 1
+        ctrl.orientation.y = 0
+        ctrl.orientation.z = 0
+        ctrl.name = "rotate_x"
+        ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+        int_marker.controls.append(ctrl)
+
+        ctrl = InteractiveMarkerControl()
+        ctrl.orientation.w = 1
+        ctrl.orientation.x = 0
+        ctrl.orientation.y = 0
+        ctrl.orientation.z = 1
+        ctrl.name = "rotate_y"
+        ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+        int_marker.controls.append(ctrl)
+
+        ctrl = InteractiveMarkerControl()
+        ctrl.orientation.w = 1
+        ctrl.orientation.x = 0
+        ctrl.orientation.y = 1
+        ctrl.orientation.z = 0
+        ctrl.name = "rotate_z"
+        ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+        int_marker.controls.append(ctrl)
 
         ctrl = InteractiveMarkerControl()
         ctrl.name = "move_x"
         ctrl.orientation.w = 1.0
         ctrl.orientation.x = 1.0
         ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
-        target_marker.controls.append(ctrl)
+        int_marker.controls.append(ctrl)
 
         ctrl = InteractiveMarkerControl()
         ctrl.name = "move_y"
         ctrl.orientation.w = 1.0
         ctrl.orientation.y = 1.0
         ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
-        target_marker.controls.append(ctrl)
+        int_marker.controls.append(ctrl)
 
         ctrl = InteractiveMarkerControl()
         ctrl.name = "move_z"
         ctrl.orientation.w = 1.0
         ctrl.orientation.z = 1.0
         ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
-        target_marker.controls.append(ctrl)
+        int_marker.controls.append(ctrl)
 
-        server.insert(target_marker, self.cb)
+        server.insert(int_marker, self.cb)
         server.applyChanges()
 
-        self.pose = pose
-
     def cb(self, feedback):
-        pos = feedback.pose.position
-        self.pose = Transform(Rotation.identity(), np.r_[pos.x, pos.y, pos.z])
+        self.pose = from_pose_msg(feedback.pose)
 
     def get_pose(self):
         return self.pose
@@ -126,6 +152,9 @@ class Transform(object):
         return cls(rotation, translation)
 
 
+# KDL Conversions
+
+
 def to_kdl_jnt_array(q):
     jnt_array = kdl.JntArray(len(q))
     for i, q_i in enumerate(q):
@@ -139,3 +168,57 @@ def kdl_to_mat(m):
         for j in range(m.columns()):
             mat[i, j] = m[i, j]
     return mat
+
+
+# ROS Conversions
+
+
+def to_point_msg(point):
+    msg = geometry_msgs.msg.Point()
+    msg.x = point[0]
+    msg.y = point[1]
+    msg.z = point[2]
+    return msg
+
+
+def from_point_msg(msg):
+    return np.r_[msg.x, msg.y, msg.z]
+
+
+def to_vector3_msg(vector3):
+    msg = geometry_msgs.msg.Vector3()
+    msg.x = vector3[0]
+    msg.y = vector3[1]
+    msg.z = vector3[2]
+    return msg
+
+
+def from_vector3_msg(msg):
+    return np.r_[msg.x, msg.y, msg.z]
+
+
+def to_quat_msg(orientation):
+    quat = orientation.as_quat()
+    msg = geometry_msgs.msg.Quaternion()
+    msg.x = quat[0]
+    msg.y = quat[1]
+    msg.z = quat[2]
+    msg.w = quat[3]
+    return msg
+
+
+def from_quat_msg(msg):
+    return Rotation.from_quat([msg.x, msg.y, msg.z, msg.w])
+
+
+def to_pose_msg(transform):
+    msg = geometry_msgs.msg.Pose()
+    msg.position = to_point_msg(transform.translation)
+    msg.orientation = to_quat_msg(transform.rotation)
+    return msg
+
+
+def from_pose_msg(msg):
+    position = from_point_msg(msg.position)
+    orientation = from_quat_msg(msg.orientation)
+    return Transform(orientation, position)
\ No newline at end of file