From 8824e339362938f471050979ba2f98a71ab732a9 Mon Sep 17 00:00:00 2001 From: Derek Witcpalek Date: Sat, 4 Apr 2020 18:29:38 -0400 Subject: [PATCH] Added motor control to the simulation, and a test program to verify that it is working as expected --- .../gazebo_models/fanuc_robotic_arm/model.sdf | 1 + .../src/simulation_env/CMakeLists.txt | 18 ++++- .../src/simulation_env/msg/arm_command.msg | 6 ++ .../src/arm_angle_sensor_plugin.cpp | 64 ++++++++--------- .../src/simulation_env/src/arm_motors.cpp | 53 ++++++++++++++ .../src/simulation_env/test/move_arm.cpp | 70 +++++++++++++++++++ 6 files changed, 178 insertions(+), 34 deletions(-) create mode 100644 rrrobot_src/src/simulation_drivers/src/simulation_env/msg/arm_command.msg create mode 100644 rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_motors.cpp create mode 100644 rrrobot_src/src/simulation_drivers/src/simulation_env/test/move_arm.cpp diff --git a/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf b/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf index e127921..3bbaf31 100644 --- a/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf +++ b/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf @@ -900,5 +900,6 @@ 0 + diff --git a/rrrobot_src/src/simulation_drivers/src/simulation_env/CMakeLists.txt b/rrrobot_src/src/simulation_drivers/src/simulation_env/CMakeLists.txt index 3ac0389..5be384d 100644 --- a/rrrobot_src/src/simulation_drivers/src/simulation_env/CMakeLists.txt +++ b/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 ## ############# diff --git a/rrrobot_src/src/simulation_drivers/src/simulation_env/msg/arm_command.msg b/rrrobot_src/src/simulation_drivers/src/simulation_env/msg/arm_command.msg new file mode 100644 index 0000000..419c9ca --- /dev/null +++ b/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 diff --git a/rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_angle_sensor_plugin.cpp b/rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_angle_sensor_plugin.cpp index 3da043f..806743b 100644 --- a/rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_angle_sensor_plugin.cpp +++ b/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("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("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(model->GetChild("shoulder_pivot").get())->Position(); - msg.shoulder_joint_angle = dynamic_cast(model->GetChild("shoulder_joint").get())->Position(); - msg.elbow_joint_angle = dynamic_cast(model->GetChild("elbow_joint").get())->Position(); - msg.wrist_pivot_angle = dynamic_cast(model->GetChild("wrist_pivot").get())->Position(); - msg.wrist_joint_angle = dynamic_cast(model->GetChild("wrist_joint").get())->Position(); - msg.end_effector_pivot_angle = dynamic_cast(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 nh; ros::Publisher publisher; - }; - GZ_REGISTER_MODEL_PLUGIN(JointAngle) -} +}; +GZ_REGISTER_MODEL_PLUGIN(JointAngle) +} // namespace gazebo diff --git a/rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_motors.cpp b/rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_motors.cpp new file mode 100644 index 0000000..099f036 --- /dev/null +++ b/rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_motors.cpp @@ -0,0 +1,53 @@ +#include +#include +#include +#include + +#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 nh; + ros::Subscriber subscriber; +}; +GZ_REGISTER_MODEL_PLUGIN(ArmControl) +} // namespace gazebo diff --git a/rrrobot_src/src/simulation_drivers/src/simulation_env/test/move_arm.cpp b/rrrobot_src/src/simulation_drivers/src/simulation_env/test/move_arm.cpp new file mode 100644 index 0000000..abd0c46 --- /dev/null +++ b/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("/arm_node/arm_commands", 1000); + ros::Subscriber sub = nh.subscribe("/arm_node/arm_positions", 1000, angle_callback); + + ros::spin(); + + return 0; +} \ No newline at end of file