Changes to arm controller to get it closer to working, and some tests for verifying the correctness of the KDL arm representation

src/rrrobot_ws/src/rrrobot/CMakeLists.txt

@@ -73,10 +73,14 @@ add_executable(rrrobot_node src/rrrobot_node.cpp)
 add_dependencies(rrrobot_node ${catkin_EXPORTED_TARGETS} rrrobot_generate_messages_cpp)
 target_link_libraries(rrrobot_node ${catkin_LIBRARIES})
 
-add_executable(arm_controller_node src/arm_controller_node.cpp)
+add_executable(arm_controller_node src/arm_controller_node.cpp  src/arm_representation.cpp)
 add_dependencies(arm_controller_node ${catkin_EXPORTED_TARGETS})
 target_link_libraries(arm_controller_node ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
 
+add_executable(test_arm test/test_arm.cpp src/arm_representation.cpp)
+add_dependencies(test_arm ${catkin_EXPORTED_TARGETS})
+target_link_libraries(test_arm ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
+
 add_executable(depth_camera_node src/depth_camera_node.cpp)
 add_dependencies(depth_camera_node ${catkin_EXPORTED_TARGETS})
 target_link_libraries(depth_camera_node ${catkin_LIBRARIES} ${PCL_LIBRARIES})

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

@@ -0,0 +1,59 @@
+// arm_controller_node.cpp
+
+#include <algorithm>
+#include <vector>
+#include <string>
+#include <memory>
+
+#include <ros/ros.h>
+
+#include <std_msgs/Float32.h>
+#include <std_msgs/String.h>
+#include <std_srvs/Trigger.h>
+
+#include <osrf_gear/VacuumGripperControl.h>
+#include <trajectory_msgs/JointTrajectory.h>
+
+#include "topic_names.h"
+#include "rrrobot/arm_command.h"
+
+#include <kdl/chain.hpp>
+#include <kdl/segment.hpp>
+#include <kdl/rigidbodyinertia.hpp>
+#include <kdl/rotationalinertia.hpp>
+#include <kdl/joint.hpp>
+#include <kdl/frames.hpp>
+#include <LinearMath/btTransform.h>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainiksolverpos_lma.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+
+#include <fstream>
+#include <iostream>
+#include <string>
+
+// using namespace std;
+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)));
+
+    int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose);
+
+    int calculateInverseKinematics(const KDL::JntArray &cur_configuration,
+                                   const KDL::Frame &desired_end_effector_pose,
+                                   KDL::JntArray &final_joint_configuration);
+
+    std::vector<string> get_joint_names();
+
+    KDL::Chain *getChain();
+
+private:
+    KDL::Chain chain;
+
+    // KDL::ChainFkSolverPos_recursive fk_solver;
+    // KDL::ChainIkSolverPos_LMA ik_solver;
+};

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

@@ -1,3 +1,24 @@
 #!/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}}'
+#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}}'
+rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location:
+  position:
+    x: 1.2
+    y: 0.2
+    z: 1.0
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    w: 0.0
+drop_location:
+  position:
+    x: -0.3
+    y: 1.15
+    z: 1.5
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    w: 0.0" \
+-1

src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test_continuous.sh

@@ -0,0 +1,23 @@
+#!/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}}'
+rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location:
+  position:
+    x: 1.2
+    y: 0.2
+    z: 1.0
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    w: 0.0
+drop_location:
+  position:
+    x: -0.3
+    y: 1.15
+    z: 1.5
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    w: 0.0"

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

@@ -18,6 +18,7 @@
 
 #include "topic_names.h"
 #include "rrrobot/arm_command.h"
+#include "arm_representation.h"
 
 #include <kdl/chain.hpp>
 #include <kdl/segment.hpp>
@@ -31,191 +32,227 @@
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/chainidsolver_recursive_newton_euler.hpp>
 
+#include <ros/console.h>
+
 #include <cstdlib>
 
 using namespace std;
 
-class ArmRepresentation
+// 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("linear_arm_actuator_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("shoulder_pan_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("shoulder_lift_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("elbow_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("wrist_1_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("wrist_2_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("wrist_3_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);
+
+//         ROS_INFO("Arm has %d joints", arm->getNrOfJoints());
+//         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);
+//     }
+
+//     vector<string> get_joint_names()
+//     {
+//         vector<string> joints;
+
+//         for (int segment = 0; segment < arm->getNrOfSegments(); ++segment)
+//         {
+//             KDL::Joint cur = arm.getSegment(segment).getJoint();
+
+//             if (cur.getType() != KDL::Joint::JointType::None)
+//             {
+//                 joints.push_back(cur.getName());
+//             }
+//         }
+
+//         return joints;
+//     }
+
+//     const KDL::Chain &getChain() const
+//     {
+//         return arm;
+//     }
+
+// private:
+//     KDL::Chain arm;
+
+//     std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver;
+//     std::unique_ptr<KDL::ChainIkSolverPos_LMA> ik_solver;
+// };
+
+class ArmController
 {
 public:
-    ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.92, 1)))
+    ArmController(ros::NodeHandle &node, ArmRepresentation *arm_) : gripper_enabled_(false), item_attached_(false), arm(arm_)
     {
-        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));
-    }
-
-    bool calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose)
-    {
-        return fk_solver->JntToCart(joint_positions, end_effector_pose);
-    }
-
-    bool 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);
-    }
-
-    vector<string> get_joint_names() {
-        vector<string> joints;
-        
-        for (int segment = 0; segment < arm.getNrOfSegments(); ++segment) {
-            KDL::Joint cur = arm.getSegment(segment).getJoint();
-            
-            if (cur.getType() != KDL::Joint::JointType::None)
-            {
-                joints.push_back(cur.getName());
-            }
-        }
-
-        return joints;
-    }
-
-    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;
-};
-
-
-class ArmController {
-public:
-    ArmController(ros::NodeHandle & node) : gripper_enabled_(false), item_attached_(false) {
         arm_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(ARM_COMMAND_CHANNEL, 10);
 
         gripper_ = node.serviceClient<osrf_gear::VacuumGripperControl>(GRIPPER_CONTROL_CHANNEL);
 
-        arm = ArmRepresentation();
+        // arm = std::move(arm_);
+        ROS_INFO("Arm size (in constructor): %d", arm->getChain()->getNrOfJoints());
+        arm_current_joint_states_ = KDL::JntArray(arm->getChain()->getNrOfJoints());
+        KDL::SetToZero(arm_current_joint_states_);
     }
 
-    void joint_state_callback(const sensor_msgs::JointState & joint_state_msg) {
+    void joint_state_callback(const sensor_msgs::JointState &joint_state_msg)
-        ROS_INFO_STREAM_THROTTLE(10, "Arm Joint States (throttled to 0.1 Hz):\n" << joint_state_msg);
+    {
-        
+        // ROS_INFO("Received joint state");
-        // ROS_INFO_STREAM("Joint States:\n" << joint_state_msg);
+
-        
         // Convert std_msgs::double[] to KDL::JntArray
-        int nbr_joints = arm.getArm().getNrOfJoints();
+        int nbr_joints = arm->getChain()->getNrOfJoints();
         vector<string> msg_joint_names = joint_state_msg.name;
-        vector<string> cur_joint_names = arm.get_joint_names();
+        vector<string> cur_joint_names = arm->get_joint_names();
         vector<double> position = joint_state_msg.position;
 
-        for (int i = 0; i < nbr_joints; ++i) {
+        // Print joint name vectors for debugging
-            for (int j = 0; i < nbr_joints; ++j) {
+        // ROS_INFO_STREAM("msg_joint_names: {");
-                if (msg_joint_names[i].compare(cur_joint_names[j]) == 0) {
+        // 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;
+        }
+
+        for (size_t i = 0; i < position.size(); ++i)
+        {
+            for (size_t j = 0; j < cur_joint_names.size(); ++j)
+            {
+                if (msg_joint_names[i].compare(cur_joint_names[j]) == 0)
+                {
                     arm_current_joint_states_(j) = position[i];
                 }
             }
         }
     }
 
-    void gripper_state_callback(const osrf_gear::VacuumGripperState::ConstPtr & gripper_state_msg) {
+    void gripper_state_callback(const osrf_gear::VacuumGripperState::ConstPtr &gripper_state_msg)
+    {
         gripper_enabled_ = gripper_state_msg->enabled;
         item_attached_ = gripper_state_msg->attached;
     }
 
-    void arm_destination_callback(const rrrobot::arm_command & target_pose) {
+    void arm_destination_callback(const rrrobot::arm_command &target_pose)
-        int nbr_joints = arm.getArm().getNrOfJoints();
+    {
+        ROS_INFO("Received target pose");
+
+        int nbr_joints = arm->getChain()->getNrOfJoints();
         vector<double> positions;
         double time_from_start = 5; // Seconds
-        
+
         // Move arm to pickup item from conveyor belt
         positions = calc_joint_positions(target_pose.grab_location);
         send_joint_trajectory(positions, time_from_start);
 
         // Wait until target position is 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.grab_location))
+        {
             // TODO: Do something if not able to reach target
             continue;
         }
-        
+
         // Turn on suction
-        while (!gripper_enabled_) {
+        while (!gripper_enabled_)
+        {
             gripper_control(true);
         }
 
         // Wait until object is attached
-        while (!item_attached_) {
+        while (!item_attached_)
+        {
             // TODO: Do something if item is not able to attach
             continue;
         }
@@ -223,20 +260,23 @@ public:
         // Move item to desired position
         positions = calc_joint_positions(target_pose.drop_location);
         send_joint_trajectory(positions, time_from_start);
-        
+
         // Wait until target position is reached
-        while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.drop_location)) {
+        while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.drop_location))
+        {
             // TODO: Do something if not able to reach target
             continue;
         }
 
         // Turn off suction
-        while (gripper_enabled_) {
+        while (gripper_enabled_)
+        {
             gripper_control(false);
         }
 
         // Wait until object is detached
-        while (item_attached_) {
+        while (item_attached_)
+        {
             // TODO: Do something if item doesn't detach
             continue;
         }
@@ -248,39 +288,60 @@ private:
     ros::Publisher arm_joint_trajectory_publisher_;
     ros::ServiceClient gripper_;
     KDL::JntArray arm_current_joint_states_;
-    ArmRepresentation arm;
+    ArmRepresentation *arm;
 
-    vector<double> calc_joint_positions(const geometry_msgs::Pose & pose) {
+    vector<double> calc_joint_positions(const geometry_msgs::Pose &pose)
-        KDL::JntArray cur_configuration = arm_current_joint_states_;
+    {
+        // KDL::JntArray cur_configuration(arm->getChain()->getNrOfJoints()); // = arm_current_joint_states_;
+        // KDL::SetToZero(cur_configuration);
         KDL::Frame desired_end_effector_pose = pose_to_frame(pose);
-        KDL::JntArray final_joint_configuration = KDL::JntArray();
+        KDL::JntArray final_joint_configuration(arm->getChain()->getNrOfJoints());
-        
+
+        // 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);
+
         // Check status of IK and print error message if there is a failure
-        if (!arm.calculateInverseKinematics(cur_configuration, desired_end_effector_pose, final_joint_configuration)) {
+        if (error_code != 0)
-            ROS_ERROR("Inverse Kinematics Failure");
+        {
+            ROS_ERROR("Inverse Kinematics Failure: %i", error_code);
         }
 
         // Convert data attribute (Eigen::VectorXd) of KDL::JntArray to double[] via data() function
-        int nbr_joints = arm.getArm().getNrOfJoints();
+        int nbr_joints = arm->getChain()->getNrOfJoints();
         Eigen::VectorXd mat = final_joint_configuration.data;
-        vector<double> positions(mat.data(), mat.data() + mat.rows() * mat.cols());
+        // cout << mat << endl;
-        
+        vector<double> positions;
+        for (size_t idx = 0; idx < arm->getChain()->getNrOfJoints(); ++idx)
+        {
+            positions.push_back(mat[idx]);
+        }
+
         return positions;
     }
 
-    const KDL::Frame& calc_end_effector_pose() {
+    KDL::Frame calc_end_effector_pose()
+    {
         KDL::Frame end_effector_pose;
-        KDL::JntArray joint_positions = arm_current_joint_states_;
+        // KDL::JntArray joint_positions = arm_current_joint_states_;
-        
+        KDL::JntArray joint_positions(arm->getChain()->getNrOfJoints());
+        KDL::SetToZero(joint_positions);
+
+        // 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
-        if (!arm.calculateForwardKinematics(joint_positions, end_effector_pose)) {
+        if (error_code != 0)
-            ROS_ERROR("Forward Kinematics Failure");
+        {
+            ROS_ERROR("Forward Kinematics Failure: %i", error_code);
         }
 
         return end_effector_pose;
     }
 
-    const KDL::Frame& pose_to_frame(const geometry_msgs::Pose & pose) {
+    KDL::Frame pose_to_frame(const geometry_msgs::Pose &pose)
+    {
         double p_x = pose.position.x;
         double p_y = pose.position.y;
         double p_z = pose.position.z;
@@ -290,10 +351,13 @@ private:
         double q_z = pose.orientation.z;
         double q_w = pose.orientation.w;
 
-        return KDL::Frame(KDL::Rotation::Quaternion(q_x, q_y, q_z, q_w), KDL::Vector(p_x, p_y, p_z));
+        KDL::Rotation rot(KDL::Rotation::Quaternion(q_x, q_y, q_z, q_w));
+        KDL::Vector pos(p_x, p_y, p_z);
+        return KDL::Frame(rot, pos);
     }
 
-    const geometry_msgs::Pose& frame_to_pose(const KDL::Frame & frame) {
+    geometry_msgs::Pose frame_to_pose(const KDL::Frame &frame)
+    {
         double p_x = frame.p.x();
         double p_y = frame.p.y();
         double p_z = frame.p.z();
@@ -315,35 +379,42 @@ private:
         return pose;
     }
 
-    void send_joint_trajectory(vector<double> & positions, double time_from_start) {
+    void send_joint_trajectory(const vector<double> &positions, double time_from_start)
+    {
         // Declare JointTrajectory message
         trajectory_msgs::JointTrajectory msg;
-        
+
         // Fill the names of the joints to be controlled
-        msg.joint_names = arm.get_joint_names();
+        msg.joint_names = arm->get_joint_names();
-        
+
         // Create one point in the trajectory
         msg.points.resize(1);
+        msg.points[0].positions.resize(msg.joint_names.size(), 0.0);
 
         // Set joint positions
-        msg.points[0].positions = positions;
+        for (size_t idx = 0; idx < positions.size(); ++idx)
-        
+        {
+            msg.points[0].positions[idx] = positions[idx];
+        }
+
         // How long to take getting to the point (floating point seconds)
         msg.points[0].time_from_start = ros::Duration(time_from_start);
 
-        ROS_INFO_STREAM("Sending command:\n" << msg);
+        //ROS_INFO("Sending command: \n%s", msg);
 
         arm_joint_trajectory_publisher_.publish(msg);
     }
 
-    void gripper_control(bool state) {
+    void gripper_control(bool state)
+    {
         osrf_gear::VacuumGripperControl srv;
         srv.request.enable = state;
 
         gripper_.call(srv);
     }
 
-    bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target) {
+    bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target)
+    {
         // TODO: Tune threshold values
         float pos_thresh = 0.01; // Meters
         float rot_thresh = 0.02; // Radians
@@ -351,8 +422,9 @@ private:
         float pos_err = fabs(cur.position.x - target.position.x) +
                         fabs(cur.position.y - target.position.y) +
                         fabs(cur.position.z - target.position.z);
-        
+
-        if (pos_err > pos_thresh) {
+        if (pos_err > pos_thresh)
+        {
             return false;
         }
 
@@ -360,20 +432,24 @@ private:
         float qy_err = fabs(cur.orientation.y - target.orientation.y);
         float qz_err = fabs(cur.orientation.z - target.orientation.z);
         float qw_err = fabs(cur.orientation.w - target.orientation.w);
-        
+
-        if (qx_err > rot_thresh) {
+        if (qx_err > rot_thresh)
+        {
             return false;
         }
 
-        if (qy_err > rot_thresh) {
+        if (qy_err > rot_thresh)
+        {
             return false;
         }
 
-        if (qz_err > rot_thresh) {
+        if (qz_err > rot_thresh)
+        {
             return false;
         }
 
-        if (qw_err > rot_thresh) {
+        if (qw_err > rot_thresh)
+        {
             return false;
         }
 
@@ -381,22 +457,24 @@ private:
     }
 };
 
-
+int main(int argc, char **argv)
-int main(int argc, char ** argv) {
+{
     cout << "Starting arm_controller_node" << endl;
-    
+
     // Last argument is the default name of the node.
     ros::init(argc, argv, "arm_controller_node");
 
     ros::NodeHandle node;
 
-    ArmController ac(node);
+    ArmRepresentation arm;
-    
+
+    ArmController ac(node, &arm);
+
     // 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 joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 10, &ArmController::joint_state_callback, &ac);
 
     ROS_INFO("Setup complete");

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

@@ -0,0 +1,127 @@
+// arm_controller_node.cpp
+
+#include "arm_representation.h"
+
+// using namespace std;
+using std::string;
+using std::vector;
+
+ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose)
+{
+    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);
+    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));
+    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));
+    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));
+    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));
+    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));
+    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));
+    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));
+    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));
+    KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None),
+                         KDL::Frame(rot_ee, pos_ee));
+    chain.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 = KDL::ChainFkSolverPos_recursive(chain);
+    // ik_solver = KDL::ChainIkSolverPos_LMA(chain);
+
+    // fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm));
+    // ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm));
+}
+
+int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose)
+{
+    KDL::ChainFkSolverPos_recursive fk_solver = KDL::ChainFkSolverPos_recursive(chain);
+    int status = fk_solver.JntToCart(joint_positions, end_effector_pose);
+
+    return status;
+}
+
+int ArmRepresentation::calculateInverseKinematics(const KDL::JntArray &cur_configuration,
+                                                  const KDL::Frame &desired_end_effector_pose,
+                                                  KDL::JntArray &final_joint_configuration)
+{
+    KDL::ChainIkSolverPos_LMA ik_solver = KDL::ChainIkSolverPos_LMA(chain);
+    int status = ik_solver.CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration);
+
+    return status;
+}
+
+vector<string> ArmRepresentation::get_joint_names()
+{
+    vector<string> joint_names;
+
+    for (auto it : chain.segments)
+    {
+        if (it.getJoint().getType() != KDL::Joint::JointType::None)
+            joint_names.push_back(it.getJoint().getName());
+    }
+
+    return joint_names;
+}
+
+KDL::Chain *ArmRepresentation::getChain()
+{
+    return &chain;
+}

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

@@ -0,0 +1,231 @@
+// arm_controller_node.cpp
+
+#include <algorithm>
+#include <vector>
+#include <string>
+#include <memory>
+
+#include <ros/ros.h>
+
+#include <std_msgs/Float32.h>
+#include <std_msgs/String.h>
+#include <std_srvs/Trigger.h>
+
+#include <osrf_gear/VacuumGripperControl.h>
+#include <trajectory_msgs/JointTrajectory.h>
+
+#include "topic_names.h"
+#include "rrrobot/arm_command.h"
+
+#include <kdl/chain.hpp>
+#include <kdl/segment.hpp>
+#include <kdl/rigidbodyinertia.hpp>
+#include <kdl/rotationalinertia.hpp>
+#include <kdl/joint.hpp>
+#include <kdl/frames.hpp>
+#include <LinearMath/btTransform.h>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainiksolverpos_lma.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+#include <kdl/frames_io.hpp>
+
+#include <fstream>
+#include <iostream>
+#include <string>
+
+#include "arm_representation.h"
+
+// 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)
+{
+    ArmRepresentation arm;
+
+    KDL::JntArray pos(arm.getChain()->getNrOfJoints());
+    KDL::SetToZero(pos);
+
+    int joint;
+
+    // std::cout << "Enter joint to exercise: ";
+    // std::cin >> joint;
+
+    KDL::Frame end_effector_pose;
+    std::ofstream f("data.txt");
+    int error_code;
+    for (joint = 0; joint < arm.getChain()->getNrOfJoints(); ++joint)
+    {
+        for (double pos_val = 0.0; pos_val <= 2 * M_PI; pos_val += 0.1)
+        {
+            pos(joint) = pos_val;
+            error_code = arm.calculateForwardKinematics(pos, end_effector_pose);
+
+            if (error_code != 0)
+            {
+                ROS_ERROR("Forward Kinematics Failure: %i", error_code);
+            }
+
+            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);
+
+            if (error_code != 0)
+            {
+                ROS_ERROR("Inverse Kinematics Failure: %i", error_code);
+            }
+        }
+    }
+
+    double linear_arm_actuator_joint, shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint;
+
+    while (true)
+    {
+        cout << "linear_arm_actuator_joint: ";
+        cin >> pos(0); //linear_arm_actuator_joint;
+        cout << "shoulder_pan_joint: ";
+        cin >> pos(1); //shoulder_pan_joint;
+        cout << "shoulder_lift_joint: ";
+        cin >> pos(2); //shoulder_lift_joint;
+        cout << "elbow_joint: ";
+        cin >> pos(3); //elbow_joint;
+        cout << "wrist_1_joint: ";
+        cin >> pos(4); //wrist_1_joint;
+        cout << "wrist_2_joint: ";
+        cin >> pos(5); //wrist_2_joint;
+        cout << "wrist_3_joint: ";
+        cin >> pos(6); //wrist_3_joint;
+
+        error_code = arm.calculateForwardKinematics(pos, end_effector_pose);
+
+        if (error_code != 0)
+        {
+            ROS_ERROR("Forward kinematics failure: %i", error_code);
+        }
+
+        cout << "x,y,z: " << end_effector_pose.p.x() << ", " << end_effector_pose.p.y() << ", " << end_effector_pose.p.z() << endl;
+        cout << end_effector_pose.M << endl;
+    }
+}