diff --git a/rrrobot_ws/src/arm_control/CMakeLists.txt b/rrrobot_ws/src/arm_control/CMakeLists.txt deleted file mode 100644 index 461509e..0000000 --- a/rrrobot_ws/src/arm_control/CMakeLists.txt +++ /dev/null @@ -1,268 +0,0 @@ -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_ws/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} ${PROJECT_NAME}_generate_messages_cpp simulation_env_generate_messages_cpp) - -## 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) diff --git a/rrrobot_ws/src/arm_control/include/arm_control/arm.h b/rrrobot_ws/src/arm_control/include/arm_control/arm.h deleted file mode 100644 index a54abfb..0000000 --- a/rrrobot_ws/src/arm_control/include/arm_control/arm.h +++ /dev/null @@ -1,47 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -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 fksolver; - std::unique_ptr iksolver; - std::unique_ptr idsolver; -}; \ No newline at end of file diff --git a/rrrobot_ws/src/arm_control/launch/arm.launch b/rrrobot_ws/src/arm_control/launch/arm.launch deleted file mode 100644 index 0ead542..0000000 --- a/rrrobot_ws/src/arm_control/launch/arm.launch +++ /dev/null @@ -1,123 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rrrobot_ws/src/arm_control/package.xml b/rrrobot_ws/src/arm_control/package.xml deleted file mode 100644 index c11d1c4..0000000 --- a/rrrobot_ws/src/arm_control/package.xml +++ /dev/null @@ -1,65 +0,0 @@ - - - arm_control - 0.0.0 - The arm_control package - - - - - dwitcpa - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - std_msgs - roscpp - std_msgs - roscpp - std_msgs - - - - - - - - diff --git a/rrrobot_ws/src/arm_control/src/arm.cpp b/rrrobot_ws/src/arm_control/src/arm.cpp deleted file mode 100644 index 3a8a6f3..0000000 --- a/rrrobot_ws/src/arm_control/src/arm.cpp +++ /dev/null @@ -1,203 +0,0 @@ -#include - -#include -#include -#include -#include - -#include -#include -#include - -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("name"); - cout << "Found " << model_name << " model" << endl; - - unordered_map links; - unordered_map link_ordering; - string first_link; - - // start reading links and joints - sdf::ElementPtr link = model->GetElement("link"); - while (link) - { - const string name(link->Get("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("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("ixx"), - inertia->Get("iyy"), - inertia->Get("izz"), - inertia->Get("ixy"), - inertia->Get("ixz"), - inertia->Get("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("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; - } -} \ No newline at end of file diff --git a/rrrobot_ws/src/arm_control/src/arm_controller.cpp b/rrrobot_ws/src/arm_control/src/arm_controller.cpp deleted file mode 100644 index 5fdd735..0000000 --- a/rrrobot_ws/src/arm_control/src/arm_controller.cpp +++ /dev/null @@ -1,128 +0,0 @@ -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "ros/ros.h" -#include -#include -#include - -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 ¤t_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("/arm_node/shoulder_pivot_setpoint", 1000); - shoulder_joint_publisher = nh.advertise("/arm_node/shoulder_joint_setpoint", 1000); - elbow_joint_publisher = nh.advertise("/arm_node/elbow_joint_setpoint", 1000); - wrist_pivot_publisher = nh.advertise("/arm_node/wrist_pivot_setpoint", 1000); - wrist_joint_publisher = nh.advertise("/arm_node/wrist_joint_setpoint", 1000); - end_effector_pivot_publisher = nh.advertise("/arm_node/end_effector_pivot_setpoint", 1000); - - ros::spin(); -} \ No newline at end of file diff --git a/rrrobot_ws/src/arm_control/test/angles_subscriber.cpp b/rrrobot_ws/src/arm_control/test/angles_subscriber.cpp deleted file mode 100644 index 0dedd1b..0000000 --- a/rrrobot_ws/src/arm_control/test/angles_subscriber.cpp +++ /dev/null @@ -1,153 +0,0 @@ -#include -#include "ros/ros.h" -#include - -#include - -#include -#include -#include - -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(); - // } -} \ No newline at end of file diff --git a/rrrobot_ws/src/arm_control/test/arm_movement_demo.cpp b/rrrobot_ws/src/arm_control/test/arm_movement_demo.cpp deleted file mode 100644 index 4c493ef..0000000 --- a/rrrobot_ws/src/arm_control/test/arm_movement_demo.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include -#include "ros/ros.h" -#include - -#include - -#include -#include -#include - -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("/arm_node/arm_pose_setpoint", 1000); - - int state = 0; - - vector 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(); - } -} \ No newline at end of file diff --git a/rrrobot_ws/src/arm_control/test/test.cpp b/rrrobot_ws/src/arm_control/test/test.cpp deleted file mode 100644 index f8c2d2d..0000000 --- a/rrrobot_ws/src/arm_control/test/test.cpp +++ /dev/null @@ -1,206 +0,0 @@ -// Copyright (C) 2007 Francois Cauwe - -// 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include - -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 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"; - } -} \ No newline at end of file diff --git a/rrrobot_ws/src/arm_control/test/test_internal_arm_representation.cpp b/rrrobot_ws/src/arm_control/test/test_internal_arm_representation.cpp deleted file mode 100644 index b5f4122..0000000 --- a/rrrobot_ws/src/arm_control/test/test_internal_arm_representation.cpp +++ /dev/null @@ -1,128 +0,0 @@ -#include - -#include "gtest/gtest.h" -#include -#include -#include - -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(); -} \ No newline at end of file diff --git a/rrrobot_ws/src/arm_control/test/test_pub_sub.cpp b/rrrobot_ws/src/arm_control/test/test_pub_sub.cpp deleted file mode 100644 index 3d93180..0000000 --- a/rrrobot_ws/src/arm_control/test/test_pub_sub.cpp +++ /dev/null @@ -1,97 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/Float64.h" - -#include - -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("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; -} \ No newline at end of file diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/arm_joint_and_link_definitions.jpg b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/arm_joint_and_link_definitions.jpg deleted file mode 100644 index 5d97c90..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/arm_joint_and_link_definitions.jpg and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL deleted file mode 100644 index 1cc6dee..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL deleted file mode 100644 index 7960a24..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL deleted file mode 100644 index d81bd58..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL deleted file mode 100644 index f764555..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL deleted file mode 100644 index 1c02f3f..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL deleted file mode 100644 index 3e2ee1c..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL deleted file mode 100644 index 6469280..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.config b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.config deleted file mode 100644 index 05b32ed..0000000 --- a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.config +++ /dev/null @@ -1,11 +0,0 @@ - - - Fanuc_robot_arm - 1.0 - model.sdf - - - - - This was created using parts from a CAD model on GrabCAD - diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf deleted file mode 100644 index 7eb02fd..0000000 --- a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf +++ /dev/null @@ -1,905 +0,0 @@ - - - - - 0 0 0 3.14159 -0 0 - - 86.082 - - 4.3404 - 0 - 0.0020207 - 2.4866 - 0 - 6.1574 - - -0.003895 -0.062844 -0.11068 0 -0 0 - - 1 - 0 - 0 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - 0.001715 0.003779 0.661683 1.5708 -0 0 - - 266.27 - - 279.51 - 0 - 4.6477 - 43.386 - 34.265 - 255.26 - - 0.093634 -0.51398 -0.1614 0 -0 0 - - 1 - 0 - 0 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - -0.00025 -0.94659 5.45119 1.5708 -0 0 - - 63.612 - - 6.6124 - 0.099584 - 0 - 5.9295 - 0 - 1.9517 - - 0.00756 -3.4862 -0.40714 0 -0 0 - - 1 - 0 - 0 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - -0.068455 0.275759 0.315754 -1.571 -0 0 - - 60.795 - - 8.1939 - 0 - 0 - 0.76659 - 0 - 8.0226 - - 0.25344 -1.009 -0.57114 0 -0 0 - - 1 - 0 - 0 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - -0.000257 -3.97477 2.07064 -1.5708 7e-06 3.14159 - - 6.6264 - - 0.089718 - 0 - 0.017666 - 0.092881 - 3e-08 - 0.028564 - - -0.028814 0 -2.454 0 -0 0 - - 1 - 0 - 0 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - 0.000743 -4.767 2.06498 -1.5708 7e-06 3.14159 - - 5.1229 - - 0.038195 - 4e-07 - 0 - 0.066675 - 6e-07 - 0.04821 - - -0.010371 1e-06 -3.0669 0 -0 0 - - 1 - 0 - 0 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - -2.06426 -5.37015 4.13075 1.88426 1.57078 0.313465 - - 0.72893 - - 0.00131 - 0 - 0 - 0.0012978 - 0 - 0.0024341 - - 2.0657 2.0642 -3.5441 0 -0 0 - - 1 - 0 - 0 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - link_3 - link_4 - 0 0 -2.7 0 -0 0 - - 0 0 1 - 0 - - -1.79769e+308 - 1.79769e+308 - -20000000 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - link_1 - link_2 - 0 -0.41 -0.61 0 -0 0 - - 1 0 0 - 0 - - -1.79769e+308 - 1.79769e+308 - -20000000 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - link_2 - link_3 - 0 -3.65 -0.6 0 -0 0 - - 1 0 0 - 0 - - -1.79769e+308 - 1.79769e+308 - -20000000 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - link_4 - link_5 - 0 0 -3.15 0 -0 0 - - 1 0 0 - 0 - - -1.79769e+308 - 1.79769e+308 - -20000000 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - base - link_1 - 0 0 0 0 -0 0 - - 0 1 0 - 0 - - -1.79769e+308 - 1.79769e+308 - -20000000 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - link_5 - link_6 - 2.065 2.065 -3.5 0 -0 0 - - 0 0 1 - 0 - - -1.79769e+308 - 1.79769e+308 - -20000000 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - world - base - - 0 - 0 - - - - - diff --git a/rrrobot_ws/src/rrrobot/CMakeLists.txt b/rrrobot_ws/src/rrrobot/CMakeLists.txt new file mode 100644 index 0000000..30d86ab --- /dev/null +++ b/rrrobot_ws/src/rrrobot/CMakeLists.txt @@ -0,0 +1,120 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rrrobot) + +find_package(catkin REQUIRED COMPONENTS + osrf_gear + roscpp + sensor_msgs + std_srvs + trajectory_msgs + std_msgs + geometry_msgs + gazebo_ros + message_generation +) +find_package(gazebo 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() + +## Generate messages in the 'msg' folder +add_message_files( + FILES + model_insertion.msg +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) + +catkin_package( + INCLUDE_DIRS +# LIBRARIES simulation_env + CATKIN_DEPENDS roscpp std_msgs geometry_msgs gazebo_ros +# DEPENDS system_lib +) + +########### +## Build ## +########### +include_directories( + ${catkin_INCLUDE_DIRS} + ${roscpp_INCLUDE_DIRS} + ${std_msgs_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${GAZEBO_INCLUDE_DIRS} +) + +add_library(insert_model SHARED + src/model_insertion_plugin.cpp +) +add_dependencies(insert_model ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + + +## Declare a C++ executable +add_executable(rrrobot_node src/rrrobot_node.cpp) +add_dependencies(rrrobot_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(rrrobot_node ${catkin_LIBRARIES}) + +add_executable(test_insert_object test/test_insert_object.cpp) +add_dependencies(test_insert_object insert_model) +target_link_libraries(test_insert_object + ${catkin_LIBRARIES} +) + +target_link_libraries(insert_model + ${catkin_LIBRARIES} + ${GAZEBO_LIBRARIES} +) + +############# +## 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 +# install(PROGRAMS +# script/rrrobot_node.py +# script/tf2_example.py +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS rrrobot_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_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 +# config/sample_gear_conf.yaml +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_ariac_example.cpp) +# 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) \ No newline at end of file diff --git a/rrrobot_ws/src/rrrobot/config/rrrobot.yaml b/rrrobot_ws/src/rrrobot/config/rrrobot.yaml new file mode 100644 index 0000000..242144e --- /dev/null +++ b/rrrobot_ws/src/rrrobot/config/rrrobot.yaml @@ -0,0 +1,163 @@ +# Competition configuration options +options: + insert_models_over_bins: true # Whether or not to insert the models that are specified in models_over_bins + spawn_extra_models: true # Whether or not to spawn the models that are specified in models_to_spawn + gazebo_state_logging: true # Whether or not to generate a gazebo state log + belt_population_cycles: 5 # How many cycles to spawn parts on the conveyor + model_type_aliases: # Aliases for model types which can be used in the configuration file + order_part1: piston_rod_part # Wherever 'order_part1' is used in the configuration file, use 'piston_rod_part' + order_part2: gear_part + order_part3: pulley_part + visualize_drop_regions: false # Whether or not to visualize drop regions (world frame only) + +time_limit: 500 # Maximum time allowed for the trial once started, in seconds +random_seed: 1 # Seed for the pseudo random number generator (used to randomize model names) + +# Orders to announce during the trial +# orders: +# order_0: +# announcement_condition: time # Announce the order base on elapsed time +# announcement_condition_value: 0 # Time in seconds to wait before announcing the order +# shipment_count: 1 # How many of the same shipment are in the order +# destinations: [agv1] # Which agv the shipments must be delivered to +# # If specified it must be a list the same length as the shipment count +# # allowed values: agv1, agv2, any +# products: # List of products required to be in the kit +# part_0: +# type: order_part1 # Type of model required +# pose: +# xyz: [0.1, -0.15, 0] # Position required in the tray frame +# rpy: [0, 0, 'pi/2'] # Orientation required in the tray frame +# part_1: +# type: order_part1 +# pose: +# xyz: [-0.1, -0.15, 0] +# rpy: [0, 0, 'pi/2'] +# part_2: +# type: order_part2 +# pose: +# xyz: [0.1, 0.15, 0] +# rpy: [0, 0, 0] +# part_3: +# type: order_part2 +# pose: +# xyz: [-0.1, 0.15, 0] +# rpy: [0, 0, 0] +# order_1: +# announcement_condition: wanted_products # Announce the order when the boxes contain products from this order +# announcement_condition_value: 2 +# shipment_count: 1 +# destinations: [any] +# products: +# part_0: +# type: order_part3 +# pose: +# xyz: [0.12, -0.2, 0] +# rpy: ['pi', 0, 0] +# part_1: +# type: order_part3 +# pose: +# xyz: [-0.12, -0.2, 0] +# rpy: [0, 'pi', 0] +# part_2: +# type: order_part1 +# pose: +# xyz: [0.15, 0.15, 0] +# rpy: [0, 0, 0] +# part_3: +# type: order_part2 +# pose: +# xyz: [-0.15, 0.15, 0] +# rpy: [0, 0, 0] +# part_4: +# type: order_part2 +# pose: +# rpy: [0, 'pi', 0] + +# # Individual products that will be reported as faulty +# faulty_products: +# - piston_rod_part_57 # The piston rod part in the bins with randomized ID of 57 +# - piston_rod_part_45 + +# Models to be inserted in the bins +models_over_bins: + bin1: # Name of the bin (bin1-bin6, as named in the environment simulation) + models: # List of models to insert + # gear_part: # Type of model to insert + # xyz_start: [0.1, 0.1, 0] # Origin of the first model to insert + # xyz_end: [0.5, 0.5, 0] # Origin of the last model to insert + # rpy: [0, 0, 'pi/4'] # Orientation of all models to insert + # num_models_x: 3 # How many models to insert along the x dimension + # num_models_y: 5 # How many models to insert along the y dimension + # bin5: + # models: + # gasket_part: + # xyz_start: [0.1, 0.1, 0.0] + # xyz_end: [0.5, 0.5, 0.0] + # rpy: [0, 0, 'pi/4'] + # num_models_x: 2 + # num_models_y: 3 + # bin6: + # models: + # piston_rod_part: + # xyz_start: [0.1, 0.1, 0.0] + # xyz_end: [0.5, 0.5, 0.0] + # rpy: [0, 0, 'pi/4'] + # num_models_x: 3 + # num_models_y: 3 + +# # Models to be spawned in particular reference frames +# models_to_spawn: +# bin2::link: # Name of the reference frame +# models: # List of models to spawn +# piston_rod_part_1: # An arbitrary unique name of the model (will be randomized) +# type: piston_rod_part # Type of model (must be installed locally) +# pose: +# xyz: [0.2, 0.2, 0.75] # Co-ordinates of the model origin in the specified reference frame +# rpy: [0, 0, '-pi/2'] # Roll, pitch, yaw of the model in the specified reference frame + +# # Models to be spawned on the conveyor belt +# belt_models: +# pulley_part: # name of part model to spawn +# 1.0: # Time in seconds after trial starts to spawn it +# pose: +# xyz: [0.0, 0.0, 0.1] # Coordinates relative to the start of the container to spawn the part +# rpy: [0, 0, 'pi/2'] # Roll, pitch, yaw of the model +# disk_part: +# 5.0: +# pose: +# xyz: [0.0, 0.0, 0.1] +# rpy: [0, 0, 'pi/2'] + +# # Drops from the vacuum gripper to be triggered at particular locations +# drops: +# drop_regions: +# above_agv_1: +# frame: agv1::kit_tray_1 # Frame the drop region/destination is specified in +# min: # Min corner of the bounding box that triggers a drop +# xyz: [-0.3, -0.3, 0.0] +# max: # Max corner of the bounding box that triggers a drop +# xyz: [0.3, 03, 0.5] +# destination: # Where to drop the part to +# xyz: [-0.35, 0.1, 0.15] +# rpy: [0, 0, 0.2] +# product_type_to_drop: gear_part +# above_agv_2: +# frame: agv2::kit_tray_2 +# min: +# xyz: [-0.3, -0.3, 0.0] +# max: +# xyz: [0.3, 0.3, 0.5] +# destination: +# xyz: [0.15, 0.15, 0.15] +# rpy: [0, 0, -0.2] +# product_type_to_drop: pulley_part +# bin5_reachable: +# min: +# xyz: [0.0, 0.7, 0.7] +# max: +# xyz: [0.2, 1.60, 1.3] +# destination: +# xyz: [0.65, 1.15, 0.76] +# rpy: [0, 0, 0.5] +# product_type_to_drop: gasket_part diff --git a/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml b/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml new file mode 100644 index 0000000..33de276 --- /dev/null +++ b/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml @@ -0,0 +1,36 @@ +sensors: + break_beam_1: + type: break_beam + pose: + xyz: [1.6, 2.25, 0.91] + rpy: [0, 0, 'pi'] + proximity_sensor_1: + type: proximity_sensor + pose: + xyz: [0.95, 2.6, 0.92] + rpy: [0, 0, 0] + logical_camera_1: + type: logical_camera + pose: + xyz: [-0.3, 0.383, 1.27] + rpy: [0.2, 1.5707, 0] + logical_camera_2: + type: logical_camera + pose: + xyz: [0.3, 3.15, 1.5] + rpy: [0, 'pi/2', 0] + logical_camera_3: + type: logical_camera + pose: + xyz: [0.3, -3.15, 1.5] + rpy: [0, 'pi/2', 0] + depth_camera_1: + type: depth_camera + pose: + xyz: [-0.3, -0.383, 1.2] + rpy: [0, 1.5707, 0] + laser_profiler_1: + type: laser_profiler + pose: + xyz: [1.21, 4, 1.6] + rpy: ['-pi', 'pi/2', 'pi/2'] diff --git a/rrrobot_ws/src/rrrobot/launch/rrrobot.launch b/rrrobot_ws/src/rrrobot/launch/rrrobot.launch new file mode 100644 index 0000000..224c26d --- /dev/null +++ b/rrrobot_ws/src/rrrobot/launch/rrrobot.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + diff --git a/rrrobot_ws/src/rrrobot/package.xml b/rrrobot_ws/src/rrrobot/package.xml new file mode 100644 index 0000000..b7b90c9 --- /dev/null +++ b/rrrobot_ws/src/rrrobot/package.xml @@ -0,0 +1,27 @@ + + + rrrobot + 0.1.0 + RRRobot package + Sravan Balaji + Chenxi Gu + Jake Johnson + Derek Witcpalek + + MIT + + catkin + + osrf_gear + roscpp + sensor_msgs + std_srvs + tf2_geometry_msgs + trajectory_msgs + std_msgs + geometry_msgs + gazebo_ros + message_generation + message_runtime + + \ No newline at end of file diff --git a/rrrobot_ws/src/rrrobot/scripts/rrrobot_run.sh b/rrrobot_ws/src/rrrobot/scripts/rrrobot_run.sh new file mode 100644 index 0000000..41155ec --- /dev/null +++ b/rrrobot_ws/src/rrrobot/scripts/rrrobot_run.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +sudo cp ../launch/rrrobot.launch /opt/ros/melodic/share/osrf_gear/launch + +roslaunch osrf_gear rrrobot.launch diff --git a/rrrobot_ws/src/gear/sample_run.sh b/rrrobot_ws/src/rrrobot/scripts/sample_run.sh similarity index 100% rename from rrrobot_ws/src/gear/sample_run.sh rename to rrrobot_ws/src/rrrobot/scripts/sample_run.sh diff --git a/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp b/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp new file mode 100644 index 0000000..2314b17 --- /dev/null +++ b/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp @@ -0,0 +1,261 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// %Tag(FULLTEXT)% +// %Tag(INCLUDE_STATEMENTS)% +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// %EndTag(INCLUDE_STATEMENTS)% + +// %Tag(START_COMP)% +/// Start the competition by waiting for and then calling the start ROS Service. +void start_competition(ros::NodeHandle & node) { + // Create a Service client for the correct service, i.e. '/ariac/start_competition'. + ros::ServiceClient start_client = + node.serviceClient("/ariac/start_competition"); + // If it's not already ready, wait for it to be ready. + // Calling the Service using the client before the server is ready would fail. + if (!start_client.exists()) { + ROS_INFO("Waiting for the competition to be ready..."); + start_client.waitForExistence(); + ROS_INFO("Competition is now ready."); + } + ROS_INFO("Requesting competition start..."); + std_srvs::Trigger srv; // Combination of the "request" and the "response". + start_client.call(srv); // Call the start Service. + if (!srv.response.success) { // If not successful, print out why. + ROS_ERROR_STREAM("Failed to start the competition: " << srv.response.message); + } else { + ROS_INFO("Competition started!"); + } +} +// %EndTag(START_COMP)% + +/// Example class that can hold state and provide methods that handle incoming data. +class MyCompetitionClass +{ +public: + explicit MyCompetitionClass(ros::NodeHandle & node) + : current_score_(0), arm_1_has_been_zeroed_(false), arm_2_has_been_zeroed_(false) + { + // %Tag(ADV_CMD)% + arm_1_joint_trajectory_publisher_ = node.advertise( + "/ariac/arm1/arm/command", 10); + + arm_2_joint_trajectory_publisher_ = node.advertise( + "/ariac/arm2/arm/command", 10); + // %EndTag(ADV_CMD)% + } + + /// Called when a new message is received. + void current_score_callback(const std_msgs::Float32::ConstPtr & msg) { + if (msg->data != current_score_) + { + ROS_INFO_STREAM("Score: " << msg->data); + } + current_score_ = msg->data; + } + + /// Called when a new message is received. + void competition_state_callback(const std_msgs::String::ConstPtr & msg) { + if (msg->data == "done" && competition_state_ != "done") + { + ROS_INFO("Competition ended."); + } + competition_state_ = msg->data; + } + + /// Called when a new Order message is received. + void order_callback(const osrf_gear::Order::ConstPtr & order_msg) { + ROS_INFO_STREAM("Received order:\n" << *order_msg); + received_orders_.push_back(*order_msg); + } + + // %Tag(CB_CLASS)% + /// Called when a new JointState message is received. + void arm_1_joint_state_callback( + const sensor_msgs::JointState::ConstPtr & joint_state_msg) + { + ROS_INFO_STREAM_THROTTLE(10, + "Joint States arm 1 (throttled to 0.1 Hz):\n" << *joint_state_msg); + // ROS_INFO_STREAM("Joint States:\n" << *joint_state_msg); + arm_1_current_joint_states_ = *joint_state_msg; + if (!arm_1_has_been_zeroed_) { + arm_1_has_been_zeroed_ = true; + ROS_INFO("Sending arm to zero joint positions..."); + send_arm_to_zero_state(arm_1_joint_trajectory_publisher_); + } + } + + void arm_2_joint_state_callback( + const sensor_msgs::JointState::ConstPtr & joint_state_msg) + { + ROS_INFO_STREAM_THROTTLE(10, + "Joint States arm 2 (throttled to 0.1 Hz):\n" << *joint_state_msg); + // ROS_INFO_STREAM("Joint States:\n" << *joint_state_msg); + arm_2_current_joint_states_ = *joint_state_msg; + if (!arm_2_has_been_zeroed_) { + arm_2_has_been_zeroed_ = true; + ROS_INFO("Sending arm 2 to zero joint positions..."); + send_arm_to_zero_state(arm_2_joint_trajectory_publisher_); + } + } + // %EndTag(CB_CLASS)% + + // %Tag(ARM_ZERO)% + /// Create a JointTrajectory with all positions set to zero, and command the arm. + void send_arm_to_zero_state(ros::Publisher & joint_trajectory_publisher) { + // Create a message to send. + trajectory_msgs::JointTrajectory msg; + + // Fill the names of the joints to be controlled. + // Note that the vacuum_gripper_joint is not controllable. + msg.joint_names.clear(); + msg.joint_names.push_back("shoulder_pan_joint"); + msg.joint_names.push_back("shoulder_lift_joint"); + msg.joint_names.push_back("elbow_joint"); + msg.joint_names.push_back("wrist_1_joint"); + msg.joint_names.push_back("wrist_2_joint"); + msg.joint_names.push_back("wrist_3_joint"); + msg.joint_names.push_back("linear_arm_actuator_joint"); + // Create one point in the trajectory. + msg.points.resize(1); + // Resize the vector to the same length as the joint names. + // Values are initialized to 0. + msg.points[0].positions.resize(msg.joint_names.size(), 0.0); + // How long to take getting to the point (floating point seconds). + msg.points[0].time_from_start = ros::Duration(0.001); + ROS_INFO_STREAM("Sending command:\n" << msg); + joint_trajectory_publisher.publish(msg); + } + // %EndTag(ARM_ZERO)% + + /// Called when a new LogicalCameraImage message is received. + void logical_camera_callback( + const osrf_gear::LogicalCameraImage::ConstPtr & image_msg) + { + ROS_INFO_STREAM_THROTTLE(10, + "Logical camera: '" << image_msg->models.size() << "' objects."); + } + + /// Called when a new Proximity message is received. + void break_beam_callback(const osrf_gear::Proximity::ConstPtr & msg) { + if (msg->object_detected) { // If there is an object in proximity. + ROS_INFO("Break beam triggered."); + } + } + +private: + std::string competition_state_; + double current_score_; + ros::Publisher arm_1_joint_trajectory_publisher_; + ros::Publisher arm_2_joint_trajectory_publisher_; + std::vector received_orders_; + sensor_msgs::JointState arm_1_current_joint_states_; + sensor_msgs::JointState arm_2_current_joint_states_; + bool arm_1_has_been_zeroed_; + bool arm_2_has_been_zeroed_; +}; + +void proximity_sensor_callback(const sensor_msgs::Range::ConstPtr & msg) { + if ((msg->max_range - msg->range) > 0.01) { // If there is an object in proximity. + ROS_INFO_THROTTLE(1, "Proximity sensor sees something."); + } +} + +void laser_profiler_callback(const sensor_msgs::LaserScan::ConstPtr & msg) { + size_t number_of_valid_ranges = std::count_if( + msg->ranges.begin(), msg->ranges.end(), [](const float f) {return std::isfinite(f);}); + if (number_of_valid_ranges > 0) { + ROS_INFO_THROTTLE(1, "Laser profiler sees something."); + } +} + +// %Tag(MAIN)% +int main(int argc, char ** argv) { + // Last argument is the default name of the node. + ros::init(argc, argv, "rrrobot_node"); + + ros::NodeHandle node; + + // Instance of custom class from above. + MyCompetitionClass comp_class(node); + + // Subscribe to the '/ariac/current_score' topic. + ros::Subscriber current_score_subscriber = node.subscribe( + "/ariac/current_score", 10, + &MyCompetitionClass::current_score_callback, &comp_class); + + // Subscribe to the '/ariac/competition_state' topic. + ros::Subscriber competition_state_subscriber = node.subscribe( + "/ariac/competition_state", 10, + &MyCompetitionClass::competition_state_callback, &comp_class); + + // %Tag(SUB_CLASS)% + // Subscribe to the '/ariac/orders' topic. + ros::Subscriber orders_subscriber = node.subscribe( + "/ariac/orders", 10, + &MyCompetitionClass::order_callback, &comp_class); + // %EndTag(SUB_CLASS)% + + // Subscribe to the '/ariac/joint_states' topic. + ros::Subscriber arm_1_joint_state_subscriber = node.subscribe( + "/ariac/arm1/joint_states", 10, + &MyCompetitionClass::arm_1_joint_state_callback, &comp_class); + + ros::Subscriber arm_2_joint_state_subscriber = node.subscribe( + "/ariac/arm2/joint_states", 10, + &MyCompetitionClass::arm_2_joint_state_callback, &comp_class); + + // %Tag(SUB_FUNC)% + // Subscribe to the '/ariac/proximity_sensor_1' topic. + ros::Subscriber proximity_sensor_subscriber = node.subscribe( + "/ariac/proximity_sensor_1", 10, proximity_sensor_callback); + // %EndTag(SUB_FUNC)% + + // Subscribe to the '/ariac/break_beam_1_change' topic. + ros::Subscriber break_beam_subscriber = node.subscribe( + "/ariac/break_beam_1_change", 10, + &MyCompetitionClass::break_beam_callback, &comp_class); + + // Subscribe to the '/ariac/logical_camera_1' topic. + ros::Subscriber logical_camera_subscriber = node.subscribe( + "/ariac/logical_camera_1", 10, + &MyCompetitionClass::logical_camera_callback, &comp_class); + + // Subscribe to the '/ariac/laser_profiler_1' topic. + ros::Subscriber laser_profiler_subscriber = node.subscribe( + "/ariac/laser_profiler_1", 10, laser_profiler_callback); + + ROS_INFO("Setup complete."); + start_competition(node); + ros::spin(); // This executes callbacks on new data until ctrl-c. + + return 0; +} +// %EndTag(MAIN)% +// %EndTag(FULLTEXT)% diff --git a/rrrobot_ws/src/simulation_env/CMakeLists.txt b/rrrobot_ws/src/simulation_env/CMakeLists.txt deleted file mode 100644 index c3a1965..0000000 --- a/rrrobot_ws/src/simulation_env/CMakeLists.txt +++ /dev/null @@ -1,246 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(simulation_env) - -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) - -## 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 - gazebo_ros - message_generation -) -find_package(gazebo REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## 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 - arm_angles.msg - arm_command.msg - model_insertion.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 - geometry_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 -# LIBRARIES simulation_env - CATKIN_DEPENDS roscpp std_msgs geometry_msgs gazebo_ros -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${roscpp_INCLUDE_DIRS} - ${std_msgs_INCLUDE_DIRS} - ${geometry_msgs_INCLUDE_DIRS} - ${GAZEBO_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(arm_angles SHARED - src/arm_angle_sensor_plugin.cpp -) - -add_library(arm_motors SHARED - src/arm_motors.cpp -) - -add_library(insert_model SHARED - src/model_insertion_plugin.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(arm_angles ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(arm_motors ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(insert_model ${${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(test_move_arm test/move_arm.cpp) -add_executable(test_insert_object test/test_insert_object.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(test_move_arm arm_angles arm_motors) -add_dependencies(test_insert_object insert_model) - -## Specify libraries to link a library or executable target against -target_link_libraries(arm_angles - ${catkin_LIBRARIES} - ${GAZEBO_LIBRARIES} -) - -target_link_libraries(arm_motors - ${catkin_LIBRARIES} - ${GAZEBO_LIBRARIES} -) - -target_link_libraries(insert_model - ${catkin_LIBRARIES} - ${GAZEBO_LIBRARIES} -) - -target_link_libraries(test_move_arm - ${catkin_LIBRARIES} -) - -target_link_libraries(test_insert_object - ${catkin_LIBRARIES} -) - -############# -## 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 arm_angles arm_motors insert_model -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION /home/rrrobot/rrrobot_ws/lib/ -# 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_simulation_env.cpp) -# 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) diff --git a/rrrobot_ws/src/simulation_env/msg/arm_angles.msg b/rrrobot_ws/src/simulation_env/msg/arm_angles.msg deleted file mode 100644 index 98d9a46..0000000 --- a/rrrobot_ws/src/simulation_env/msg/arm_angles.msg +++ /dev/null @@ -1,13 +0,0 @@ -float32 shoulder_pivot_angle -float32 shoulder_joint_angle -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 diff --git a/rrrobot_ws/src/simulation_env/msg/arm_command.msg b/rrrobot_ws/src/simulation_env/msg/arm_command.msg deleted file mode 100644 index 419c9ca..0000000 --- a/rrrobot_ws/src/simulation_env/msg/arm_command.msg +++ /dev/null @@ -1,6 +0,0 @@ -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_ws/src/simulation_env/msg/model_insertion.msg b/rrrobot_ws/src/simulation_env/msg/model_insertion.msg deleted file mode 100644 index 72337ab..0000000 --- a/rrrobot_ws/src/simulation_env/msg/model_insertion.msg +++ /dev/null @@ -1,2 +0,0 @@ -string model_name -geometry_msgs/Pose pose \ No newline at end of file diff --git a/rrrobot_ws/src/simulation_env/package.xml b/rrrobot_ws/src/simulation_env/package.xml deleted file mode 100644 index a40842f..0000000 --- a/rrrobot_ws/src/simulation_env/package.xml +++ /dev/null @@ -1,73 +0,0 @@ - - - simulation_env - 0.0.0 - The simulation_env package - - - - - root - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - std_msgs - geometry_msgs - gazebo_ros - message_generation - roscpp - std_msgs - geometry_msgs - gazebo_ros - roscpp - std_msgs - geometry_msgs - gazebo_ros - message_runtime - - - - - - - - diff --git a/rrrobot_ws/src/simulation_env/src/arm_angle_sensor_plugin.cpp b/rrrobot_ws/src/simulation_env/src/arm_angle_sensor_plugin.cpp deleted file mode 100644 index eb4ab1b..0000000 --- a/rrrobot_ws/src/simulation_env/src/arm_angle_sensor_plugin.cpp +++ /dev/null @@ -1,133 +0,0 @@ -#include -#include -#include -#include -#include - -#include "ros/ros.h" -#include "simulation_env/arm_angles.h" -#include - -using std::cout; -using std::endl; - -namespace gazebo -{ -class JointAngle : public ModelPlugin -{ -public: - void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) - { - cout << "Loading JointAngle sensor plugin for arm" << endl; - - 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", 1000); - - shoulder_pivot_pub = nh->advertise("/arm_node/arm_positions/shoulder_pivot_angle", 1); - shoulder_joint_pub = nh->advertise("/arm_node/arm_positions/shoulder_joint_angle", 1); - elbow_joint_pub = nh->advertise("/arm_node/arm_positions/elbow_joint_angle", 1); - wrist_pivot_pub = nh->advertise("/arm_node/arm_positions/wrist_pivot_angle", 1); - wrist_joint_pub = nh->advertise("/arm_node/arm_positions/wrist_joint_angle", 1); - end_effector_pivot_pub = nh->advertise("/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: - void onUpdate() - { - 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() + M_PI_2; - // msg.end_effector_pivot_angle = model->GetJoint("end_effector_pivot")->Position(); - - // // 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(); - } - - physics::ModelPtr model; - event::ConnectionPtr updateConnection; - std::unique_ptr 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 diff --git a/rrrobot_ws/src/simulation_env/src/arm_motors.cpp b/rrrobot_ws/src/simulation_env/src/arm_motors.cpp deleted file mode 100644 index f4fa526..0000000 --- a/rrrobot_ws/src/simulation_env/src/arm_motors.cpp +++ /dev/null @@ -1,117 +0,0 @@ -#include -#include -#include -#include - -#include "ros/ros.h" -#include "simulation_env/arm_command.h" -#include - -using std::cout; -using std::endl; - -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); - - 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); - 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 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 diff --git a/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp b/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp deleted file mode 100644 index 4c22b67..0000000 --- a/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp +++ /dev/null @@ -1,78 +0,0 @@ -#include -#include "gazebo/physics/physics.hh" -#include "gazebo/common/common.hh" -#include "gazebo/gazebo.hh" - -#include "ros/ros.h" -#include -#include "simulation_env/model_insertion.h" - -#include -#include -#include - -namespace gazebo -{ -class ModelInsertion : public WorldPlugin -{ -public: - void Load(physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/) - { - std::cout << "Loading model insertion plugin" << std::endl; - - // Option 1: Insert model from file via function call. - // The filename must be in the GAZEBO_MODEL_PATH environment variable. - parent = _parent; //->InsertModelFile("model://box"); - - if (!ros::isInitialized()) - { - int argc = 0; - char **argv = NULL; - ros::init(argc, argv, "arm"); - } - - nh.reset(new ros::NodeHandle()); - subscriber = nh->subscribe("object_to_load", 1000, &ModelInsertion::insertModel, this); - } - - void insertModel(const simulation_env::model_insertion &msg) - { - std::cout << "Received message to load model: " << msg.model_name << std::endl; - - // Create a new transport node - transport::NodePtr node(new transport::Node()); - - // Initialize the node with the world name - node->Init(parent->Name()); - - // Create a publisher on the ~/factory topic - transport::PublisherPtr factoryPub = - node->Advertise("~/factory"); - - // Create the message - msgs::Factory to_pub; - - // Model file to load - to_pub.set_sdf_filename(std::string("model://") + msg.model_name); - - // Pose to initialize the model to - msgs::Set(to_pub.mutable_pose(), - ignition::math::Pose3d( - ignition::math::Vector3d(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z), - ignition::math::Quaterniond(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w))); - - // Send the message - factoryPub->Publish(to_pub); - - // parent->InsertModelFile(std::string("model://") + msg.data); - } - -private: - physics::WorldPtr parent; - std::unique_ptr nh; - ros::Subscriber subscriber; -}; - -// Register this plugin with the simulator -GZ_REGISTER_WORLD_PLUGIN(ModelInsertion) -} // namespace gazebo \ No newline at end of file diff --git a/rrrobot_ws/src/simulation_env/test/move_arm.cpp b/rrrobot_ws/src/simulation_env/test/move_arm.cpp deleted file mode 100644 index abd0c46..0000000 --- a/rrrobot_ws/src/simulation_env/test/move_arm.cpp +++ /dev/null @@ -1,70 +0,0 @@ -#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 diff --git a/rrrobot_ws/src/simulation_env/test/test_insert_object.cpp b/rrrobot_ws/src/simulation_env/test/test_insert_object.cpp deleted file mode 100644 index d30df7a..0000000 --- a/rrrobot_ws/src/simulation_env/test/test_insert_object.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include "simulation_env/model_insertion.h" -#include "ros/ros.h" - -#include - -using std::cin; -using std::cout; -using std::endl; - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "insert_models"); - - ros::NodeHandle nh; - - ros::Publisher pub = nh.advertise("/object_to_load", 1000); - - while (ros::ok()) - { - simulation_env::model_insertion msg; - cout << "Enter the model name: "; - cin >> msg.model_name; - cout << "Enter the pose: " << endl; - cout << "x: "; - cin >> msg.pose.position.x; - cout << "y: "; - cin >> msg.pose.position.y; - cout << "z: "; - cin >> msg.pose.position.z; - - pub.publish(msg); - ros::spinOnce(); - } -} \ No newline at end of file diff --git a/rrrobot_ws/world/rrrobot.world b/rrrobot_ws/world/rrrobot.world deleted file mode 100644 index 1032fdc..0000000 --- a/rrrobot_ws/world/rrrobot.world +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - model://ground_plane - - - - model://sun - - - - model://fanuc_robotic_arm_with_gripper - - - - -