diff --git a/docker_env/gazebo/Dockerfile b/docker_env/gazebo/Dockerfile index 5f799a1..f6fecd2 100644 --- a/docker_env/gazebo/Dockerfile +++ b/docker_env/gazebo/Dockerfile @@ -17,6 +17,7 @@ RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc RUN apt install -y python-rosdep RUN rosdep init RUN rosdep update +RUN apt-get install -y ros-melodic-pid # user id 1000 should be the same as the host user, so that you can access files # from inside the docker container and also on the host RUN useradd -u 1000 rrrobot diff --git a/rrrobot_src/src/arm_control/CMakeLists.txt b/rrrobot_src/src/arm_control/CMakeLists.txt new file mode 100644 index 0000000..3ac02e5 --- /dev/null +++ b/rrrobot_src/src/arm_control/CMakeLists.txt @@ -0,0 +1,268 @@ +cmake_minimum_required(VERSION 2.8.3) +project(arm_control) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11;-g3) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + geometry_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +find_package(orocos_kdl) +find_package(kdl_parser) + +# sdfconfig +find_package(PkgConfig REQUIRED) +pkg_check_modules(SDF sdformat REQUIRED) + +find_package(ignition-math4 REQUIRED) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include ../simulation_env/include +# LIBRARIES arm_control +# CATKIN_DEPENDS roscpp std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include/arm_control + ${catkin_INCLUDE_DIRS} + ${roscpp_INCLUDE_DIRS} + /usr/include/bullet + /usr/include/sdformat-6.2 + /usr/include/ignition/math4 + /home/rrrobot/rrrobot_src/devel/include/ +) +include_directories( + /usr/include/eigen3 +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/arm_control.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node src/arm_controller.cpp src/arm.cpp) +add_executable(test test/test.cpp src/arm.cpp) +add_executable(arm_movement_demo test/arm_movement_demo.cpp src/arm.cpp) +add_executable(pub_sub test/test_pub_sub.cpp) +add_executable(angles_subscriber test/angles_subscriber.cpp src/arm.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} + ${kdl_parser_LIBRARIES} + ${orocos_kdl_LIBRARIES} + ${SDF_LIBRARIES} + ignition-math4::ignition-math4 +) + +target_link_libraries(test + ${catkin_LIBRARIES} + ${kdl_parser_LIBRARIES} + ${orocos_kdl_LIBRARIES} + ${SDF_LIBRARIES} + ignition-math4::ignition-math4 +) + +target_link_libraries(arm_movement_demo + ${catkin_LIBRARIES} + ${kdl_parser_LIBRARIES} + ${orocos_kdl_LIBRARIES} + ${SDF_LIBRARIES} + ignition-math4::ignition-math4 +) + +target_link_libraries(pub_sub + ${catkin_LIBRARIES} + ${kdl_parser_LIBRARIES} + ${orocos_kdl_LIBRARIES} + ${SDF_LIBRARIES} + ignition-math4::ignition-math4 +) + +target_link_libraries(angles_subscriber + ${catkin_LIBRARIES} + ${kdl_parser_LIBRARIES} + ${orocos_kdl_LIBRARIES} + ${SDF_LIBRARIES} + ignition-math4::ignition-math4 +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +install(TARGETS ${PROJECT_NAME}_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +catkin_add_gtest(${PROJECT_NAME}-test test/test_internal_arm_representation.cpp src/arm.cpp) +target_link_libraries(${PROJECT_NAME}-test + ${catkin_LIBRARIES} + ${kdl_parser_LIBRARIES} + ${orocos_kdl_LIBRARIES} + ${SDF_LIBRARIES} + ignition-math4::ignition-math4 +) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/rrrobot_src/src/arm_control/include/arm_control/arm.h b/rrrobot_src/src/arm_control/include/arm_control/arm.h new file mode 100644 index 0000000..a54abfb --- /dev/null +++ b/rrrobot_src/src/arm_control/include/arm_control/arm.h @@ -0,0 +1,47 @@ +#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_src/src/arm_control/launch/arm.launch b/rrrobot_src/src/arm_control/launch/arm.launch new file mode 100644 index 0000000..0ead542 --- /dev/null +++ b/rrrobot_src/src/arm_control/launch/arm.launch @@ -0,0 +1,123 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rrrobot_src/src/arm_control/package.xml b/rrrobot_src/src/arm_control/package.xml new file mode 100644 index 0000000..c11d1c4 --- /dev/null +++ b/rrrobot_src/src/arm_control/package.xml @@ -0,0 +1,65 @@ + + + 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_src/src/arm_control/src/arm.cpp b/rrrobot_src/src/arm_control/src/arm.cpp new file mode 100644 index 0000000..3a8a6f3 --- /dev/null +++ b/rrrobot_src/src/arm_control/src/arm.cpp @@ -0,0 +1,203 @@ +#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_src/src/arm_control/src/arm_controller.cpp b/rrrobot_src/src/arm_control/src/arm_controller.cpp new file mode 100644 index 0000000..5fdd735 --- /dev/null +++ b/rrrobot_src/src/arm_control/src/arm_controller.cpp @@ -0,0 +1,128 @@ +#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_src/src/arm_control/test/angles_subscriber.cpp b/rrrobot_src/src/arm_control/test/angles_subscriber.cpp new file mode 100644 index 0000000..0dedd1b --- /dev/null +++ b/rrrobot_src/src/arm_control/test/angles_subscriber.cpp @@ -0,0 +1,153 @@ +#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_src/src/arm_control/test/arm_movement_demo.cpp b/rrrobot_src/src/arm_control/test/arm_movement_demo.cpp new file mode 100644 index 0000000..4c493ef --- /dev/null +++ b/rrrobot_src/src/arm_control/test/arm_movement_demo.cpp @@ -0,0 +1,47 @@ +#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_src/src/arm_control/test/test.cpp b/rrrobot_src/src/arm_control/test/test.cpp new file mode 100644 index 0000000..f8c2d2d --- /dev/null +++ b/rrrobot_src/src/arm_control/test/test.cpp @@ -0,0 +1,206 @@ +// 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_src/src/arm_control/test/test_internal_arm_representation.cpp b/rrrobot_src/src/arm_control/test/test_internal_arm_representation.cpp new file mode 100644 index 0000000..b5f4122 --- /dev/null +++ b/rrrobot_src/src/arm_control/test/test_internal_arm_representation.cpp @@ -0,0 +1,128 @@ +#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_src/src/arm_control/test/test_pub_sub.cpp b/rrrobot_src/src/arm_control/test/test_pub_sub.cpp new file mode 100644 index 0000000..3d93180 --- /dev/null +++ b/rrrobot_src/src/arm_control/test/test_pub_sub.cpp @@ -0,0 +1,97 @@ +#include "ros/ros.h" +#include "std_msgs/Float64.h" + +#include + +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_src/src/gazebo_models/unit_box/model.config b/rrrobot_src/src/gazebo_models/unit_box/model.config new file mode 100644 index 0000000..2a703a6 --- /dev/null +++ b/rrrobot_src/src/gazebo_models/unit_box/model.config @@ -0,0 +1,11 @@ + + + unit_box + 1.0 + model.sdf + + + + + + diff --git a/rrrobot_src/src/gazebo_models/unit_box/model.sdf b/rrrobot_src/src/gazebo_models/unit_box/model.sdf new file mode 100644 index 0000000..1c42598 --- /dev/null +++ b/rrrobot_src/src/gazebo_models/unit_box/model.sdf @@ -0,0 +1,104 @@ + + + + + + 0.208095 + + 0.0180922 + 0 + 0 + 0.0180922 + 0 + 0.0346825 + + 0 0 0 0 -0 0 + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + + + 0.05 0.05 0.05 + + + + + + __default__ + + 1 0 0 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 0 0 0 -0 0 + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.05 0.05 0.05 + + + + + + 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 + 1 + + diff --git a/rrrobot_src/test.sh b/rrrobot_src/test.sh new file mode 100755 index 0000000..f0068c4 --- /dev/null +++ b/rrrobot_src/test.sh @@ -0,0 +1,2 @@ +source build.sh +catkin_make run_tests diff --git a/rrrobot_src/world/rrrobot_setpoint.world b/rrrobot_src/world/rrrobot_setpoint.world new file mode 100644 index 0000000..feacd48 --- /dev/null +++ b/rrrobot_src/world/rrrobot_setpoint.world @@ -0,0 +1,68 @@ + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + 65535 + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + model://fanuc_robotic_arm_with_gripper + + + diff --git a/rrrobot_ws/build.sh b/rrrobot_ws/build.sh index 6b4669d..912ee85 100644 --- a/rrrobot_ws/build.sh +++ b/rrrobot_ws/build.sh @@ -5,3 +5,4 @@ catkin_make && catkin_make install && source devel/setup.bash +export GAZEBO_RESOURCE_PATH=$(pwd)/world:$GAZEBO_RESOURCE_PATH diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf index 3bbaf31..7eb02fd 100644 --- a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf +++ b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf @@ -17,7 +17,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -116,7 +116,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -215,7 +215,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -314,7 +314,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -413,7 +413,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -512,7 +512,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -611,7 +611,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -704,7 +704,7 @@ -1.79769e+308 1.79769e+308 - -2000 + -20000000 -1 @@ -737,7 +737,7 @@ -1.79769e+308 1.79769e+308 - -2000 + -20000000 -1 @@ -770,7 +770,7 @@ -1.79769e+308 1.79769e+308 - -2000 + -20000000 -1 @@ -803,7 +803,7 @@ -1.79769e+308 1.79769e+308 - -2000 + -20000000 -1 @@ -836,7 +836,7 @@ -1.79769e+308 1.79769e+308 - -2000 + -20000000 -1 @@ -869,7 +869,7 @@ -1.79769e+308 1.79769e+308 - -2000 + -20000000 -1 diff --git a/rrrobot_ws/src/simulation_env/CMakeLists.txt b/rrrobot_ws/src/simulation_env/CMakeLists.txt index 5be384d..78f75d7 100644 --- a/rrrobot_ws/src/simulation_env/CMakeLists.txt +++ b/rrrobot_ws/src/simulation_env/CMakeLists.txt @@ -137,7 +137,8 @@ add_library(arm_motors SHARED ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(arm_angles ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(arm_motors ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context @@ -191,7 +192,7 @@ target_link_libraries(test_move_arm ## Mark libraries for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -install(TARGETS arm_angles +install(TARGETS arm_angles arm_motors # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION /home/rrrobot/rrrobot_src/lib/ # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} diff --git a/rrrobot_ws/src/simulation_env/msg/arm_angles.msg b/rrrobot_ws/src/simulation_env/msg/arm_angles.msg index a6e84c6..98d9a46 100644 --- a/rrrobot_ws/src/simulation_env/msg/arm_angles.msg +++ b/rrrobot_ws/src/simulation_env/msg/arm_angles.msg @@ -4,3 +4,10 @@ float32 elbow_joint_angle float32 wrist_pivot_angle float32 wrist_joint_angle float32 end_effector_pivot_angle + +float32 shoulder_pivot_velocity +float32 shoulder_joint_velocity +float32 elbow_joint_velocity +float32 wrist_pivot_velocity +float32 wrist_joint_velocity +float32 end_effector_pivot_velocity 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 index 806743b..eb4ab1b 100644 --- a/rrrobot_ws/src/simulation_env/src/arm_angle_sensor_plugin.cpp +++ b/rrrobot_ws/src/simulation_env/src/arm_angle_sensor_plugin.cpp @@ -2,9 +2,11 @@ #include #include #include +#include #include "ros/ros.h" #include "simulation_env/arm_angles.h" +#include using std::cout; using std::endl; @@ -29,7 +31,33 @@ public: } nh.reset(new ros::NodeHandle("arm_node")); - publisher = nh->advertise("arm_positions", 1); + // 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: @@ -37,16 +65,53 @@ private: { simulation_env::arm_angles msg; - // read in the joint angle for each joint in the arm - msg.shoulder_pivot_angle = model->GetJoint("shoulder_pivot")->Position(); - msg.shoulder_joint_angle = model->GetJoint("shoulder_joint")->Position(); - msg.elbow_joint_angle = model->GetJoint("elbow_joint")->Position(); - msg.wrist_pivot_angle = model->GetJoint("wrist_pivot")->Position(); - msg.wrist_joint_angle = model->GetJoint("wrist_joint")->Position(); - msg.end_effector_pivot_angle = model->GetJoint("end_effector_pivot")->Position(); + // // read in the joint angle for each joint in the arm + // msg.shoulder_pivot_angle = model->GetJoint("shoulder_pivot")->Position(); + // msg.shoulder_joint_angle = -model->GetJoint("shoulder_joint")->Position(); + // msg.elbow_joint_angle = -model->GetJoint("elbow_joint")->Position(); + // msg.wrist_pivot_angle = model->GetJoint("wrist_pivot")->Position(); + // msg.wrist_joint_angle = model->GetJoint("wrist_joint")->Position() + M_PI_2; + // msg.end_effector_pivot_angle = model->GetJoint("end_effector_pivot")->Position(); - // publish the updated sensor measurements - publisher.publish(msg); + // // read in the angular velocity for each joint + // msg.shoulder_pivot_velocity = model->GetJoint("shoulder_pivot")->GetVelocity(0); + // msg.shoulder_joint_velocity = -model->GetJoint("shoulder_joint")->GetVelocity(0); + // msg.elbow_joint_velocity = -model->GetJoint("elbow_joint")->GetVelocity(0); + // msg.wrist_pivot_velocity = model->GetJoint("wrist_pivot")->GetVelocity(0); + // msg.wrist_joint_velocity = model->GetJoint("wrist_joint")->GetVelocity(0); + // msg.end_effector_pivot_velocity = model->GetJoint("end_effector_pivot")->GetVelocity(0); + + // // publish the updated sensor measurements + // publisher.publish(msg); + // ros::spinOnce(); + + std_msgs::Float64 shoulder_pivot_angle; + shoulder_pivot_angle.data = model->GetJoint("shoulder_pivot")->Position(); //msg.shoulder_pivot_angle; + std_msgs::Float64 shoulder_joint_angle; + shoulder_joint_angle.data = model->GetJoint("shoulder_joint")->Position(); //msg.shoulder_joint_angle; + std_msgs::Float64 elbow_joint_angle; + elbow_joint_angle.data = model->GetJoint("elbow_joint")->Position(); //msg.elbow_joint_angle; + std_msgs::Float64 wrist_pivot_angle; + wrist_pivot_angle.data = model->GetJoint("wrist_pivot")->Position(); //msg.wrist_pivot_angle; + std_msgs::Float64 wrist_joint_angle; + wrist_joint_angle.data = model->GetJoint("wrist_joint")->Position(); //msg.wrist_joint_angle; + std_msgs::Float64 end_effector_pivot_angle; + end_effector_pivot_angle.data = model->GetJoint("end_effector_pivot")->Position(); //msg.end_effector_pivot_angle; + + // cout << "Publishing: " << shoulder_pivot_angle.data << '\t' << shoulder_joint_angle.data << '\t' << elbow_joint_angle.data << '\t' << wrist_pivot_angle.data << '\t' << wrist_joint_angle.data << '\t' << end_effector_pivot_angle.data << endl; + + shoulder_pivot_pub.publish(shoulder_pivot_angle); + // ros::spinOnce(); + shoulder_joint_pub.publish(shoulder_joint_angle); + // ros::spinOnce(); + elbow_joint_pub.publish(elbow_joint_angle); + // ros::spinOnce(); + wrist_pivot_pub.publish(wrist_pivot_angle); + // ros::spinOnce(); + wrist_joint_pub.publish(wrist_joint_angle); + // ros::spinOnce(); + end_effector_pivot_pub.publish(end_effector_pivot_angle); + // ros::spinOnce(); // make sure the message gets published ros::spinOnce(); @@ -56,6 +121,13 @@ private: event::ConnectionPtr updateConnection; std::unique_ptr 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 index 099f036..f4fa526 100644 --- a/rrrobot_ws/src/simulation_env/src/arm_motors.cpp +++ b/rrrobot_ws/src/simulation_env/src/arm_motors.cpp @@ -5,6 +5,7 @@ #include "ros/ros.h" #include "simulation_env/arm_command.h" +#include using std::cout; using std::endl; @@ -14,15 +15,63 @@ namespace gazebo class ArmControl : public ModelPlugin { public: + void shoulder_pivot_callback(const std_msgs::Float64 &cmd) + { + // update the torque applied to each joint when a message is received + model->GetJoint("shoulder_pivot")->SetForce(0, cmd.data); + + ros::spinOnce(); + } + + void shoulder_joint_callback(const std_msgs::Float64 &cmd) + { + // update the torque applied to each joint when a message is received + model->GetJoint("shoulder_joint")->SetForce(0, cmd.data); + + ros::spinOnce(); + } + + void elbow_joint_callback(const std_msgs::Float64 &cmd) + { + // update the torque applied to each joint when a message is received + model->GetJoint("elbow_joint")->SetForce(0, cmd.data); + + ros::spinOnce(); + } + + void wrist_pivot_callback(const std_msgs::Float64 &cmd) + { + // update the torque applied to each joint when a message is received + model->GetJoint("wrist_pivot")->SetForce(0, cmd.data); + + ros::spinOnce(); + } + + void wrist_joint_callback(const std_msgs::Float64 &cmd) + { + // update the torque applied to each joint when a message is received + model->GetJoint("wrist_joint")->SetForce(0, cmd.data); + + ros::spinOnce(); + } + + void end_effector_pivot_callback(const std_msgs::Float64 &cmd) + { + // update the torque applied to each joint when a message is received + model->GetJoint("end_effector_pivot")->SetForce(0, cmd.data); + + ros::spinOnce(); + } + void arm_command_callback(const simulation_env::arm_command &cmd) { // update the torque applied to each joint when a message is received - model->GetJoint("shoulder_pivot")->SetForce(0, cmd.shoulder_pivot_force); - model->GetJoint("shoulder_joint")->SetForce(0, cmd.shoulder_joint_force); - model->GetJoint("elbow_joint")->SetForce(0, cmd.elbow_joint_force); - model->GetJoint("wrist_pivot")->SetForce(0, cmd.wrist_pivot_force); - model->GetJoint("wrist_joint")->SetForce(0, cmd.wrist_joint_force); - model->GetJoint("end_effector_pivot")->SetForce(0, cmd.end_effector_pivot_force); + // model->GetJoint("shoulder_pivot")->SetForce(0, cmd.shoulder_pivot_force); + // model->GetJoint("shoulder_joint")->SetForce(0, -cmd.shoulder_joint_force); + // model->GetJoint("elbow_joint")->SetForce(0, cmd.elbow_joint_force); + // model->GetJoint("wrist_pivot")->SetForce(0, cmd.wrist_pivot_force); + // model->GetJoint("wrist_joint")->SetForce(0, cmd.wrist_joint_force); + // model->GetJoint("end_effector_pivot")->SetForce(0, cmd.end_effector_pivot_force); ros::spinOnce(); } @@ -42,12 +91,27 @@ public: nh.reset(new ros::NodeHandle("arm_node")); subscriber = nh->subscribe("arm_commands", 1000, &ArmControl::arm_command_callback, this); + shoulder_pivot_sub = nh->subscribe("/arm_node/arm_commands/shoulder_pivot_torque", 1000, &ArmControl::shoulder_pivot_callback, this); + shoulder_joint_sub = nh->subscribe("/arm_node/arm_commands/shoulder_joint_torque", 1000, &ArmControl::shoulder_joint_callback, this); + elbow_joint_sub = nh->subscribe("/arm_node/arm_commands/elbow_joint_torque", 1000, &ArmControl::elbow_joint_callback, this); + wrist_pivot_sub = nh->subscribe("/arm_node/arm_commands/wrist_pivot_torque", 1000, &ArmControl::wrist_pivot_callback, this); + wrist_joint_sub = nh->subscribe("/arm_node/arm_commands/wrist_joint_torque", 1000, &ArmControl::wrist_joint_callback, this); + end_effector_sub = nh->subscribe("/arm_node/arm_commands/end_effector_pivot_torque", 1000, &ArmControl::end_effector_pivot_callback, this); + + cout << "Subscribed to all torque channels" << endl; } private: physics::ModelPtr model; std::unique_ptr 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