Merge branch 'arm_control' into GEAR

2025-08-17 14:52:46 +00:00 · 2020-04-16 20:59:12 -04:00
parent 54c38f8119 ccae110574
commit d9e2073644
22 changed files with 1827 additions and 31 deletions
--- a/docker_env/gazebo/Dockerfile
+++ b/docker_env/gazebo/Dockerfile
@@ -17,6 +17,7 @@ RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
 RUN apt install -y python-rosdep
 RUN rosdep init
 RUN rosdep update
+RUN apt-get install -y ros-melodic-pid
 # user id 1000 should be the same as the host user, so that you can access files
 # from inside the docker container and also on the host
 RUN useradd -u 1000 rrrobot
--- a/rrrobot_src/src/arm_control/CMakeLists.txt
+++ b/rrrobot_src/src/arm_control/CMakeLists.txt
@@ -0,0 +1,268 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(arm_control)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11;-g3)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  geometry_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+find_package(orocos_kdl)
+find_package(kdl_parser)
+
+# sdfconfig
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(SDF sdformat REQUIRED)
+
+find_package(ignition-math4 REQUIRED)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include ../simulation_env/include
+#  LIBRARIES arm_control
+#  CATKIN_DEPENDS roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ include/arm_control
+  ${catkin_INCLUDE_DIRS}
+  ${roscpp_INCLUDE_DIRS}
+  /usr/include/bullet
+  /usr/include/sdformat-6.2
+  /usr/include/ignition/math4
+  /home/rrrobot/rrrobot_src/devel/include/
+)
+include_directories(
+  /usr/include/eigen3
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/arm_control.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/arm_controller.cpp src/arm.cpp)
+add_executable(test test/test.cpp src/arm.cpp)
+add_executable(arm_movement_demo test/arm_movement_demo.cpp src/arm.cpp)
+add_executable(pub_sub test/test_pub_sub.cpp)
+add_executable(angles_subscriber test/angles_subscriber.cpp src/arm.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node
+  ${catkin_LIBRARIES}
+  ${kdl_parser_LIBRARIES}
+  ${orocos_kdl_LIBRARIES}
+  ${SDF_LIBRARIES}
+  ignition-math4::ignition-math4
+)
+
+target_link_libraries(test
+  ${catkin_LIBRARIES}
+  ${kdl_parser_LIBRARIES}
+  ${orocos_kdl_LIBRARIES}
+  ${SDF_LIBRARIES}
+  ignition-math4::ignition-math4
+)
+
+target_link_libraries(arm_movement_demo
+  ${catkin_LIBRARIES}
+  ${kdl_parser_LIBRARIES}
+  ${orocos_kdl_LIBRARIES}
+  ${SDF_LIBRARIES}
+  ignition-math4::ignition-math4
+)
+
+target_link_libraries(pub_sub
+  ${catkin_LIBRARIES}
+  ${kdl_parser_LIBRARIES}
+  ${orocos_kdl_LIBRARIES}
+  ${SDF_LIBRARIES}
+  ignition-math4::ignition-math4
+)
+
+target_link_libraries(angles_subscriber
+  ${catkin_LIBRARIES}
+  ${kdl_parser_LIBRARIES}
+  ${orocos_kdl_LIBRARIES}
+  ${SDF_LIBRARIES}
+  ignition-math4::ignition-math4
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+install(TARGETS ${PROJECT_NAME}_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+catkin_add_gtest(${PROJECT_NAME}-test test/test_internal_arm_representation.cpp src/arm.cpp)
+target_link_libraries(${PROJECT_NAME}-test
+  ${catkin_LIBRARIES}
+  ${kdl_parser_LIBRARIES}
+  ${orocos_kdl_LIBRARIES}
+  ${SDF_LIBRARIES}
+  ignition-math4::ignition-math4
+)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- a/rrrobot_src/src/arm_control/include/arm_control/arm.h
+++ b/rrrobot_src/src/arm_control/include/arm_control/arm.h
@@ -0,0 +1,47 @@
+#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 <memory>
+
+#include <string>
+
+class Arm
+{
+public:
+    /*
+     * sdf_file: this is the sdf file that defines the model. This
+     *      class will use that description of the robot to model 
+     *      its dynamics.
+     */
+    Arm(const std::string &sdf_file);
+
+    /*
+     * 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);
+
+    bool calculateInverseKinematics(const KDL::JntArray &cur_configuration, const KDL::Frame &desired_end_effector_frame, KDL::JntArray &final_configuration);
+
+    int calculateInverseDynamics(const KDL::JntArray &pos, const KDL::JntArray &vel, const KDL::JntArray &accel, const KDL::Wrenches &f_external, KDL::JntArray &required_torque);
+
+    const KDL::Chain &getArm() const;
+
+    void print() const;
+
+private:
+    KDL::Chain arm;
+
+    // Create solver based on kinematic chain
+    std::unique_ptr<KDL::ChainFkSolverPos_recursive> fksolver;
+    std::unique_ptr<KDL::ChainIkSolverPos_LMA> iksolver;
+    std::unique_ptr<KDL::ChainIdSolver_RNE> idsolver;
+};
--- a/rrrobot_src/src/arm_control/launch/arm.launch
+++ b/rrrobot_src/src/arm_control/launch/arm.launch
@@ -0,0 +1,123 @@
+<launch>
+    <node name="shoulder_pivot_pid" pkg="pid" type="controller" output="screen" >
+      <param name="Kp" value="450.0" />
+      <param name="Ki" value="5.0" />
+      <param name="Kd" value="600.0" />
+      <param name="upper_limit" value="10000" />
+      <param name="lower_limit" value="-10000" />
+      <param name="windup_limit" value="1000" />
+      <param name="cutoff_frequency" value="20" />
+      <param name="max_loop_frequency" value="105.0" />
+      <param name="min_loop_frequency" value="95.0" />
+      <param name="setpoint_timeout" value="-1.0" />
+      <param name="topic_from_controller" value="/arm_node/arm_commands/shoulder_pivot_torque" />
+      <param name="topic_from_plant" value="/arm_node/arm_positions/shoulder_pivot_angle" />
+      <param name="setpoint_topic" value="/arm_node/shoulder_pivot_setpoint" />
+    </node>
+
+    <node name="shoulder_joint_pid" pkg="pid" type="controller" output="screen" >
+      <param name="Kp" value="1200.0" />
+      <param name="Ki" value="40.0" />
+      <param name="Kd" value="1400.0" />
+      <param name="upper_limit" value="10000" />
+      <param name="lower_limit" value="-10000" />
+      <param name="windup_limit" value="1000" />
+      <param name="cutoff_frequency" value="20" />
+      <param name="max_loop_frequency" value="105.0" />
+      <param name="min_loop_frequency" value="95.0" />
+      <param name="setpoint_timeout" value="-1.0" />
+      <param name="topic_from_controller" value="/arm_node/arm_commands/shoulder_joint_torque" />
+      <param name="topic_from_plant" value="/arm_node/arm_positions/shoulder_joint_angle" />
+      <param name="setpoint_topic" value="/arm_node/shoulder_joint_setpoint" />
+    </node>
+
+    <node name="elbow_joint_pid" pkg="pid" type="controller" output="screen" >
+      <param name="Kp" value="400.0" />
+      <param name="Ki" value="15.0" />
+      <param name="Kd" value="400.0" />
+      <param name="upper_limit" value="10000" />
+      <param name="lower_limit" value="-10000" />
+      <param name="windup_limit" value="1000" />
+      <param name="cutoff_frequency" value="20" />
+      <param name="max_loop_frequency" value="105.0" />
+      <param name="min_loop_frequency" value="95.0" />
+      <param name="setpoint_timeout" value="-1.0" />
+      <param name="topic_from_controller" value="/arm_node/arm_commands/elbow_joint_torque" />
+      <param name="topic_from_plant" value="/arm_node/arm_positions/elbow_joint_angle" />
+      <param name="setpoint_topic" value="/arm_node/elbow_joint_setpoint" />
+    </node>
+
+    <node name="wrist_pivot_pid" pkg="pid" type="controller" output="screen" >
+      <param name="Kp" value="3.0" />
+      <param name="Ki" value="0.5" />
+      <param name="Kd" value="1.0" />
+      <param name="upper_limit" value="1000" />
+      <param name="lower_limit" value="-1000" />
+      <param name="windup_limit" value="100" />
+      <param name="cutoff_frequency" value="20" />
+      <param name="max_loop_frequency" value="105.0" />
+      <param name="min_loop_frequency" value="95.0" />
+      <param name="setpoint_timeout" value="-1.0" />
+      <param name="topic_from_controller" value="/arm_node/arm_commands/wrist_pivot_torque" />
+      <param name="topic_from_plant" value="/arm_node/arm_positions/wrist_pivot_angle" />
+      <param name="setpoint_topic" value="/arm_node/wrist_pivot_setpoint" />
+    </node>
+
+    <node name="wrist_joint_pid" pkg="pid" type="controller" output="screen" >
+      <param name="Kp" value="2.0" />
+      <param name="Ki" value="0.5" />
+      <param name="Kd" value="1.0" />
+      <param name="upper_limit" value="1000" />
+      <param name="lower_limit" value="-1000" />
+      <param name="windup_limit" value="100" />
+      <param name="cutoff_frequency" value="20" />
+      <param name="max_loop_frequency" value="105.0" />
+      <param name="min_loop_frequency" value="95.0" />
+      <param name="setpoint_timeout" value="-1.0" />
+      <param name="topic_from_controller" value="/arm_node/arm_commands/wrist_joint_torque" />
+      <param name="topic_from_plant" value="/arm_node/arm_positions/wrist_joint_angle" />
+      <param name="setpoint_topic" value="/arm_node/wrist_joint_setpoint" />
+    </node>
+
+    <node name="end_effector_pivot_pid" pkg="pid" type="controller" output="screen" >
+      <param name="Kp" value="0.1" />
+      <param name="Ki" value="0.01" />
+      <param name="Kd" value="0.05" />
+      <param name="upper_limit" value="1000" />
+      <param name="lower_limit" value="-1000" />
+      <param name="windup_limit" value="100" />
+      <param name="cutoff_frequency" value="20" />
+      <param name="max_loop_frequency" value="105.0" />
+      <param name="min_loop_frequency" value="95.0" />
+      <param name="setpoint_timeout" value="-1.0" />
+      <param name="topic_from_controller" value="/arm_node/arm_commands/end_effector_pivot_torque" />
+      <param name="topic_from_plant" value="/arm_node/arm_positions/end_effector_pivot_angle" />
+      <param name="setpoint_topic" value="/arm_node/end_effector_pivot_setpoint" />
+    </node>
+
+    <!-- <node name="servo_sim_node" pkg="pid" type="plant_sim" output="screen" >
+      <param name="plant_order" value="2" />
+    </node> -->
+
+    <node name="arm_controller" pkg="arm_control" type="arm_control_node" output="screen" />
+    <node name="arm_movement_demo" pkg="arm_control" type="arm_movement_demo" output="screen" />
+
+    <!-- rqt_plot is a resource hog, so if you're seeing high CPU usage, don't launch rqt_plot  -->
+    <!-- node name="rqt_plot" pkg="rqt_plot" type="rqt_plot"
+    args="/control_effort/data /state/data /setpoint/data" -->
+
+    <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" />
+
+    <!--node name="rqt_robot_monitor" pkg="rqt_robot_monitor" type="rqt_robot_monitor" -->
+
+    <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+      <arg name="world_name" value="rrrobot_setpoint.world"/> <!--Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
+      <arg name="paused" value="true"/>
+      <arg name="use_sim_time" value="true"/>
+      <arg name="gui" value="true"/>
+      <arg name="recording" value="false"/>
+      <arg name="debug" value="false"/>
+    </include>
+
+</launch>
--- a/rrrobot_src/src/arm_control/package.xml
+++ b/rrrobot_src/src/arm_control/package.xml
@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>arm_control</name>
+  <version>0.0.0</version>
+  <description>The arm_control package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="dwitcpa@todo.todo">dwitcpa</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/arm_control</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
--- a/rrrobot_src/src/arm_control/src/arm.cpp
+++ b/rrrobot_src/src/arm_control/src/arm.cpp
@@ -0,0 +1,203 @@
+#include <arm.h>
+
+#include <sdf/sdf.hh>
+#include <ignition/math/Pose3.hh>
+#include <LinearMath/btTransform.h>
+#include <kdl/frames_io.hpp>
+
+#include <iostream>
+#include <sstream>
+#include <unordered_map>
+
+using std::cout;
+using std::endl;
+using std::string;
+using std::stringstream;
+using std::unordered_map;
+using std::vector;
+
+struct FrameInformation
+{
+    string link_name;       // link name
+    KDL::Joint joint;       // joint information (name, type)
+    KDL::Frame link_frame;  // link location (world coordinates)
+    KDL::Frame joint_frame; // joint location relative to parent(link_name)
+    float mass;
+    KDL::Vector com_location; // relative to frame origin
+    KDL::RotationalInertia rotational_inertia;
+};
+
+/*
+ * sdf_file: this is the sdf file that defines the model. This
+ *      class will use that description of the robot to model 
+ *      its dynamics.
+ */
+Arm::Arm(const std::string &sdf_file)
+{
+    // read sdf file
+    sdf::SDFPtr sdf(new sdf::SDF());
+    sdf::init(sdf);
+    sdf::readFile(sdf_file, sdf);
+
+    // get model
+    const sdf::ElementPtr root = sdf->Root();
+    const sdf::ElementPtr model = root->GetElement("model");
+    const string model_name = model->Get<string>("name");
+    cout << "Found " << model_name << " model" << endl;
+
+    unordered_map<string, FrameInformation> links;
+    unordered_map<string, string> link_ordering;
+    string first_link;
+
+    // start reading links and joints
+    sdf::ElementPtr link = model->GetElement("link");
+    while (link)
+    {
+        const string name(link->Get<string>("name"));
+        links[name].link_name = name;
+
+        // Transformation from world to link coordinates
+        stringstream frame_location(link->GetElement("pose")->GetValue()->GetAsString());
+        float x, y, z;
+        float roll, pitch, yaw;
+        frame_location >> x >> y >> z >> roll >> pitch >> yaw;
+        KDL::Rotation rotation = KDL::Rotation::RPY(roll, pitch, yaw);
+        KDL::Vector link_position(x, y, z);
+        links[name].link_frame = KDL::Frame(rotation, link_position);
+
+        const sdf::ElementPtr inertial_data = link->GetElement("inertial");
+        float mass = inertial_data->Get<float>("mass");
+        links[name].mass = mass;
+
+        // Get the center of mass for this link
+        stringstream inertial_data_ss(inertial_data->GetElement("pose")->GetValue()->GetAsString());
+        inertial_data_ss >> links[name].com_location[0] >> links[name].com_location[1] >> links[name].com_location[2];
+        links[name].com_location += links[name].link_frame.p;
+
+        const sdf::ElementPtr inertia = inertial_data->GetElement("inertia");
+        links[name].rotational_inertia = KDL::RotationalInertia(
+            inertia->Get<float>("ixx"),
+            inertia->Get<float>("iyy"),
+            inertia->Get<float>("izz"),
+            inertia->Get<float>("ixy"),
+            inertia->Get<float>("ixz"),
+            inertia->Get<float>("iyz"));
+
+        link = link->GetNextElement("link");
+    }
+
+    sdf::ElementPtr joint = model->GetElement("joint");
+    while (joint) // TODO(dwitcpa): add support for translational joints
+    {
+        const string name(joint->Get<string>("name"));
+        const string parent(joint->GetElement("parent")->GetValue()->GetAsString());
+        const string child(joint->GetElement("child")->GetValue()->GetAsString());
+        link_ordering[parent] = child;
+        if (parent == string("world"))
+        {
+            joint = joint->GetNextElement("joint");
+            continue;
+        }
+
+        stringstream axis(joint->GetElement("axis")->GetElement("xyz")->GetValue()->GetAsString());
+
+        int axis_num = 0;
+        int cur;
+        while (axis >> cur)
+        {
+            if (cur != 0)
+                break;
+            axis_num++;
+        }
+        KDL::Joint::JointType joint_type = KDL::Joint::JointType::RotAxis;
+        if (axis_num == 0)
+            joint_type = KDL::Joint::JointType::RotX;
+        else if (axis_num == 1)
+            joint_type = KDL::Joint::JointType::RotY;
+        else if (axis_num == 2)
+            joint_type = KDL::Joint::JointType::RotZ;
+
+        links[child].joint = KDL::Joint(name, joint_type);
+
+        stringstream pose_string(joint->GetElement("pose")->GetValue()->GetAsString());
+        float x, y, z;
+        float roll, pitch, yaw;
+        pose_string >> x >> y >> z >> roll >> pitch >> yaw;
+
+        KDL::Rotation frame_rotation = KDL::Rotation::RPY(roll, pitch, yaw);
+        KDL::Vector frame_location(x, y, z);
+        links[child].joint_frame = KDL::Frame(frame_rotation, frame_location);
+
+        joint = joint->GetNextElement("joint");
+    }
+
+    string cur_link_name = "world";
+    KDL::Frame prev_frame(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(0, 0, 0));
+    KDL::Joint::JointType to_set(KDL::Joint::JointType::None);
+
+    while (link_ordering.find(cur_link_name) != link_ordering.end())
+    {
+        cur_link_name = link_ordering[cur_link_name];
+
+        // get the world coordinates of the joint
+        KDL::Frame joint_in_world = links[cur_link_name].link_frame * links[cur_link_name].joint_frame;
+        KDL::Frame prev_frame_inverse = prev_frame.Inverse();
+        KDL::Frame joint_relative_to_prev = prev_frame.Inverse() * joint_in_world;
+
+        KDL::Vector com_in_prev = joint_relative_to_prev * links[cur_link_name].com_location;
+
+        KDL::Joint::JointType next(links[cur_link_name].joint.getType());
+        KDL::RigidBodyInertia inertia(links[cur_link_name].mass, com_in_prev /*links[cur_link_name].com_location*/, links[cur_link_name].rotational_inertia);
+        KDL::Segment to_add(links[cur_link_name].link_name, KDL::Joint(links[cur_link_name].joint.getName(), to_set), joint_relative_to_prev, inertia);
+        to_set = next;
+
+        arm.addSegment(to_add);
+
+        prev_frame = joint_in_world;
+    }
+
+    arm.addSegment(KDL::Segment(string("pseudo_link"), KDL::Joint("pseudo_joint", to_set), KDL::Frame::Identity(), KDL::RigidBodyInertia()));
+
+    cout << "# of arm segments: " << arm.getNrOfSegments() << "\t# of arm joints: " << arm.getNrOfJoints() << endl;
+    fksolver.reset(new KDL::ChainFkSolverPos_recursive(arm));
+    iksolver.reset(new KDL::ChainIkSolverPos_LMA(arm));
+    idsolver.reset(new KDL::ChainIdSolver_RNE(arm, KDL::Vector(0, 0, -9.81)));
+}
+
+/*
+ * 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);
+}
+
+bool Arm::calculateInverseKinematics(const KDL::JntArray &cur_configuration, const KDL::Frame &desired_end_effector_frame, KDL::JntArray &final_configuration)
+{
+    return iksolver->CartToJnt(cur_configuration, desired_end_effector_frame, final_configuration);
+}
+
+int Arm::calculateInverseDynamics(const KDL::JntArray &pos, const KDL::JntArray &vel, const KDL::JntArray &accel, const KDL::Wrenches &f_external, KDL::JntArray &required_torque)
+{
+    return idsolver->CartToJnt(pos, vel, accel, f_external, required_torque);
+}
+
+const KDL::Chain &Arm::getArm() const
+{
+    return arm;
+}
+
+void Arm::print() const
+{
+    for (const KDL::Segment &seg : arm.segments)
+    {
+        cout << seg.getName() << endl;
+        cout << seg.getFrameToTip() << endl;
+        cout << seg.getJoint().getTypeName() << endl;
+        cout << "Inertial frame:" << endl;
+        cout << "\tmass: " << seg.getInertia().getMass() << endl;
+        cout << '\t' << seg.getInertia().getCOG() << endl;
+        cout << endl;
+    }
+}
--- a/rrrobot_src/src/arm_control/src/arm_controller.cpp
+++ b/rrrobot_src/src/arm_control/src/arm_controller.cpp
@@ -0,0 +1,128 @@
+#include <arm.h>
+#include <iostream>
+
+#include <geometry_msgs/Pose.h>
+#include <std_msgs/Float64.h>
+
+#include <kdl/chain.hpp>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/frames_io.hpp>
+#include <stdio.h>
+#include <vector>
+
+#include "ros/ros.h"
+#include <simulation_env/arm_command.h>
+#include <simulation_env/arm_angles.h>
+#include <simulation_env/arm_command.h>
+
+using std::cout;
+using std::endl;
+using std::vector;
+using namespace KDL;
+
+Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
+KDL::Chain chain = arm.getArm();
+KDL::JntArray pos(arm.getArm().getNrOfJoints());
+
+ros::Publisher shoulder_pivot_publisher;
+ros::Publisher shoulder_joint_publisher;
+ros::Publisher elbow_joint_publisher;
+ros::Publisher wrist_pivot_publisher;
+ros::Publisher wrist_joint_publisher;
+ros::Publisher end_effector_pivot_publisher;
+
+void shoulder_pivot_callback(const std_msgs::Float64 &cmd)
+{
+    pos(0) = cmd.data;
+}
+
+void shoulder_joint_callback(const std_msgs::Float64 &cmd)
+{
+    pos(1) = cmd.data;
+}
+
+void elbow_joint_callback(const std_msgs::Float64 &cmd)
+{
+    pos(2) = cmd.data;
+}
+
+void wrist_pivot_callback(const std_msgs::Float64 &cmd)
+{
+    pos(3) = cmd.data;
+}
+
+void wrist_joint_callback(const std_msgs::Float64 &cmd)
+{
+    pos(4) = cmd.data;
+}
+
+void end_effector_pivot_callback(const std_msgs::Float64 &cmd)
+{
+    pos(5) = cmd.data;
+}
+
+void fixAngles(const KDL::JntArray &current_angles, KDL::JntArray &desired_angles)
+{
+    // TODO: go clockwise or counter-clockwise depending on which is shorter
+}
+
+void endEffectorPoseTargetCallback(const geometry_msgs::Pose &msg)
+{
+    KDL::JntArray required_angles(arm.getArm().getNrOfJoints());
+    KDL::SetToZero(required_angles);
+
+    KDL::Frame desired(KDL::Rotation::Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w),
+                       KDL::Vector(msg.position.x, msg.position.y, msg.position.z));
+
+    arm.calculateInverseKinematics(pos, desired, required_angles);
+
+    fixAngles(pos, required_angles);
+
+    std_msgs::Float64 shoulder_pivot_setpoint;
+    shoulder_pivot_setpoint.data = required_angles(0);
+    std_msgs::Float64 shoulder_joint_setpoint;
+    shoulder_joint_setpoint.data = required_angles(1);
+    std_msgs::Float64 elbow_joint_setpoint;
+    elbow_joint_setpoint.data = required_angles(2);
+    std_msgs::Float64 wrist_pivot_setpoint;
+    wrist_pivot_setpoint.data = required_angles(3);
+    std_msgs::Float64 wrist_joint_setpoint;
+    wrist_joint_setpoint.data = required_angles(4);
+    std_msgs::Float64 end_effector_setpoint;
+    end_effector_setpoint.data = required_angles(5);
+
+    shoulder_pivot_publisher.publish(shoulder_pivot_setpoint);
+    shoulder_joint_publisher.publish(shoulder_joint_setpoint);
+    elbow_joint_publisher.publish(elbow_joint_setpoint);
+    wrist_pivot_publisher.publish(wrist_pivot_setpoint);
+    wrist_joint_publisher.publish(wrist_joint_setpoint);
+    end_effector_pivot_publisher.publish(end_effector_setpoint);
+}
+
+int main(int argc, char **argv)
+{
+    cout << "Starting arm controller..." << endl;
+    ros::init(argc, argv, "arm_control_node");
+
+    ros::NodeHandle nh;
+    ros::Subscriber shoulder_pivot_sub = nh.subscribe("/arm_node/arm_positions/shoulder_pivot_angle", 1000, &shoulder_pivot_callback);
+    ros::Subscriber shoulder_joint_sub = nh.subscribe("/arm_node/arm_positions/shoulder_joint_angle", 1000, &shoulder_joint_callback);
+    ros::Subscriber elbow_joint_sub = nh.subscribe("/arm_node/arm_positions/elbow_joint_angle", 1000, &elbow_joint_callback);
+    ros::Subscriber wrist_pivot_sub = nh.subscribe("/arm_node/arm_positions/wrist_pivot_angle", 1000, &wrist_pivot_callback);
+    ros::Subscriber wrist_joint_sub = nh.subscribe("/arm_node/arm_positions/wrist_joint_angle", 1000, &wrist_joint_callback);
+    ros::Subscriber end_effector_sub = nh.subscribe("/arm_node/arm_positions/end_effector_pivot_angle", 1000, &end_effector_pivot_callback);
+
+    ros::Subscriber end_effector_pose_setpoint_sub = nh.subscribe("/arm_node/arm_pose_setpoint", 1000, &endEffectorPoseTargetCallback);
+
+    KDL::SetToZero(pos);
+
+    shoulder_pivot_publisher = nh.advertise<std_msgs::Float64>("/arm_node/shoulder_pivot_setpoint", 1000);
+    shoulder_joint_publisher = nh.advertise<std_msgs::Float64>("/arm_node/shoulder_joint_setpoint", 1000);
+    elbow_joint_publisher = nh.advertise<std_msgs::Float64>("/arm_node/elbow_joint_setpoint", 1000);
+    wrist_pivot_publisher = nh.advertise<std_msgs::Float64>("/arm_node/wrist_pivot_setpoint", 1000);
+    wrist_joint_publisher = nh.advertise<std_msgs::Float64>("/arm_node/wrist_joint_setpoint", 1000);
+    end_effector_pivot_publisher = nh.advertise<std_msgs::Float64>("/arm_node/end_effector_pivot_setpoint", 1000);
+
+    ros::spin();
+}
--- a/rrrobot_src/src/arm_control/test/angles_subscriber.cpp
+++ b/rrrobot_src/src/arm_control/test/angles_subscriber.cpp
@@ -0,0 +1,153 @@
+#include <std_msgs/Float64.h>
+#include "ros/ros.h"
+#include <ros/rate.h>
+
+#include <arm.h>
+
+#include <iostream>
+#include <vector>
+#include <cmath>
+
+using std::cout;
+using std::endl;
+using std::vector;
+
+std_msgs::Float64 shoulder_pivot_angle;
+std_msgs::Float64 shoulder_joint_angle;
+std_msgs::Float64 elbow_joint_angle;
+std_msgs::Float64 wrist_pivot_angle;
+std_msgs::Float64 wrist_joint_angle;
+std_msgs::Float64 end_effector_pivot_angle;
+Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
+KDL::JntArray pos(arm.getArm().getNrOfJoints());
+char received_angles = 0;
+
+void shoulder_pivot_callback(const std_msgs::Float64 &cmd)
+{
+    // update the torque applied to each joint when a message is received
+    shoulder_pivot_angle = cmd;
+    pos(0) = cmd.data;
+    cout << "shoulder pivot: " << shoulder_pivot_angle.data << endl;
+    received_angles |= 0x01;
+}
+
+void shoulder_joint_callback(const std_msgs::Float64 &cmd)
+{
+    // update the torque applied to each joint when a message is received
+    shoulder_joint_angle = cmd;
+    pos(1) = cmd.data;
+    cout << "shoulder joint: " << shoulder_joint_angle.data << endl;
+    received_angles |= 0x02;
+}
+
+void elbow_joint_callback(const std_msgs::Float64 &cmd)
+{
+    // update the torque applied to each joint when a message is received
+    elbow_joint_angle = cmd;
+    pos(2) = cmd.data;
+    cout << "elbow joint: " << elbow_joint_angle.data << endl;
+    received_angles |= 0x04;
+}
+
+void wrist_pivot_callback(const std_msgs::Float64 &cmd)
+{
+    // update the torque applied to each joint when a message is received
+    wrist_pivot_angle = cmd;
+    pos(3) = cmd.data;
+    cout << "wrist pivot: " << wrist_pivot_angle.data << endl;
+    received_angles |= 0x08;
+}
+
+void wrist_joint_callback(const std_msgs::Float64 &cmd)
+{
+    // update the torque applied to each joint when a message is received
+    wrist_joint_angle = cmd;
+    pos(4) = cmd.data;
+    cout << "wrist joint: " << wrist_joint_angle.data << endl;
+    received_angles |= 0x10;
+}
+
+void end_effector_pivot_callback(const std_msgs::Float64 &cmd)
+{
+    // update the torque applied to each joint when a message is received
+    end_effector_pivot_angle = cmd;
+    pos(5) = cmd.data;
+    cout << "end effector pivot: " << end_effector_pivot_angle.data << endl;
+    received_angles |= 0x20;
+}
+
+void fix_angle(double cur_pos, double &desired)
+{
+    while (desired > M_PI)
+    {
+        desired -= 2 * M_PI;
+    }
+    while (desired < -M_PI)
+    {
+        desired += 2 * M_PI;
+    }
+
+    double cur_pos_truncated = cur_pos; // % (M_PI);
+    while (cur_pos_truncated > M_PI)
+    {
+        cur_pos_truncated -= 2 * M_PI;
+    }
+    while (cur_pos_truncated < -M_PI)
+    {
+        cur_pos_truncated += 2 * M_PI;
+    }
+    // say desired = 1.5708
+    // 4 cases:
+    //  1. cur_pos = 0.2 --> set target as cur_pos + 1.3708
+    //  2. cur_pos = -0.2 --> set target as cur_pos + 1.7708
+    //  3. cur_pos = -2.5 --> set target as cur_pos + 2.21239
+    //  4. cur_pos = 2.5 --> set target as cur_pos + 0.9292
+
+    if ((cur_pos_truncated - desired) < M_PI && (cur_pos_truncated - desired) > 0)
+    {
+        desired = cur_pos - (cur_pos_truncated - desired);
+    }
+    else if ((desired - cur_pos_truncated) < M_PI && (desired - cur_pos_truncated) > 0)
+    {
+        desired = cur_pos + (desired - cur_pos_truncated);
+    }
+    else if ((cur_pos_truncated - desired) > -M_PI && (cur_pos_truncated - desired) < 0)
+    {
+        desired = cur_pos - (cur_pos_truncated - desired);
+    }
+    else if ((desired - cur_pos_truncated) > -M_PI && (desired - cur_pos_truncated) < 0)
+    {
+        desired = cur_pos + (desired - cur_pos_truncated);
+    }
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "arm_control_demo");
+
+    ros::Subscriber shoulder_pivot_sub;
+    ros::Subscriber shoulder_joint_sub;
+    ros::Subscriber elbow_joint_sub;
+    ros::Subscriber wrist_pivot_sub;
+    ros::Subscriber wrist_joint_sub;
+    ros::Subscriber end_effector_sub;
+
+    ros::NodeHandle nh;
+    ros::Rate pub_rate(0.25); // publish every 10 seconds
+    ros::Rate quick(1);
+
+    shoulder_pivot_sub = nh.subscribe("/arm_node/arm_positions/shoulder_pivot_angle", 1000, &shoulder_pivot_callback);
+    shoulder_joint_sub = nh.subscribe("/arm_node/arm_positions/shoulder_joint_angle", 1000, &shoulder_joint_callback);
+    elbow_joint_sub = nh.subscribe("/arm_node/arm_positions/elbow_joint_angle", 1000, &elbow_joint_callback);
+    wrist_pivot_sub = nh.subscribe("/arm_node/arm_positions/wrist_pivot_angle", 1000, &wrist_pivot_callback);
+    wrist_joint_sub = nh.subscribe("/arm_node/arm_positions/wrist_joint_angle", 1000, &wrist_joint_callback);
+    end_effector_sub = nh.subscribe("/arm_node/arm_positions/end_effector_pivot_angle", 1000, &end_effector_pivot_callback);
+
+    ros::spin();
+    // while (ros::ok())
+    // {
+    //     quick.sleep();
+
+    //     ros::spinOnce();
+    // }
+}
--- a/rrrobot_src/src/arm_control/test/arm_movement_demo.cpp
+++ b/rrrobot_src/src/arm_control/test/arm_movement_demo.cpp
@@ -0,0 +1,47 @@
+#include <geometry_msgs/Pose.h>
+#include "ros/ros.h"
+#include <ros/rate.h>
+
+#include <arm.h>
+
+#include <iostream>
+#include <vector>
+#include <cmath>
+
+using std::cout;
+using std::endl;
+using std::vector;
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "arm_control_demo");
+
+    ros::NodeHandle nh;
+    ros::Rate pub_rate(0.1); // publish every 10 seconds
+    ros::Publisher end_effector_setpoint_publisher = nh.advertise<geometry_msgs::Pose>("/arm_node/arm_pose_setpoint", 1000);
+
+    int state = 0;
+
+    vector<KDL::Frame> desired_positions;
+    desired_positions.push_back(KDL::Frame(KDL::Rotation::RPY(-2.35619, 0.7853, 0.0), KDL::Vector(1, 1, 1)));
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, -1, 1)));
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, -1, 2)));
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, 1, 3)));
+
+    while (ros::ok())
+    {
+        const KDL::Frame &cur_setpoint = desired_positions[state];
+        geometry_msgs::Pose msg;
+        msg.position.x = cur_setpoint.p.x();
+        msg.position.y = cur_setpoint.p.y();
+        msg.position.z = cur_setpoint.p.z();
+        cur_setpoint.M.GetQuaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w);
+
+        end_effector_setpoint_publisher.publish(msg);
+        ros::spinOnce();
+
+        state = (state + 1) % desired_positions.size();
+
+        pub_rate.sleep();
+    }
+}
--- a/rrrobot_src/src/arm_control/test/test.cpp
+++ b/rrrobot_src/src/arm_control/test/test.cpp
@@ -0,0 +1,206 @@
+// Copyright  (C)  2007  Francois Cauwe <francois at cauwe dot org>
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+#include <kdl/chain.hpp>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/frames_io.hpp>
+#include <stdio.h>
+#include <iostream>
+#include <arm.h>
+#include <fstream>
+#include <string>
+#include <vector>
+
+using namespace KDL;
+using std::cin;
+using std::cout;
+using std::endl;
+using std::ofstream;
+using std::string;
+using std::vector;
+
+int main(int argc, char **argv)
+{
+    //Definition of a kinematic chain & add segments to the chain
+    Arm arm(/*"/home/rrrobot/rrrobot_src/src/gazebo_models/basic_arm/model.sdf"); /(*/ "/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
+    //KDL::Chain chain = arm.getArm();
+
+    // chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.0))));
+    // chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1))));
+    // chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1))));
+    // chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.480))));
+    // chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.645))));
+    // chain.addSegment(Segment(Joint(Joint::RotZ)));
+    // chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.120))));
+    // chain.addSegment(Segment(Joint(Joint::RotZ)));
+
+    // Create solver based on kinematic chain
+    //ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);
+
+    // Create joint array
+    unsigned int nj = arm.getArm().getNrOfJoints();
+    KDL::JntArray jointpositions = JntArray(nj);
+    for (int pos = 0; pos < nj; ++pos)
+    {
+        jointpositions(pos) = 0;
+    }
+
+    // // Assign some values to the joint positions for (unsigned int i = 0; i < nj; i++)
+    // for (int i = 0; i < nj; ++i)
+    // {
+    //     float myinput;
+    //     printf("Enter the position of joint %i: ", i);
+    //     scanf("%e", &myinput);
+    //     jointpositions(i) = (double)myinput;
+    // }
+    // // Create the frame that will contain the results
+    // KDL::Frame cartpos;
+
+    // // Calculate forward position kinematics
+    // bool kinematics_status;
+    // kinematics_status = fksolver.JntToCart(jointpositions, cartpos);
+    // if (kinematics_status >= 0)
+    // {
+    //     // cout << segments << endl;
+    //     std::cout << cartpos << std::endl;
+    //     // printf("%s \n", "Succes, thanks KDL!");
+    // }
+    // else
+    // {
+    //     // printf("%s \n", "Error: could not calculate forward kinematics :(");
+    // }
+    // while (1)
+    // {
+    //     cout << "Enter the shoulder_pivot angle: ";
+    //     cin >> jointpositions(0);
+    //     cout << "Enter the shoulder_joint angle: ";
+    //     cin >> jointpositions(1);
+    //     cout << "Enter the elbow_joint angle: ";
+    //     cin >> jointpositions(2);
+    //     cout << "Enter the wrist_pivot angle: ";
+    //     cin >> jointpositions(3);
+    //     cout << "Enter the wrist_joint angle: ";
+    //     cin >> jointpositions(4);
+    //     cout << "Enter the end_effector_pivot angle: ";
+    //     cin >> jointpositions(5);
+
+    // Create the frame that will contain the results
+    KDL::Frame cartpos;
+    ofstream f_raw("configurations.txt");
+    ofstream f_corrected("corrected_configurations.txt");
+    int which_joint;
+    cout << "Which joint should be moved (0: shoulder pivot, 5: end effector pivot): ";
+    cin >> which_joint;
+
+    for (double shoulder_pivot = 0.0; shoulder_pivot <= 6.28; shoulder_pivot += 0.1)
+    {
+        // Calculate forward position kinematics
+        bool kinematics_status;
+        // for (int segments = 0; segments <= chain.getNrOfSegments(); ++segments)
+        // {
+        //     kinematics_status = fksolver.JntToCart(jointpositions, cartpos, segments);
+        //     if (kinematics_status >= 0)
+        //     {
+        //         cout << segments << endl;
+        //         std::cout << cartpos << std::endl;
+        //         // printf("%s \n", "Succes, thanks KDL!");
+        //     }
+        //     else
+        //     {
+        //         // printf("%s \n", "Error: could not calculate forward kinematics :(");
+        //     }
+        // }
+        jointpositions(which_joint) = shoulder_pivot;
+        kinematics_status = arm.calculateForwardKinematics(jointpositions, cartpos); //fksolver.JntToCart(jointpositions, cartpos);
+        cout << "Final position: " << endl;
+        cout << cartpos << endl;
+
+        KDL::Vector pos = cartpos.p;
+        f_raw << pos.x() << "," << pos.y() << "," << pos.z() << "\n";
+
+        KDL::Vector corrected = cartpos.M * cartpos.p;
+        f_corrected << corrected.x() << "," << corrected.y() << "," << corrected.z() << "\n";
+    }
+    // }
+
+    KDL::Frame desired(KDL::Vector(0.000767, -1.87014, 2.0658));
+    KDL::JntArray final_config(arm.getArm().getNrOfJoints());
+    bool error = arm.calculateInverseKinematics(jointpositions, desired, final_config);
+    if (error == false)
+    {
+        for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint)
+        {
+            cout << final_config(joint) << endl;
+        }
+    }
+    else
+    {
+        cout << "Failed to find configuration" << endl;
+    }
+
+    KDL::JntArray vel(arm.getArm().getNrOfJoints());
+    KDL::JntArray accel(arm.getArm().getNrOfJoints());
+    KDL::Wrenches ext_force(arm.getArm().getNrOfSegments());
+    KDL::JntArray required_force(arm.getArm().getNrOfJoints());
+    int error_val = arm.calculateInverseDynamics(jointpositions, vel, accel, ext_force, required_force);
+    if (error_val == 0)
+    {
+        for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint)
+        {
+            cout << required_force(joint) << endl;
+        }
+    }
+    else
+    {
+        cout << "Failed to find required torques, error #" << error_val << endl;
+    }
+
+    // record required torques for a variety of joint positions
+    KDL::JntArray pos(arm.getArm().getNrOfJoints());
+    for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint)
+    {
+        pos(joint) = 0;
+    }
+
+    ofstream f("required_" + std::to_string(which_joint) + "_torque.txt");
+    for (double angle = -3.14159; angle < 3.14159; angle += 0.1)
+    {
+        pos(which_joint) = angle;
+        arm.calculateInverseDynamics(pos, vel, accel, ext_force, required_force);
+
+        f << angle << "," << required_force(0) << "," << required_force(1) << "," << required_force(2) << "," << required_force(3) << "," << required_force(4) << "," << required_force(5) << "\n";
+    }
+
+    // KDL::JntArray pos(arm.getArm().getNrOfJoints());
+    KDL::JntArray required_angles(arm.getArm().getNrOfJoints());
+
+    KDL::SetToZero(pos);
+    KDL::SetToZero(required_angles);
+
+    vector<KDL::Frame> desired_positions;
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.5, 1)));
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.4, 1)));
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.3, 1)));
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.2, 1)));
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.1, 1)));
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.0, 1)));
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.1, 1)));
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.2, 1)));
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.3, 1)));
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.4, 1)));
+    desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.5, 1)));
+
+    ofstream ik_f("inverse_kinematics.txt");
+    for (int state = 0; state < desired_positions.size(); ++state)
+    {
+        arm.calculateInverseKinematics(pos, desired_positions[state], required_angles);
+
+        ik_f << desired_positions[state].p.x() << "," << desired_positions[state].p.y() << "," << desired_positions[state].p.z() << ",";
+        ik_f << required_angles(0) << "," << required_angles(1) << "," << required_angles(2) << "," << required_angles(3) << "," << required_angles(4) << "," << required_angles(5) << "\n";
+    }
+}
--- a/rrrobot_src/src/arm_control/test/test_internal_arm_representation.cpp
+++ b/rrrobot_src/src/arm_control/test/test_internal_arm_representation.cpp
@@ -0,0 +1,128 @@
+#include <arm.h>
+
+#include "gtest/gtest.h"
+#include <kdl/frames_io.hpp>
+#include <kdl/chain.hpp>
+#include <cmath>
+
+const double ALLOWED_ERROR = 0.05;
+
+double getDistance(const KDL::Vector &a, const KDL::Vector &b)
+{
+    return sqrt(pow(a.x() - b.x(), 2) + pow(a.y() - b.y(), 2) + pow(a.z() - b.z(), 2));
+}
+
+void EXPECT_ROTATION_EQ(const KDL::Rotation &a, const KDL::Rotation &b)
+{
+    EXPECT_TRUE(KDL::Equal(a, b, 0.005));
+}
+
+TEST(ArmRepresentationTests, shoulder_pivot)
+{
+    Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
+
+    // Create joint array
+    unsigned int nj = arm.getArm().getNrOfJoints();
+    KDL::JntArray jointpositions = KDL::JntArray(nj);
+    for (int pos = 0; pos < nj; ++pos)
+    {
+        jointpositions(pos) = 0;
+    }
+
+    // Create the frame that will contain the results
+    KDL::Frame cartpos;
+    KDL::Vector correct(0.000767, -1.87014, 2.0658);
+
+    jointpositions(0) = 0.0;
+    arm.calculateForwardKinematics(jointpositions, cartpos);
+
+    EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
+
+    jointpositions(0) = 2 * M_PI;
+    arm.calculateForwardKinematics(jointpositions, cartpos);
+    EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
+
+    jointpositions(0) = M_PI;
+    arm.calculateForwardKinematics(jointpositions, cartpos);
+    correct = KDL::Vector(0.000767, 1.87014, 2.0658);
+    EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
+}
+
+TEST(ArmRepresentationTests, elbow_pivot)
+{
+    Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
+
+    // Create joint array
+    unsigned int nj = arm.getArm().getNrOfJoints();
+    KDL::JntArray jointpositions = KDL::JntArray(nj);
+    for (int pos = 0; pos < nj; ++pos)
+    {
+        jointpositions(pos) = 0;
+    }
+
+    // Create the frame that will contain the results
+    KDL::Frame cartpos;
+    KDL::Vector correct(0.000767, -1.87014, 2.0658);
+
+    for (double pos = 0.0; pos <= M_2_PI; pos += 0.1)
+    {
+        jointpositions(3) = pos;
+        arm.calculateForwardKinematics(jointpositions, cartpos);
+        EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
+    }
+}
+
+TEST(ArmRepresentationTests, end_effector_pivot)
+{
+    Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
+
+    // Create joint array
+    unsigned int nj = arm.getArm().getNrOfJoints();
+    KDL::JntArray jointpositions = KDL::JntArray(nj);
+    for (int pos = 0; pos < nj; ++pos)
+    {
+        jointpositions(pos) = 0;
+    }
+
+    // Create the frame that will contain the results
+    KDL::Frame cartpos;
+    KDL::Vector correct(0.000767, -1.87014, 2.0658);
+
+    for (double pos = 0.0; pos <= M_2_PI; pos += 0.1)
+    {
+        jointpositions(5) = pos;
+        arm.calculateForwardKinematics(jointpositions, cartpos);
+        EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
+    }
+}
+
+TEST(ArmRepresentationTests, all_joints_rotated)
+{
+    Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
+
+    // Create joint array
+    unsigned int nj = arm.getArm().getNrOfJoints();
+    KDL::JntArray jointpositions = KDL::JntArray(nj);
+
+    jointpositions(0) = -12.0027;
+    jointpositions(1) = -1.86177;
+    jointpositions(2) = -0.991728;
+    jointpositions(3) = 39.5032;
+    jointpositions(4) = -1.45472;
+    jointpositions(5) = -25.4681;
+
+    // Create the frame that will contain the results
+    KDL::Frame cartpos;
+    KDL::Vector correct(-0.876742, 1.85761, 0.482547);
+    KDL::Rotation correct_rot = KDL::Rotation::RPY(-1.75917, -0.0162929, -0.823634);
+
+    arm.calculateForwardKinematics(jointpositions, cartpos);
+    EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
+    EXPECT_ROTATION_EQ(cartpos.M, correct_rot);
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}
--- a/rrrobot_src/src/arm_control/test/test_pub_sub.cpp
+++ b/rrrobot_src/src/arm_control/test/test_pub_sub.cpp
@@ -0,0 +1,97 @@
+#include "ros/ros.h"
+#include "std_msgs/Float64.h"
+
+#include <iostream>
+
+using std::cout;
+using std::endl;
+
+void msg_callback(const std_msgs::Float64 &cmd)
+{
+    // update the torque applied to each joint when a message is received
+    cout << "received: " << cmd.data << endl;
+}
+
+/**
+ * This tutorial demonstrates simple sending of messages over the ROS system.
+ */
+int main(int argc, char **argv)
+{
+    /**
+     * The ros::init() function needs to see argc and argv so that it can perform
+     * any ROS arguments and name remapping that were provided at the command line.
+     * For programmatic remappings you can use a different version of init() which takes
+     * remappings directly, but for most command-line programs, passing argc and argv is
+     * the easiest way to do it.  The third argument to init() is the name of the node.
+     *
+     * You must call one of the versions of ros::init() before using any other
+     * part of the ROS system.
+     */
+    ros::init(argc, argv, "talker");
+
+    /**
+     * NodeHandle is the main access point to communications with the ROS system.
+     * The first NodeHandle constructed will fully initialize this node, and the last
+     * NodeHandle destructed will close down the node.
+     */
+    ros::NodeHandle n;
+    /**
+     * The advertise() function is how you tell ROS that you want to
+     * publish on a given topic name. This invokes a call to the ROS
+     * master node, which keeps a registry of who is publishing and who
+     * is subscribing. After this advertise() call is made, the master
+     * node will notify anyone who is trying to subscribe to this topic name,
+     * and they will in turn negotiate a peer-to-peer connection with this
+     * node.  advertise() returns a Publisher object which allows you to
+     * publish messages on that topic through a call to publish().  Once
+     * all copies of the returned Publisher object are destroyed, the topic
+     * will be automatically unadvertised.
+     *
+     * The second parameter to advertise() is the size of the message queue
+     * used for publishing messages.  If messages are published more quickly
+     * than we can send them, the number here specifies how many messages to
+     * buffer up before throwing some away.
+     */
+    ros::Publisher pub = n.advertise<std_msgs::Float64>("chatter", 1000);
+    ros::Subscriber sub = n.subscribe("chatter", 1000, &msg_callback);
+
+    ros::Rate loop_rate(10);
+
+    /**
+     * A count of how many messages we have sent. This is used to create
+     * a unique string for each message.
+     */
+    int count = 0;
+    double data = -5e-5; //.1254987465413251e-5;
+    while (ros::ok())
+    {
+        /**
+         * This is a message object. You stuff it with data, and then publish it.
+         */
+        std_msgs::Float64 msg;
+        msg.data = data;
+
+        cout << "Publishing: " << data << '\t' << msg.data << endl;
+
+        /**
+         * The publish() function is how you send messages. The parameter
+         * is the message object. The type of this object must agree with the type
+         * given as a template parameter to the advertise<>() call, as was done
+         * in the constructor above.
+         */
+        pub.publish(msg);
+
+        ros::spinOnce();
+
+        loop_rate.sleep();
+        ++count;
+
+        data += 1e-5;
+        if (data > 5.0)
+        {
+            data = -5.1254987465413251;
+        }
+    }
+
+    return 0;
+}
--- a/rrrobot_src/src/gazebo_models/unit_box/model.config
+++ b/rrrobot_src/src/gazebo_models/unit_box/model.config
@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>unit_box</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>
--- a/rrrobot_src/src/gazebo_models/unit_box/model.sdf
+++ b/rrrobot_src/src/gazebo_models/unit_box/model.sdf
@@ -0,0 +1,104 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='unit_box'>
+    <link name='link'>
+      <inertial>
+        <mass>0.208095</mass>
+        <inertia>
+          <ixx>0.0180922</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.0180922</iyy>
+          <iyz>0</iyz>
+          <izz>0.0346825</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <self_collide>0</self_collide>
+      <enable_wind>0</enable_wind>
+      <kinematic>0</kinematic>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <gravity>1</gravity>
+      <visual name='visual'>
+        <geometry>
+          <box>
+            <size>0.05 0.05 0.05</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/Grey</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>1 0 0 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.05 0.05 0.05</size>
+          </box>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <static>0</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>
--- a/rrrobot_src/test.sh
+++ b/rrrobot_src/test.sh
@@ -0,0 +1,2 @@
+source build.sh
+catkin_make run_tests
--- a/rrrobot_src/world/rrrobot_setpoint.world
+++ b/rrrobot_src/world/rrrobot_setpoint.world
@@ -0,0 +1,68 @@
+<sdf version='1.6'>
+  <world name='default'>
+    <model name='ground_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <contact>
+              <collide_bitmask>65535</collide_bitmask>
+              <ode/>
+            </contact>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+    </model>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose frame=''>0 0 10 0 -0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+    <include>
+      <uri>model://fanuc_robotic_arm_with_gripper</uri>
+    </include>
+  </world>
+</sdf>
--- a/rrrobot_ws/build.sh
+++ b/rrrobot_ws/build.sh
@@ -5,3 +5,4 @@ catkin_make &&
 catkin_make install &&

 source devel/setup.bash
+export GAZEBO_RESOURCE_PATH=$(pwd)/world:$GAZEBO_RESOURCE_PATH
--- a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf
+++ b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf
@@ -17,7 +17,7 @@
      </inertial>
      <self_collide>1</self_collide>
      <kinematic>0</kinematic>
-      <gravity>1</gravity>
+      <gravity>0</gravity>
      <visual name='visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
@@ -116,7 +116,7 @@
      </inertial>
      <self_collide>1</self_collide>
      <kinematic>0</kinematic>
-      <gravity>1</gravity>
+      <gravity>0</gravity>
      <visual name='visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
@@ -215,7 +215,7 @@
      </inertial>
      <self_collide>1</self_collide>
      <kinematic>0</kinematic>
-      <gravity>1</gravity>
+      <gravity>0</gravity>
      <visual name='visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
@@ -314,7 +314,7 @@
      </inertial>
      <self_collide>1</self_collide>
      <kinematic>0</kinematic>
-      <gravity>1</gravity>
+      <gravity>0</gravity>
      <visual name='visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
@@ -413,7 +413,7 @@
      </inertial>
      <self_collide>1</self_collide>
      <kinematic>0</kinematic>
-      <gravity>1</gravity>
+      <gravity>0</gravity>
      <visual name='visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
@@ -512,7 +512,7 @@
      </inertial>
      <self_collide>1</self_collide>
      <kinematic>0</kinematic>
-      <gravity>1</gravity>
+      <gravity>0</gravity>
      <visual name='visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
@@ -611,7 +611,7 @@
      </inertial>
      <self_collide>1</self_collide>
      <kinematic>0</kinematic>
-      <gravity>1</gravity>
+      <gravity>0</gravity>
      <visual name='visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
@@ -704,7 +704,7 @@
        <limit>
          <lower>-1.79769e+308</lower>
          <upper>1.79769e+308</upper>
-          <effort>-2000</effort>
+          <effort>-20000000</effort>
          <velocity>-1</velocity>
        </limit>
        <dynamics>
@@ -737,7 +737,7 @@
        <limit>
          <lower>-1.79769e+308</lower>
          <upper>1.79769e+308</upper>
-          <effort>-2000</effort>
+          <effort>-20000000</effort>
          <velocity>-1</velocity>
        </limit>
        <dynamics>
@@ -770,7 +770,7 @@
        <limit>
          <lower>-1.79769e+308</lower>
          <upper>1.79769e+308</upper>
-          <effort>-2000</effort>
+          <effort>-20000000</effort>
          <velocity>-1</velocity>
        </limit>
        <dynamics>
@@ -803,7 +803,7 @@
        <limit>
          <lower>-1.79769e+308</lower>
          <upper>1.79769e+308</upper>
-          <effort>-2000</effort>
+          <effort>-20000000</effort>
          <velocity>-1</velocity>
        </limit>
        <dynamics>
@@ -836,7 +836,7 @@
        <limit>
          <lower>-1.79769e+308</lower>
          <upper>1.79769e+308</upper>
-          <effort>-2000</effort>
+          <effort>-20000000</effort>
          <velocity>-1</velocity>
        </limit>
        <dynamics>
@@ -869,7 +869,7 @@
        <limit>
          <lower>-1.79769e+308</lower>
          <upper>1.79769e+308</upper>
-          <effort>-2000</effort>
+          <effort>-20000000</effort>
          <velocity>-1</velocity>
        </limit>
        <dynamics>
--- a/rrrobot_ws/src/simulation_env/CMakeLists.txt
+++ b/rrrobot_ws/src/simulation_env/CMakeLists.txt
@@ -137,7 +137,8 @@ add_library(arm_motors SHARED
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(arm_angles ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(arm_motors ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
@@ -191,7 +192,7 @@ target_link_libraries(test_move_arm

 ## Mark libraries for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-install(TARGETS arm_angles
+install(TARGETS arm_angles arm_motors
 #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION /home/rrrobot/rrrobot_src/lib/
 #   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
--- a/rrrobot_ws/src/simulation_env/msg/arm_angles.msg
+++ b/rrrobot_ws/src/simulation_env/msg/arm_angles.msg
@@ -4,3 +4,10 @@ float32 elbow_joint_angle
 float32 wrist_pivot_angle
 float32 wrist_joint_angle
 float32 end_effector_pivot_angle
+
+float32 shoulder_pivot_velocity
+float32 shoulder_joint_velocity
+float32 elbow_joint_velocity
+float32 wrist_pivot_velocity
+float32 wrist_joint_velocity
+float32 end_effector_pivot_velocity
--- a/rrrobot_ws/src/simulation_env/src/arm_angle_sensor_plugin.cpp
+++ b/rrrobot_ws/src/simulation_env/src/arm_angle_sensor_plugin.cpp
@@ -2,9 +2,11 @@
 #include <gazebo/physics/physics.hh>
 #include <memory>
 #include <iostream>
+#include <cmath>

 #include "ros/ros.h"
 #include "simulation_env/arm_angles.h"
+#include <std_msgs/Float64.h>

 using std::cout;
 using std::endl;
@@ -29,7 +31,33 @@ public:
        }

        nh.reset(new ros::NodeHandle("arm_node"));
-        publisher = nh->advertise<simulation_env::arm_angles>("arm_positions", 1);
+        // publisher = nh->advertise<simulation_env::arm_angles>("arm_positions", 1000);
+
+        shoulder_pivot_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/shoulder_pivot_angle", 1);
+        shoulder_joint_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/shoulder_joint_angle", 1);
+        elbow_joint_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/elbow_joint_angle", 1);
+        wrist_pivot_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/wrist_pivot_angle", 1);
+        wrist_joint_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/wrist_joint_angle", 1);
+        end_effector_pivot_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/end_effector_pivot_angle", 1);
+
+        std_msgs::Float64 shoulder_pivot_angle;
+        shoulder_pivot_angle.data = model->GetJoint("shoulder_pivot")->Position(); //msg.shoulder_pivot_angle;
+        std_msgs::Float64 shoulder_joint_angle;
+        shoulder_joint_angle.data = model->GetJoint("shoulder_joint")->Position(); //msg.shoulder_joint_angle;
+        std_msgs::Float64 elbow_joint_angle;
+        elbow_joint_angle.data = model->GetJoint("elbow_joint")->Position(); //msg.elbow_joint_angle;
+        std_msgs::Float64 wrist_pivot_angle;
+        wrist_pivot_angle.data = model->GetJoint("wrist_pivot")->Position(); //msg.wrist_pivot_angle;
+        std_msgs::Float64 wrist_joint_angle;
+        wrist_joint_angle.data = model->GetJoint("wrist_joint")->Position(); //msg.wrist_joint_angle;
+        std_msgs::Float64 end_effector_pivot_angle;
+        end_effector_pivot_angle.data = model->GetJoint("end_effector_pivot")->Position(); //msg.end_effector_pivot_angle;
+        // cout << "shoulder_pivot: " << shoulder_pivot_angle.data << endl;
+        // cout << "shoulder_joint: " << shoulder_joint_angle.data << endl;
+        // cout << "elbow_joint: " << elbow_joint_angle.data << endl;
+        // cout << "wrist_pivot: " << wrist_pivot_angle.data << endl;
+        // cout << "wrist_joint: " << wrist_joint_angle.data << endl;
+        // cout << "end_effector_pivot: " << end_effector_pivot_angle.data << endl;
    }

 private:
@@ -37,16 +65,53 @@ private:
    {
        simulation_env::arm_angles msg;

-        // read in the joint angle for each joint in the arm
-        msg.shoulder_pivot_angle = model->GetJoint("shoulder_pivot")->Position();
-        msg.shoulder_joint_angle = model->GetJoint("shoulder_joint")->Position();
-        msg.elbow_joint_angle = model->GetJoint("elbow_joint")->Position();
-        msg.wrist_pivot_angle = model->GetJoint("wrist_pivot")->Position();
-        msg.wrist_joint_angle = model->GetJoint("wrist_joint")->Position();
-        msg.end_effector_pivot_angle = model->GetJoint("end_effector_pivot")->Position();
+        // // read in the joint angle for each joint in the arm
+        // msg.shoulder_pivot_angle = model->GetJoint("shoulder_pivot")->Position();
+        // msg.shoulder_joint_angle = -model->GetJoint("shoulder_joint")->Position();
+        // msg.elbow_joint_angle = -model->GetJoint("elbow_joint")->Position();
+        // msg.wrist_pivot_angle = model->GetJoint("wrist_pivot")->Position();
+        // msg.wrist_joint_angle = model->GetJoint("wrist_joint")->Position() + M_PI_2;
+        // msg.end_effector_pivot_angle = model->GetJoint("end_effector_pivot")->Position();

-        // publish the updated sensor measurements
-        publisher.publish(msg);
+        // // read in the angular velocity for each joint
+        // msg.shoulder_pivot_velocity = model->GetJoint("shoulder_pivot")->GetVelocity(0);
+        // msg.shoulder_joint_velocity = -model->GetJoint("shoulder_joint")->GetVelocity(0);
+        // msg.elbow_joint_velocity = -model->GetJoint("elbow_joint")->GetVelocity(0);
+        // msg.wrist_pivot_velocity = model->GetJoint("wrist_pivot")->GetVelocity(0);
+        // msg.wrist_joint_velocity = model->GetJoint("wrist_joint")->GetVelocity(0);
+        // msg.end_effector_pivot_velocity = model->GetJoint("end_effector_pivot")->GetVelocity(0);
+
+        // // publish the updated sensor measurements
+        // publisher.publish(msg);
+        // ros::spinOnce();
+
+        std_msgs::Float64 shoulder_pivot_angle;
+        shoulder_pivot_angle.data = model->GetJoint("shoulder_pivot")->Position(); //msg.shoulder_pivot_angle;
+        std_msgs::Float64 shoulder_joint_angle;
+        shoulder_joint_angle.data = model->GetJoint("shoulder_joint")->Position(); //msg.shoulder_joint_angle;
+        std_msgs::Float64 elbow_joint_angle;
+        elbow_joint_angle.data = model->GetJoint("elbow_joint")->Position(); //msg.elbow_joint_angle;
+        std_msgs::Float64 wrist_pivot_angle;
+        wrist_pivot_angle.data = model->GetJoint("wrist_pivot")->Position(); //msg.wrist_pivot_angle;
+        std_msgs::Float64 wrist_joint_angle;
+        wrist_joint_angle.data = model->GetJoint("wrist_joint")->Position(); //msg.wrist_joint_angle;
+        std_msgs::Float64 end_effector_pivot_angle;
+        end_effector_pivot_angle.data = model->GetJoint("end_effector_pivot")->Position(); //msg.end_effector_pivot_angle;
+
+        // cout << "Publishing: " << shoulder_pivot_angle.data << '\t' << shoulder_joint_angle.data << '\t' << elbow_joint_angle.data << '\t' << wrist_pivot_angle.data << '\t' << wrist_joint_angle.data << '\t' << end_effector_pivot_angle.data << endl;
+
+        shoulder_pivot_pub.publish(shoulder_pivot_angle);
+        // ros::spinOnce();
+        shoulder_joint_pub.publish(shoulder_joint_angle);
+        // ros::spinOnce();
+        elbow_joint_pub.publish(elbow_joint_angle);
+        // ros::spinOnce();
+        wrist_pivot_pub.publish(wrist_pivot_angle);
+        // ros::spinOnce();
+        wrist_joint_pub.publish(wrist_joint_angle);
+        // ros::spinOnce();
+        end_effector_pivot_pub.publish(end_effector_pivot_angle);
+        // ros::spinOnce();

        // make sure the message gets published
        ros::spinOnce();
@@ -56,6 +121,13 @@ private:
    event::ConnectionPtr updateConnection;
    std::unique_ptr<ros::NodeHandle> nh;
    ros::Publisher publisher;
+
+    ros::Publisher shoulder_pivot_pub;
+    ros::Publisher shoulder_joint_pub;
+    ros::Publisher elbow_joint_pub;
+    ros::Publisher wrist_pivot_pub;
+    ros::Publisher wrist_joint_pub;
+    ros::Publisher end_effector_pivot_pub;
 };
 GZ_REGISTER_MODEL_PLUGIN(JointAngle)
 } // namespace gazebo
--- a/rrrobot_ws/src/simulation_env/src/arm_motors.cpp
+++ b/rrrobot_ws/src/simulation_env/src/arm_motors.cpp
@@ -5,6 +5,7 @@

 #include "ros/ros.h"
 #include "simulation_env/arm_command.h"
+#include <std_msgs/Float64.h>

 using std::cout;
 using std::endl;
@@ -14,15 +15,63 @@ namespace gazebo
 class ArmControl : public ModelPlugin
 {
 public:
+    void shoulder_pivot_callback(const std_msgs::Float64 &cmd)
+    {
+        // update the torque applied to each joint when a message is received
+        model->GetJoint("shoulder_pivot")->SetForce(0, cmd.data);
+
+        ros::spinOnce();
+    }
+
+    void shoulder_joint_callback(const std_msgs::Float64 &cmd)
+    {
+        // update the torque applied to each joint when a message is received
+        model->GetJoint("shoulder_joint")->SetForce(0, cmd.data);
+
+        ros::spinOnce();
+    }
+
+    void elbow_joint_callback(const std_msgs::Float64 &cmd)
+    {
+        // update the torque applied to each joint when a message is received
+        model->GetJoint("elbow_joint")->SetForce(0, cmd.data);
+
+        ros::spinOnce();
+    }
+
+    void wrist_pivot_callback(const std_msgs::Float64 &cmd)
+    {
+        // update the torque applied to each joint when a message is received
+        model->GetJoint("wrist_pivot")->SetForce(0, cmd.data);
+
+        ros::spinOnce();
+    }
+
+    void wrist_joint_callback(const std_msgs::Float64 &cmd)
+    {
+        // update the torque applied to each joint when a message is received
+        model->GetJoint("wrist_joint")->SetForce(0, cmd.data);
+
+        ros::spinOnce();
+    }
+
+    void end_effector_pivot_callback(const std_msgs::Float64 &cmd)
+    {
+        // update the torque applied to each joint when a message is received
+        model->GetJoint("end_effector_pivot")->SetForce(0, cmd.data);
+
+        ros::spinOnce();
+    }
+
    void arm_command_callback(const simulation_env::arm_command &cmd)
    {
        // update the torque applied to each joint when a message is received
-        model->GetJoint("shoulder_pivot")->SetForce(0, cmd.shoulder_pivot_force);
-        model->GetJoint("shoulder_joint")->SetForce(0, cmd.shoulder_joint_force);
-        model->GetJoint("elbow_joint")->SetForce(0, cmd.elbow_joint_force);
-        model->GetJoint("wrist_pivot")->SetForce(0, cmd.wrist_pivot_force);
-        model->GetJoint("wrist_joint")->SetForce(0, cmd.wrist_joint_force);
-        model->GetJoint("end_effector_pivot")->SetForce(0, cmd.end_effector_pivot_force);
+        // model->GetJoint("shoulder_pivot")->SetForce(0, cmd.shoulder_pivot_force);
+        // model->GetJoint("shoulder_joint")->SetForce(0, -cmd.shoulder_joint_force);
+        // model->GetJoint("elbow_joint")->SetForce(0, cmd.elbow_joint_force);
+        // model->GetJoint("wrist_pivot")->SetForce(0, cmd.wrist_pivot_force);
+        // model->GetJoint("wrist_joint")->SetForce(0, cmd.wrist_joint_force);
+        // model->GetJoint("end_effector_pivot")->SetForce(0, cmd.end_effector_pivot_force);

        ros::spinOnce();
    }
@@ -42,12 +91,27 @@ public:

        nh.reset(new ros::NodeHandle("arm_node"));
        subscriber = nh->subscribe("arm_commands", 1000, &ArmControl::arm_command_callback, this);
+        shoulder_pivot_sub = nh->subscribe("/arm_node/arm_commands/shoulder_pivot_torque", 1000, &ArmControl::shoulder_pivot_callback, this);
+        shoulder_joint_sub = nh->subscribe("/arm_node/arm_commands/shoulder_joint_torque", 1000, &ArmControl::shoulder_joint_callback, this);
+        elbow_joint_sub = nh->subscribe("/arm_node/arm_commands/elbow_joint_torque", 1000, &ArmControl::elbow_joint_callback, this);
+        wrist_pivot_sub = nh->subscribe("/arm_node/arm_commands/wrist_pivot_torque", 1000, &ArmControl::wrist_pivot_callback, this);
+        wrist_joint_sub = nh->subscribe("/arm_node/arm_commands/wrist_joint_torque", 1000, &ArmControl::wrist_joint_callback, this);
+        end_effector_sub = nh->subscribe("/arm_node/arm_commands/end_effector_pivot_torque", 1000, &ArmControl::end_effector_pivot_callback, this);
+
+        cout << "Subscribed to all torque channels" << endl;
    }

 private:
    physics::ModelPtr model;
    std::unique_ptr<ros::NodeHandle> nh;
    ros::Subscriber subscriber;
+
+    ros::Subscriber shoulder_pivot_sub;
+    ros::Subscriber shoulder_joint_sub;
+    ros::Subscriber elbow_joint_sub;
+    ros::Subscriber wrist_pivot_sub;
+    ros::Subscriber wrist_joint_sub;
+    ros::Subscriber end_effector_sub;
 };
 GZ_REGISTER_MODEL_PLUGIN(ArmControl)
 } // namespace gazebo