Added motor control to the simulation, and a test program to verify that it is working as expected

rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf

@@ -900,5 +900,6 @@
     <allow_auto_disable>0</allow_auto_disable>
 
     <plugin name="joint_angles" filename="libarm_angles.so"/>
+    <plugin name="arm_control" filename="libarm_motors.so"/>
   </model>
 </sdf>

rrrobot_src/src/simulation_drivers/src/simulation_env/CMakeLists.txt

@@ -52,6 +52,7 @@ find_package(gazebo REQUIRED)
 add_message_files(
   FILES
   arm_angles.msg
+  arm_command.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -129,6 +130,10 @@ add_library(arm_angles SHARED
   src/arm_angle_sensor_plugin.cpp
 )
 
+add_library(arm_motors SHARED
+  src/arm_motors.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
@@ -137,7 +142,7 @@ add_library(arm_angles SHARED
 ## 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/simulation_env_node.cpp)
+add_executable(test_move_arm test/move_arm.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -147,7 +152,7 @@ add_library(arm_angles SHARED
 
 ## 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})
+add_dependencies(test_move_arm arm_angles arm_motors)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(arm_angles
@@ -155,6 +160,15 @@ target_link_libraries(arm_angles
   ${GAZEBO_LIBRARIES}
 )
 
+target_link_libraries(arm_motors
+  ${catkin_LIBRARIES}
+  ${GAZEBO_LIBRARIES}
+)
+
+target_link_libraries(test_move_arm
+  ${catkin_LIBRARIES}
+)
+
 #############
 ## Install ##
 #############

rrrobot_src/src/simulation_drivers/src/simulation_env/msg/arm_command.msg

@@ -0,0 +1,6 @@
+float32 shoulder_pivot_force
+float32 shoulder_joint_force
+float32 elbow_joint_force
+float32 wrist_pivot_force
+float32 wrist_joint_force
+float32 end_effector_pivot_force

rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_angle_sensor_plugin.cpp

@@ -11,51 +11,51 @@ using std::endl;
 
 namespace gazebo
 {
-  class JointAngle : public ModelPlugin
-  {
-  public: 
+class JointAngle : public ModelPlugin
+{
+public:
     void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
     {
-      cout << "Loading JointAngle sensor plugin for arm" << endl;
+        cout << "Loading JointAngle sensor plugin for arm" << endl;
 
-      model = _parent;
+        model = _parent;
 
-      updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&JointAngle::onUpdate, this));
-      if(!ros::isInitialized())
-      {
-        int argc = 0;
-        char **argv = NULL;
-        ros::init(argc, argv, "arm");
-      }
-      
-      nh.reset(new ros::NodeHandle("arm_node"));
-      publisher = nh->advertise<simulation_env::arm_angles>("arm_positions", 1);      
+        updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&JointAngle::onUpdate, this));
+        if (!ros::isInitialized())
+        {
+            int argc = 0;
+            char **argv = NULL;
+            ros::init(argc, argv, "arm");
+        }
+
+        nh.reset(new ros::NodeHandle("arm_node"));
+        publisher = nh->advertise<simulation_env::arm_angles>("arm_positions", 1);
     }
 
-  private:
+private:
     void onUpdate()
     {
-      simulation_env::arm_angles msg;
+        simulation_env::arm_angles msg;
 
-      // read in the joint angle for each joint in the arm
-      msg.shoulder_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("shoulder_pivot").get())->Position();
-      msg.shoulder_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("shoulder_joint").get())->Position();
-      msg.elbow_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("elbow_joint").get())->Position();
-      msg.wrist_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("wrist_pivot").get())->Position();
-      msg.wrist_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("wrist_joint").get())->Position();
-      msg.end_effector_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("end_effector_pivot").get())->Position();
-      
-      // publish the updated sensor measurements
-      publisher.publish(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();
 
-      // make sure the message gets published
-      ros::spinOnce();
+        // publish the updated sensor measurements
+        publisher.publish(msg);
+
+        // make sure the message gets published
+        ros::spinOnce();
     }
 
     physics::ModelPtr model;
     event::ConnectionPtr updateConnection;
     std::unique_ptr<ros::NodeHandle> nh;
     ros::Publisher publisher;
-  };
-  GZ_REGISTER_MODEL_PLUGIN(JointAngle)
-}
+};
+GZ_REGISTER_MODEL_PLUGIN(JointAngle)
+} // namespace gazebo

rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_motors.cpp

@@ -0,0 +1,53 @@
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <memory>
+#include <iostream>
+
+#include "ros/ros.h"
+#include "simulation_env/arm_command.h"
+
+using std::cout;
+using std::endl;
+
+namespace gazebo
+{
+class ArmControl : public ModelPlugin
+{
+public:
+    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);
+
+        ros::spinOnce();
+    }
+
+    void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
+    {
+        cout << "Loading motor control plugin for arm" << endl;
+
+        model = _parent;
+
+        if (!ros::isInitialized())
+        {
+            int argc = 0;
+            char **argv = NULL;
+            ros::init(argc, argv, "arm");
+        }
+
+        nh.reset(new ros::NodeHandle("arm_node"));
+        subscriber = nh->subscribe("arm_commands", 1000, &ArmControl::arm_command_callback, this);
+    }
+
+private:
+    physics::ModelPtr model;
+    std::unique_ptr<ros::NodeHandle> nh;
+    ros::Subscriber subscriber;
+};
+GZ_REGISTER_MODEL_PLUGIN(ArmControl)
+} // namespace gazebo

rrrobot_src/src/simulation_drivers/src/simulation_env/test/move_arm.cpp

@@ -0,0 +1,70 @@
+#include "ros/ros.h"
+#include "simulation_env/arm_command.h"
+#include "simulation_env/arm_angles.h"
+
+class PID_Control
+{
+public:
+    PID_Control(float Kp_in, float Ki_in, float Kd_in)
+        : Kp(Kp_in), Ki(Ki_in), Kd(Kd_in)
+    {}
+
+    float update(float error)
+    {
+        if(error > 0 and prev_error < 0 
+            || error < 0 and prev_error > 0)
+        {
+            reset();
+        }
+        sum_error += error;
+        float to_ret = Kp * error + Ki * sum_error + Kd * (error - prev_error);
+
+        prev_error = error;
+        return to_ret;
+    }
+
+    void reset()
+    {
+        sum_error = 0;
+        prev_error = 0;
+    }
+
+private:
+    const float Kp;
+    const float Ki;
+    const float Kd;
+
+    double sum_error;
+    double prev_error;
+};
+
+
+PID_Control pid_controller(1000, 1, 0);
+ros::Publisher publisher;
+
+
+
+void angle_callback(const simulation_env::arm_angles &msg)
+{
+    // just control the shoulder joint to go to 0 (should be roughly straight up)
+    float output = pid_controller.update(0 - msg.shoulder_joint_angle);
+
+    simulation_env::arm_command cmd;
+    cmd.shoulder_joint_force = output;
+
+    publisher.publish(cmd);
+    ros::spinOnce();
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "arm_control_test");
+
+    ros::NodeHandle nh;
+    publisher = nh.advertise<simulation_env::arm_command>("/arm_node/arm_commands", 1000);
+    ros::Subscriber sub = nh.subscribe("/arm_node/arm_positions", 1000, angle_callback);
+
+    ros::spin();
+
+    return 0;
+}