diff --git a/src/rrrobot_ws/src/rrrobot/include/arm_representation.h b/src/rrrobot_ws/src/rrrobot/include/arm_representation.h
index d58ab73..56ac021 100644
--- a/src/rrrobot_ws/src/rrrobot/include/arm_representation.h
+++ b/src/rrrobot_ws/src/rrrobot/include/arm_representation.h
@@ -39,7 +39,7 @@ using std::string;
 class ArmRepresentation
 {
 public:
-    ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.0, 0.9))); //KDL::Frame(KDL::Vector(0.3, 0.92, 1)));
+    ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Rotation::Quaternion(0, 0, 0, 1), KDL::Vector(0.3, 0.92, 0.9))); //KDL::Frame(KDL::Vector(0.3, 0.92, 1)));
 
     int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose);
 
diff --git a/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp b/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp
index 64c2dfd..e7285bf 100644
--- a/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp
+++ b/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp
@@ -1,6 +1,7 @@
 // arm_controller_node.cpp
 
 #include "arm_representation.h"
+#include <math.h>
 
 // using namespace std;
 using std::string;
@@ -27,49 +28,49 @@ ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose)
     chain.addSegment(linear_arm_actuator);
 
     const KDL::Vector pos_base(0, 0, base_len / 2);
-    const KDL::Rotation rot_base(KDL::Rotation::RPY(0, 0, 0));
+    const KDL::Rotation rot_base(KDL::Rotation::Quaternion(0, 0, 0, 1));
     KDL::Segment base_link("base_link", KDL::Joint("linear_arm_actuator_joint", KDL::Joint::JointType::TransY),
                            KDL::Frame(rot_base, pos_base));
     chain.addSegment(base_link);
 
     const KDL::Vector pos_shoulder(0, 0, shoulder_height);
-    const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0));
+    const KDL::Rotation rot_shoulder(KDL::Rotation::Quaternion(0, 0, 0, 1));
     KDL::Segment shoulder_link("shoulder_link", KDL::Joint("shoulder_pan_joint", KDL::Joint::JointType::RotZ),
                                KDL::Frame(rot_shoulder, pos_shoulder));
     chain.addSegment(shoulder_link);
 
     const KDL::Vector pos_upper_arm(0, shoulder_offset, 0);
-    const KDL::Rotation rot_upper_arm(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
+    const KDL::Rotation rot_upper_arm(KDL::Rotation::Quaternion(0, sqrt(2)/2, 0, sqrt(2)/2));
     KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint("shoulder_lift_joint", KDL::Joint::JointType::RotY),
                                 KDL::Frame(rot_upper_arm, pos_upper_arm));
     chain.addSegment(upper_arm_link);
 
     const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length);
-    const KDL::Rotation rot_forearm(KDL::Rotation::RPY(0.0, 0.0, 0.0));
+    const KDL::Rotation rot_forearm(KDL::Rotation::Quaternion(0, 0, 0, 1));
     KDL::Segment forearm_link("forearm_link", KDL::Joint("elbow_joint", KDL::Joint::JointType::RotY),
                               KDL::Frame(rot_forearm, pos_forearm));
     chain.addSegment(forearm_link);
 
     const KDL::Vector pos_wrist_1(0, 0, forearm_length);
-    const KDL::Rotation rot_wrist_1(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
+    const KDL::Rotation rot_wrist_1(KDL::Rotation::Quaternion(0, sqrt(2)/2, 0, sqrt(2)/2));
     KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint("wrist_1_joint", KDL::Joint::JointType::RotY),
                               KDL::Frame(rot_wrist_1, pos_wrist_1));
     chain.addSegment(wrist_1_link);
 
     const KDL::Vector pos_wrist_2(0, wrist_1_length, 0);
-    const KDL::Rotation rot_wrist_2(KDL::Rotation::RPY(0.0, 0.0, 0.0));
+    const KDL::Rotation rot_wrist_2(KDL::Rotation::Quaternion(0, 0, 0, 1));
     KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint("wrist_2_joint", KDL::Joint::JointType::RotZ),
                               KDL::Frame(rot_wrist_2, pos_wrist_2));
     chain.addSegment(wrist_2_link);
 
     const KDL::Vector pos_wrist_3(0, 0, wrist_2_length);
-    const KDL::Rotation rot_wrist_3(KDL::Rotation::RPY(0.0, 0.0, 0.0));
+    const KDL::Rotation rot_wrist_3(KDL::Rotation::Quaternion(0, 0, 0, 1));
     KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint("wrist_3_joint", KDL::Joint::JointType::RotY),
                               KDL::Frame(rot_wrist_3, pos_wrist_3));
     chain.addSegment(wrist_3_link);
 
     const KDL::Vector pos_ee(0, wrist_3_length, 0.0);
-    const KDL::Rotation rot_ee(KDL::Rotation::RPY(0.0, 0.0, M_PI / 2.0));
+    const KDL::Rotation rot_ee(KDL::Rotation::Quaternion(0, 0, sqrt(2)/2, sqrt(2)/2));
     KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None),
                          KDL::Frame(rot_ee, pos_ee));
     chain.addSegment(ee_link);