diff --git a/rrrobot_src/src/arm_control/include/arm_control/arm.h b/rrrobot_src/src/arm_control/include/arm_control/arm.h
index 0ddb737..45dc26c 100644
--- a/rrrobot_src/src/arm_control/include/arm_control/arm.h
+++ b/rrrobot_src/src/arm_control/include/arm_control/arm.h
@@ -5,6 +5,9 @@
 #include <kdl/joint.hpp>
 #include <kdl/frames.hpp>
 #include <LinearMath/btTransform.h>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <memory>
 
 #include <string>
 
@@ -59,11 +62,19 @@ public:
      */
     std::vector<float> getRequiredTorques(/*std::vector<float> theta_des, std::vector<float> theta_dot_des, */ std::vector<float> theta_double_dot_des);
 
+    /*
+     * Using the provided joint positions, this calculates the position of the end of the
+     * arm. Returns whether it was successful
+     */
+    bool calculateForwardKinematics(KDL::JntArray joint_positions, KDL::Frame &end_effector_frame);
+
     const KDL::Chain &getArm() const;
 
 private:
     KDL::Chain arm;
-    KDL::Chain simple_arm;
+
+    // Create solver based on kinematic chain
+    std::unique_ptr<KDL::ChainFkSolverPos_recursive> fksolver;
 
     /*
      * i_com: the inertia about the center of mass of this link
diff --git a/rrrobot_src/src/arm_control/src/arm.cpp b/rrrobot_src/src/arm_control/src/arm.cpp
index 6f0e0f6..ba382ee 100644
--- a/rrrobot_src/src/arm_control/src/arm.cpp
+++ b/rrrobot_src/src/arm_control/src/arm.cpp
@@ -302,16 +302,13 @@ Arm::Arm(const std::string &sdf_file)
 
         //KDL::Segment to_add(cur.getName(), cur.getJoint(), KDL::Frame(new_rotation, new_position), cur.getInertia());
         arm.addSegment(to_add);
-        if (simple_arm.getNrOfSegments() < 4)
-        {
-            simple_arm.addSegment(to_add);
-        }
 
         prev_frame = joint_in_world;
         // prev_frame_world_transform = cur_frame_world_transform;
     }
 
     cout << arm.getNrOfJoints() << '\t' << arm.getNrOfSegments() << endl;
+    fksolver.reset(new KDL::ChainFkSolverPos_recursive(arm));
 }
 
 /*
@@ -372,6 +369,15 @@ std::vector<float> Arm::getRequiredTorques(/*std::vector<float> theta_des, std::
 {
 }
 
+/*
+ * Using the provided joint positions, this calculates the pose of the end of the
+ * arm. Returns whether it was successful
+ */
+bool Arm::calculateForwardKinematics(KDL::JntArray joint_positions, KDL::Frame &end_effector_frame)
+{
+    return fksolver->JntToCart(joint_positions, end_effector_frame);
+}
+
 const KDL::Chain &Arm::getArm() const
 {
     return arm;