Merge branch 'GEAR_arm_controller' of github.com:EECS-467-W20-RRRobot-Project/RRRobot into GEAR_arm_controller

.gitignore

@@ -34,3 +34,4 @@ core
 *.old
 *.custom
 *.run
+*.debug

docs/gear.md

@@ -86,18 +86,38 @@ rostopic pub /ariac/arm1/arm/command trajectory_msgs/JointTrajectory    "{joint_
 
 ## Running Full Simulation
 
-### ARIAC Environment
+### Terminal 1: ARIAC Environment
 
 - `cd /app/rrrobot_ws/src/rrrobot/scripts`
-- `./rrrobot_run.sh`
+- `./rrrobot_run_no_build.sh`
 
-### Build RRRobot Package
+### Terminal 2: Build RRRobot Package & Arm Controller Node
 
 - `cd /app/rrrobot_ws`
 - `catkin_make clean`
 - `catkin_make`
 - `source devel/setup.bash`
-
-### Arm Controller Node
-
 - `rosrun rrrobot arm_controller_node`
+
+### Terminal 3: CV Model
+
+- `cd /app/rrrobot_ws/src/rrrobot/src/`
+- `python3 cv_model.py`
+
+### Terminal 4: Depth Camera Node (for getting pick up location)
+
+- `cd /app/rrrobot_ws/`
+- `source devel/setup.bash`
+- `rosrun rrrobot depth_camera_node`
+
+### Terminal 5: Run the main rrrobot node
+
+- `cd /app/rrrobot_ws/`
+- `source devel/setup.bash`
+- `rosrun rrrobot rrrobot_node`
+
+### Terminal 6: Run the node to spawn random objects onto the conveyor belt
+
+- `cd /app/rrrobot_ws/`
+- `source devel/setup.bash`
+- `rosrun rrrobot object_spawner_node`

src/rrrobot_ws/src/rrrobot/include/arm_representation.h

@@ -43,7 +43,7 @@ public:
 
     int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose, int joint_nbr = -1);
 
-    int calculateInverseKinematics(const KDL::JntArray &cur_configuration,
+    int calculateInverseKinematics(const std::vector<double> &cur_configuration,
                                    const KDL::Frame &desired_end_effector_pose,
                                    KDL::JntArray &final_joint_configuration);
 

src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh

@@ -1,11 +1,12 @@
 #!/bin/bash
 
-#rostopic pub /target_pose geometry_msgs/Pose '{position: {x: 0.875, y: 0.75, z: 1.5}, orientation: {x: 0, y: 0, z: 0, w: 0}}'
+source /app/rrrobot_ws/devel/setup.bash
+
 rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location:
   position:
-    x: 1.2
+    x: 1.22
-    y: -1
+    y: 1.22
-    z: 1.5
+    z: 0.9725
   orientation:
     x: 0.0
     y: 0.707
@@ -14,11 +15,11 @@ rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location:
 drop_location:
   position:
     x: -0.3
-    y: 1.15
+    y: 0.383
-    z: 1.5
+    z: 1.0
   orientation:
     x: 0.0
-    y: 0.0
+    y: 0.707
     z: 0.0
-    w: 0.0" \
+    w: 0.707" \
 -1

src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp

@@ -48,10 +48,12 @@ public:
 
         gripper_ = node.serviceClient<osrf_gear::VacuumGripperControl>(GRIPPER_CONTROL_CHANNEL);
 
+        arm_current_joint_states_.resize(arm->getChain()->getNrOfJoints(), 0.0);
+
         // arm = std::move(arm_);
         // ROS_INFO("Arm size (in constructor): %d", arm->getChain()->getNrOfJoints());
-        arm_current_joint_states_ = KDL::JntArray(arm->getChain()->getNrOfJoints());
+        // arm_current_joint_states_ = KDL::JntArray(arm->getChain()->getNrOfJoints());
-        KDL::SetToZero(arm_current_joint_states_);
+        // KDL::SetToZero(arm_current_joint_states_);
     }
 
     void joint_state_callback(const sensor_msgs::JointState &joint_state_msg)
@@ -64,22 +66,9 @@ public:
         vector<string> cur_joint_names = arm->get_joint_names();
         vector<double> position = joint_state_msg.position;
 
-        // Print joint name vectors for debugging
-        // ROS_INFO_STREAM("msg_joint_names: {");
-        // for (int i = 0; i < nbr_joints; ++i)
-        // {
-        //     ROS_INFO_STREAM(msg_joint_names[i] << ", ");
-        // }
-        // ROS_INFO_STREAM("}\ncur_joint_names: {");
-        // for (int j = 0; j < cur_joint_names.size(); ++j)
-        // {
-        //     ROS_INFO_STREAM(cur_joint_names[j] << ", ");
-        // }
-        // ROS_INFO_STREAM("}");
-
         for (int i = 0; i < nbr_joints; ++i)
         {
-            arm_current_joint_states_(i) = 0.0;
+            arm_current_joint_states_[i] = 0.0;
         }
 
         for (size_t i = 0; i < position.size(); ++i)
@@ -88,7 +77,7 @@ public:
             {
                 if (msg_joint_names[i].compare(cur_joint_names[j]) == 0)
                 {
-                    arm_current_joint_states_(j) = position[i];
+                    arm_current_joint_states_[j] = position[i];
                 }
             }
         }
@@ -122,7 +111,7 @@ public:
 
         // Move arm to pickup item from conveyor belt
         attempts = 1;
-        positions = calc_joint_positions(target_pose.grab_location);
+        positions = calc_joint_positions(target_pose.grab_location, above_conveyor);
         send_joint_trajectory(positions, arm_action_phase::bin_to_belt);
 
         // Check if target has been reached
@@ -151,7 +140,7 @@ public:
 
             // If item is not able to attach, adjust z and try again
             target_location.position.z -= item_attach_z_adjustment; // meters
-            positions = calc_joint_positions(target_location);
+            positions = calc_joint_positions(target_location, arm_current_joint_states_);
             send_joint_trajectory(positions, arm_action_phase::item_pickup_adjustment);
             attempts++;
         }
@@ -160,11 +149,11 @@ public:
 
         // Move item to desired position
         attempts = 1;
-        positions = calc_joint_positions(target_pose.drop_location);
+        positions = calc_joint_positions(target_pose.drop_location, above_bins);
         send_joint_trajectory(positions, arm_action_phase::belt_to_bin);
 
         // Check if target has been reached
-        while(!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.grab_location)) {
+        while(!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.drop_location)) {
             if (attempts == retry_attempts) {
                 ROS_ERROR("STEP 4: Did not reach target in allotted time");
                 return;
@@ -198,7 +187,7 @@ public:
                 ROS_ERROR("STEP 6: Item not detaching.. ¯\\_(ツ)_/¯");
             }
             
-            ros::Duration(0.1).sleep();
+            ros::Duration(1).sleep();
             attempts++;
         }
 
@@ -206,11 +195,12 @@ public:
     }
 
 private:
+    // Private variables
     bool gripper_enabled_;
     bool item_attached_;
     ros::Publisher arm_joint_trajectory_publisher_;
     ros::ServiceClient gripper_;
-    KDL::JntArray arm_current_joint_states_;
+    vector<double> arm_current_joint_states_;
     ArmRepresentation *arm;
     enum arm_action_phase {
         belt_to_bin,
@@ -220,8 +210,11 @@ private:
     double time_per_step;
     int retry_attempts;
     double item_attach_z_adjustment;
+    // Define intermediate joint positions
+    const vector<double> above_conveyor = {0, 0, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0};
+    const vector<double> above_bins = {0, M_PI, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0};
 
-    vector<double> calc_joint_positions(const geometry_msgs::Pose &pose)
+    vector<double> calc_joint_positions(const geometry_msgs::Pose &pose, const vector<double> &start_state)
     {
         // KDL::JntArray cur_configuration(arm->getChain()->getNrOfJoints()); // = arm_current_joint_states_;
         // KDL::SetToZero(cur_configuration);
@@ -230,7 +223,7 @@ private:
 
         // ROS_INFO("cur_configuration (%i x %i)", cur_configuration.rows(), cur_configuration.columns());
 
-        int error_code = arm->calculateInverseKinematics(arm_current_joint_states_, desired_end_effector_pose, final_joint_configuration);
+        int error_code = arm->calculateInverseKinematics(start_state, desired_end_effector_pose, final_joint_configuration);
 
         // Check status of IK and print error message if there is a failure
         if (error_code != 0)
@@ -253,13 +246,16 @@ private:
 
     KDL::Frame calc_end_effector_pose()
     {
+        int num_joints = arm->getChain()->getNrOfJoints();
+        
         KDL::Frame end_effector_pose;
-        // KDL::JntArray joint_positions = arm_current_joint_states_;
+        KDL::JntArray joint_positions(num_joints);
-        KDL::JntArray joint_positions(arm->getChain()->getNrOfJoints());
+
-        KDL::SetToZero(joint_positions);
+        // Copy current joint states values into JntArray
+        for (int i = 0; i < num_joints; ++i) {
+            joint_positions(i) = arm_current_joint_states_[i];
+        }
 
-        // ROS_INFO("joint_positions (%i x %i)", joint_positions.rows(), joint_positions.columns());
-        // ROS_INFO("arm chain # of joints: %d", arm->getChain()->getNrOfJoints());
         int error_code = arm->calculateForwardKinematics(joint_positions, end_effector_pose);
 
         // Check status of FK and do something if there is a failure
@@ -315,10 +311,6 @@ private:
         // Declare JointTrajectory message
         trajectory_msgs::JointTrajectory msg;
 
-        // Define intermediate joint positions
-        const vector<double> above_conveyor = {0, 0, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0};
-        const vector<double> above_bins = {0, M_PI, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0};
-
         // Fill the names of the joints to be controlled
         msg.joint_names = arm->get_joint_names();
 
@@ -383,51 +375,52 @@ private:
 
     bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target)
     {
-        // TODO: Tune threshold values
+        // Tune threshold values
-        float pos_thresh = 0.1; // Meters
+        float pos_thresh = 0.15; // Meters
-        float rot_thresh = 0.0875; // Radians
+        float rot_thresh = 0.05;
 
         float pos_err = fabs(cur.position.x - target.position.x) +
                         fabs(cur.position.y - target.position.y) +
                         fabs(cur.position.z - target.position.z);
 
+        // ROS_INFO_STREAM("pos_err: " << pos_err << " (" << pos_thresh << ")");
+
         if (pos_err > pos_thresh)
         {
             return false;
         }
 
-        vector<double> rpy1 = QuatToEuler(cur.orientation);
+        float qx_err = fabs(fabs(cur.orientation.x) - fabs(target.orientation.x));
-        vector<double> rpy2 = QuatToEuler(target.orientation);
+        float qy_err = fabs(fabs(cur.orientation.y) - fabs(target.orientation.y));
+        float qz_err = fabs(fabs(cur.orientation.z) - fabs(target.orientation.z));
+        float qw_err = fabs(fabs(cur.orientation.w) - fabs(target.orientation.w));
 
-        float rx_err = fabs(fmod(rpy1[0] - rpy2[0] + M_PI, M_PI));
+        ROS_INFO_STREAM("qx_err: " << qx_err << " (" << rot_thresh << ")");
-        float ry_err = fabs(fmod(rpy1[1] - rpy2[1] + M_PI, M_PI));
+        ROS_INFO_STREAM("qy_err: " << qy_err << " (" << rot_thresh << ")");
-        float rz_err = fabs(fmod(rpy1[2] - rpy2[2] + M_PI, M_PI));
+        ROS_INFO_STREAM("qz_err: " << qz_err << " (" << rot_thresh << ")");
+        ROS_INFO_STREAM("qw_err: " << qw_err << " (" << rot_thresh << ")");
 
-        if (rx_err > rot_thresh)
+        if (qx_err > rot_thresh)
         {
             return false;
         }
 
-        if (ry_err > rot_thresh)
+        if (qy_err > rot_thresh)
         {
             return false;
         }
 
-        if (rz_err > rot_thresh)
+        if (qz_err > rot_thresh)
         {
             return false;
         }
 
+        if (qw_err > rot_thresh) {
+            return false;
+        }
+
         return true;
     }
-
-    vector<double> QuatToEuler(geometry_msgs::Quaternion q) {
-        KDL::Rotation r = KDL::Rotation::Quaternion(q.x, q.y, q.z, q.w);
-        vector<double> rpy = {0, 0, 0};
-        r.GetRPY(rpy[0], rpy[1], rpy[2]);
-        
-        return rpy;
-    }
 };
 
 int main(int argc, char **argv)
@@ -443,20 +436,23 @@ int main(int argc, char **argv)
 
     double time_per_step = 3.0; // seconds
     int retry_attempts = 3;
-    double item_attach_z_adjustment = 0.025; // meters
+    double item_attach_z_adjustment = 0.05; // meters
     ArmController ac(node, &arm, time_per_step, retry_attempts, item_attach_z_adjustment);
 
+    ros::AsyncSpinner spinner(0);
+    spinner.start();
+
     // Subscribe to arm destination and joint states channels
     ros::Subscriber arm_destination_sub = node.subscribe(ARM_DESTINATION_CHANNEL, 1, &ArmController::arm_destination_callback, &ac);
 
-    ros::Subscriber gripper_state_sub = node.subscribe(GRIPPER_STATE_CHANNEL, 10, &ArmController::gripper_state_callback, &ac);
+    ros::Subscriber gripper_state_sub = node.subscribe(GRIPPER_STATE_CHANNEL, 1, &ArmController::gripper_state_callback, &ac);
 
-    ros::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 10, &ArmController::joint_state_callback, &ac);
+    ros::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 1, &ArmController::joint_state_callback, &ac);
 
     ROS_INFO("Setup complete");
 
     // Execute callbacks on new data until ctrl-c
-    ros::spin();
+    ros::waitForShutdown();
 
     return 0;
 }

src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp

@@ -99,12 +99,22 @@ int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_pos
     return status;
 }
 
-int ArmRepresentation::calculateInverseKinematics(const KDL::JntArray &cur_configuration,
+int ArmRepresentation::calculateInverseKinematics(const vector<double> &cur_configuration,
                                                   const KDL::Frame &desired_end_effector_pose,
                                                   KDL::JntArray &final_joint_configuration)
 {
     KDL::ChainIkSolverPos_LMA ik_solver(chain, 1e-3, 2000, 1e-8);
-    int status = ik_solver.CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration);
+    
+    // Convert cur_configuration vector to JntArray
+    int num_joints = chain.getNrOfJoints();
+    KDL::JntArray pos(num_joints);
+    KDL::SetToZero(pos);
+
+    for (int i = 0; i < num_joints; ++i) {
+        pos(i) = cur_configuration[i];
+    }
+
+    int status = ik_solver.CartToJnt(pos, desired_end_effector_pose, final_joint_configuration);
 
     return status;
 }

src/rrrobot_ws/src/rrrobot/src/cv_model.py

@@ -38,7 +38,7 @@ def call_back(filename):
     # query model
     predicted_logits = model(input_tensor).squeeze()
     # print(predicted_logits.shape)
-    predicted_label = torch.argmax(predicted_logits).numpy().tolist()
+    predicted_label = torch.argmax(predicted_logits).detach().numpy().tolist()
 
     type_dict = {0: 'cardboard',
                  1: 'glass'    ,

src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp

@@ -218,7 +218,7 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg)
 		geometry_msgs::Point p;
 		geometry_msgs::Quaternion q;
 
-		// pick up from top
+		// pick up from top (-z direction)
 		if (area_top > .002)
 		{
 			std::cout << "picking up object from top" << std::endl;
@@ -228,23 +228,23 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg)
 
 			// rpy = 0 0 0
 			q.x = 0;
-			q.y = 0;
+			q.y = 0.707;
 			q.z = 0;
-			q.w = 1;
+			q.w = 0.707;
 		}
 		else
 		{
-			// pick up from front
+			// pick up from front (+x direction)
 			std::cout << "picking up object from front" << std::endl;
 			p.x = xmin;
 			p.y = (ymin + ymax) / 2;
 			p.z = (zmax + zmin) / 2;
 
 			// rpy = -pi/2 0 pi/2
-			q.x = -0.63;
+			q.x = 0.707;
-			q.y = 0;
+			q.y = 0.0;
-			q.z = 0.63;
+			q.z = 0.0;
-			q.w = 0.44;
+			q.w = 0.707;
 		}
 
 		pose.position = p;
@@ -271,18 +271,16 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg)
 
 int main(int argc, char **argv)
 {
-	// TODO: Subscribe to depth camera topic
 	// Last argument is the default name of the node.
 	ros::init(argc, argv, "depth_camera_node");
 
 	ros::NodeHandle node;
 
+	// Subscribe to depth camera topic
 	ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback);
 
+	// Publish object's current location to topic for RRRobot node to listen to
 	pub = node.advertise<geometry_msgs::Pose>(DESIRED_GRASP_POSE_CHANNEL, 1);
 
 	ros::spin();
-	// TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item
-
-	// TODO: Publish object's current location to topic for RRRobot node to listen to
 }

src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp

@@ -99,7 +99,7 @@ private:
             image_file_msg.data = models[rand_idx].image_file;
             cout << "Spawning " << msg.model_name << endl;
 
-            // TODO: Determine position to spawn items
+            // Determine position to spawn items
             // can place objects on conveyor belt just upstream from the arms
             // at roughly (0.2516105, 5.474367, 0.935669)
             // x range: 1.022548-1.413952

src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp

@@ -126,6 +126,8 @@ public:
 		set_conveyor(0);
 
 		desired_grasp_pose = grasp_pose;
+		// TODO: Tune z offset so end effector doesn't hit object
+		desired_grasp_pose.position.z -= 0.025;
 
 		if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION)
 		{

src/rrrobot_ws/src/rrrobot/test/test_arm.cpp

@@ -36,130 +36,8 @@
 
 #include "arm_representation.h"
 
-// using namespace std;
+using namespace std;
-using std::cin;
-using std::cout;
-using std::endl;
-using std::string;
 
-// class ArmRepresentation
-// {
-// public:
-//     ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.92, 1)))
-//     {
-//         const double base_len = 0.2;
-//         const double shoulder_height = 0.1273;
-//         const double upper_arm_length = 0.612;
-//         const double forearm_length = 0.5723;
-//         const double shoulder_offset = 0.220941;
-//         const double elbow_offset = -0.1719;
-//         const double wrist_1_length = 0.163941 - elbow_offset - shoulder_offset;
-//         const double wrist_2_length = 0.1157;
-//         const double wrist_3_length = 0.0922;
-
-//         // KDL::Vector pos; //(0, 0, base_len/2);
-//         // KDL::Rotation rot;
-
-//         // The joints might be off by one
-//         KDL::Segment linear_arm_actuator("linear_arm_actuator_joint",
-//                                          KDL::Joint(KDL::Joint::JointType::None), base_pose);
-//         arm.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));
-//         KDL::Segment base_link("base_link", KDL::Joint(KDL::Joint::JointType::TransY),
-//                                KDL::Frame(rot_base, pos_base));
-//         arm.addSegment(base_link);
-
-//         const KDL::Vector pos_shoulder(0, 0, shoulder_height);
-//         const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0));
-//         KDL::Segment shoulder_link("shoulder_link", KDL::Joint(KDL::Joint::JointType::RotZ),
-//                                    KDL::Frame(rot_shoulder, pos_shoulder));
-//         arm.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));
-//         KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint(KDL::Joint::JointType::RotY),
-//                                     KDL::Frame(rot_upper_arm, pos_upper_arm));
-//         arm.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));
-//         KDL::Segment forearm_link("forearm_link", KDL::Joint(KDL::Joint::JointType::RotY),
-//                                   KDL::Frame(rot_forearm, pos_forearm));
-//         arm.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));
-//         KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint(KDL::Joint::JointType::RotY),
-//                                   KDL::Frame(rot_wrist_1, pos_wrist_1));
-//         arm.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));
-//         KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint(KDL::Joint::JointType::RotZ),
-//                                   KDL::Frame(rot_wrist_2, pos_wrist_2));
-//         arm.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));
-//         KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint(KDL::Joint::JointType::RotY),
-//                                   KDL::Frame(rot_wrist_3, pos_wrist_3));
-//         arm.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));
-//         KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None),
-//                              KDL::Frame(rot_ee, pos_ee));
-//         arm.addSegment(ee_link);
-
-//         // arm.addSegment(base_link);
-//         // arm.addSegment(shoulder_link);
-//         // arm.addSegment(upper_arm_link);
-//         // arm.addSegment(forearm_link);
-//         // arm.addSegment(wrist_1_link);
-//         // arm.addSegment(wrist_2_link);
-//         // arm.addSegment(wrist_3_link);
-//         // arm.addSegment(ee_link);
-
-//         fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm));
-//         ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm));
-//     }
-
-//     int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose)
-//     {
-//         return fk_solver->JntToCart(joint_positions, end_effector_pose);
-//     }
-
-//     int calculateInverseKinematics(const KDL::JntArray &cur_configuration,
-//                                    const KDL::Frame &desired_end_effector_pose,
-//                                    KDL::JntArray &final_joint_configuration)
-//     {
-//         return ik_solver->CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration);
-//     }
-
-//     string *get_joint_names()
-//     {
-//         // TODO: return ["linear_arm_actuator_joint",
-//         //         "shoulder_pan_joint",
-//         //         "shoulder_lift_joint",
-//         //         "elbow_joint",
-//         //         "wrist_1_joint",
-//         //         "wrist_2_joint",
-//         //         "wrist_3_joint"];
-//     }
-
-//     const KDL::Chain &getArm() const
-//     {
-//         return arm;
-//     }
-
-// private:
-//     KDL::Chain arm;
-
-//     std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver;
-//     std::unique_ptr<KDL::ChainIkSolverPos_LMA> ik_solver;
-// };
 
 int main(int argc, char **argv)
 {
@@ -174,7 +52,7 @@ int main(int argc, char **argv)
     // std::cin >> joint;
 
     KDL::Frame end_effector_pose;
-    std::ofstream f("data.txt");
+    std::ofstream f("arm_test.debug");
     int error_code;
     for (joint = 0; joint < arm.getChain()->getNrOfJoints(); ++joint)
     {
@@ -190,7 +68,7 @@ int main(int argc, char **argv)
 
             f << end_effector_pose.p.x() << "," << end_effector_pose.p.y() << "," << end_effector_pose.p.z() << '\n';
 
-            error_code = arm.calculateInverseKinematics(pos, end_effector_pose, pos);
+            error_code = arm.calculateInverseKinematics(vector<double>(arm.getChain()->getNrOfJoints(), 0.0), end_effector_pose, pos);
 
             if (error_code != 0)
             {