sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-24 16:52:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'GEAR' of github.com:EECS-467-W20-RRRobot-Project/RRRobot into GEAR

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-18 16:10:12 -04:00
parent 00c880cf3b 1f3e445903
commit 43909349e2
40 changed files with 640 additions and 3173 deletions
Download Patch File Download Diff File Expand all files Collapse all files

268
rrrobot_ws/src/arm_control/CMakeLists.txt
View File

@@ -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)

47
rrrobot_ws/src/arm_control/include/arm_control/arm.h
View File

@@ -1,47 +0,0 @@
#include <kdl/chain.hpp>
#include <kdl/segment.hpp>
#include <kdl/rigidbodyinertia.hpp>
#include <kdl/rotationalinertia.hpp>
#include <kdl/joint.hpp>
#include <kdl/frames.hpp>
#include <LinearMath/btTransform.h>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <memory>
#include <string>
class Arm
{
public:
/*
* sdf_file: this is the sdf file that defines the model. This
* class will use that description of the robot to model
* its dynamics.
*/
Arm(const std::string &sdf_file);
/*
* Using the provided joint positions, this calculates the position of the end of the
* arm. Returns whether it was successful
*/
bool calculateForwardKinematics(KDL::JntArray joint_positions, KDL::Frame &end_effector_frame);
bool calculateInverseKinematics(const KDL::JntArray &cur_configuration, const KDL::Frame &desired_end_effector_frame, KDL::JntArray &final_configuration);
int calculateInverseDynamics(const KDL::JntArray &pos, const KDL::JntArray &vel, const KDL::JntArray &accel, const KDL::Wrenches &f_external, KDL::JntArray &required_torque);
const KDL::Chain &getArm() const;
void print() const;
private:
KDL::Chain arm;
// Create solver based on kinematic chain
std::unique_ptr<KDL::ChainFkSolverPos_recursive> fksolver;
std::unique_ptr<KDL::ChainIkSolverPos_LMA> iksolver;
std::unique_ptr<KDL::ChainIdSolver_RNE> idsolver;
};

123
rrrobot_ws/src/arm_control/launch/arm.launch
View File

@@ -1,123 +0,0 @@
<launch>
<node name="shoulder_pivot_pid" pkg="pid" type="controller" output="screen" >
<param name="Kp" value="450.0" />
<param name="Ki" value="5.0" />
<param name="Kd" value="600.0" />
<param name="upper_limit" value="10000" />
<param name="lower_limit" value="-10000" />
<param name="windup_limit" value="1000" />
<param name="cutoff_frequency" value="20" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<param name="setpoint_timeout" value="-1.0" />
<param name="topic_from_controller" value="/arm_node/arm_commands/shoulder_pivot_torque" />
<param name="topic_from_plant" value="/arm_node/arm_positions/shoulder_pivot_angle" />
<param name="setpoint_topic" value="/arm_node/shoulder_pivot_setpoint" />
</node>
<node name="shoulder_joint_pid" pkg="pid" type="controller" output="screen" >
<param name="Kp" value="1200.0" />
<param name="Ki" value="40.0" />
<param name="Kd" value="1400.0" />
<param name="upper_limit" value="10000" />
<param name="lower_limit" value="-10000" />
<param name="windup_limit" value="1000" />
<param name="cutoff_frequency" value="20" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<param name="setpoint_timeout" value="-1.0" />
<param name="topic_from_controller" value="/arm_node/arm_commands/shoulder_joint_torque" />
<param name="topic_from_plant" value="/arm_node/arm_positions/shoulder_joint_angle" />
<param name="setpoint_topic" value="/arm_node/shoulder_joint_setpoint" />
</node>
<node name="elbow_joint_pid" pkg="pid" type="controller" output="screen" >
<param name="Kp" value="400.0" />
<param name="Ki" value="15.0" />
<param name="Kd" value="400.0" />
<param name="upper_limit" value="10000" />
<param name="lower_limit" value="-10000" />
<param name="windup_limit" value="1000" />
<param name="cutoff_frequency" value="20" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<param name="setpoint_timeout" value="-1.0" />
<param name="topic_from_controller" value="/arm_node/arm_commands/elbow_joint_torque" />
<param name="topic_from_plant" value="/arm_node/arm_positions/elbow_joint_angle" />
<param name="setpoint_topic" value="/arm_node/elbow_joint_setpoint" />
</node>
<node name="wrist_pivot_pid" pkg="pid" type="controller" output="screen" >
<param name="Kp" value="3.0" />
<param name="Ki" value="0.5" />
<param name="Kd" value="1.0" />
<param name="upper_limit" value="1000" />
<param name="lower_limit" value="-1000" />
<param name="windup_limit" value="100" />
<param name="cutoff_frequency" value="20" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<param name="setpoint_timeout" value="-1.0" />
<param name="topic_from_controller" value="/arm_node/arm_commands/wrist_pivot_torque" />
<param name="topic_from_plant" value="/arm_node/arm_positions/wrist_pivot_angle" />
<param name="setpoint_topic" value="/arm_node/wrist_pivot_setpoint" />
</node>
<node name="wrist_joint_pid" pkg="pid" type="controller" output="screen" >
<param name="Kp" value="2.0" />
<param name="Ki" value="0.5" />
<param name="Kd" value="1.0" />
<param name="upper_limit" value="1000" />
<param name="lower_limit" value="-1000" />
<param name="windup_limit" value="100" />
<param name="cutoff_frequency" value="20" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<param name="setpoint_timeout" value="-1.0" />
<param name="topic_from_controller" value="/arm_node/arm_commands/wrist_joint_torque" />
<param name="topic_from_plant" value="/arm_node/arm_positions/wrist_joint_angle" />
<param name="setpoint_topic" value="/arm_node/wrist_joint_setpoint" />
</node>
<node name="end_effector_pivot_pid" pkg="pid" type="controller" output="screen" >
<param name="Kp" value="0.1" />
<param name="Ki" value="0.01" />
<param name="Kd" value="0.05" />
<param name="upper_limit" value="1000" />
<param name="lower_limit" value="-1000" />
<param name="windup_limit" value="100" />
<param name="cutoff_frequency" value="20" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<param name="setpoint_timeout" value="-1.0" />
<param name="topic_from_controller" value="/arm_node/arm_commands/end_effector_pivot_torque" />
<param name="topic_from_plant" value="/arm_node/arm_positions/end_effector_pivot_angle" />
<param name="setpoint_topic" value="/arm_node/end_effector_pivot_setpoint" />
</node>
<!-- <node name="servo_sim_node" pkg="pid" type="plant_sim" output="screen" >
<param name="plant_order" value="2" />
</node> -->
<node name="arm_controller" pkg="arm_control" type="arm_control_node" output="screen" />
<node name="arm_movement_demo" pkg="arm_control" type="arm_movement_demo" output="screen" />
<!-- rqt_plot is a resource hog, so if you're seeing high CPU usage, don't launch rqt_plot -->
<!-- node name="rqt_plot" pkg="rqt_plot" type="rqt_plot"
args="/control_effort/data /state/data /setpoint/data" -->
<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" />
<!--node name="rqt_robot_monitor" pkg="rqt_robot_monitor" type="rqt_robot_monitor" -->
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="rrrobot_setpoint.world"/> <!--Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
<arg name="paused" value="true"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="recording" value="false"/>
<arg name="debug" value="false"/>
</include>
</launch>

65
rrrobot_ws/src/arm_control/package.xml
View File

@@ -1,65 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>arm_control</name>
<version>0.0.0</version>
<description>The arm_control package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="dwitcpa@todo.todo">dwitcpa</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/arm_control</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

203
rrrobot_ws/src/arm_control/src/arm.cpp
View File

@@ -1,203 +0,0 @@
#include <arm.h>
#include <sdf/sdf.hh>
#include <ignition/math/Pose3.hh>
#include <LinearMath/btTransform.h>
#include <kdl/frames_io.hpp>
#include <iostream>
#include <sstream>
#include <unordered_map>
using std::cout;
using std::endl;
using std::string;
using std::stringstream;
using std::unordered_map;
using std::vector;
struct FrameInformation
{
string link_name; // link name
KDL::Joint joint; // joint information (name, type)
KDL::Frame link_frame; // link location (world coordinates)
KDL::Frame joint_frame; // joint location relative to parent(link_name)
float mass;
KDL::Vector com_location; // relative to frame origin
KDL::RotationalInertia rotational_inertia;
};
/*
* sdf_file: this is the sdf file that defines the model. This
* class will use that description of the robot to model
* its dynamics.
*/
Arm::Arm(const std::string &sdf_file)
{
// read sdf file
sdf::SDFPtr sdf(new sdf::SDF());
sdf::init(sdf);
sdf::readFile(sdf_file, sdf);
// get model
const sdf::ElementPtr root = sdf->Root();
const sdf::ElementPtr model = root->GetElement("model");
const string model_name = model->Get<string>("name");
cout << "Found " << model_name << " model" << endl;
unordered_map<string, FrameInformation> links;
unordered_map<string, string> link_ordering;
string first_link;
// start reading links and joints
sdf::ElementPtr link = model->GetElement("link");
while (link)
{
const string name(link->Get<string>("name"));
links[name].link_name = name;
// Transformation from world to link coordinates
stringstream frame_location(link->GetElement("pose")->GetValue()->GetAsString());
float x, y, z;
float roll, pitch, yaw;
frame_location >> x >> y >> z >> roll >> pitch >> yaw;
KDL::Rotation rotation = KDL::Rotation::RPY(roll, pitch, yaw);
KDL::Vector link_position(x, y, z);
links[name].link_frame = KDL::Frame(rotation, link_position);
const sdf::ElementPtr inertial_data = link->GetElement("inertial");
float mass = inertial_data->Get<float>("mass");
links[name].mass = mass;
// Get the center of mass for this link
stringstream inertial_data_ss(inertial_data->GetElement("pose")->GetValue()->GetAsString());
inertial_data_ss >> links[name].com_location[0] >> links[name].com_location[1] >> links[name].com_location[2];
links[name].com_location += links[name].link_frame.p;
const sdf::ElementPtr inertia = inertial_data->GetElement("inertia");
links[name].rotational_inertia = KDL::RotationalInertia(
inertia->Get<float>("ixx"),
inertia->Get<float>("iyy"),
inertia->Get<float>("izz"),
inertia->Get<float>("ixy"),
inertia->Get<float>("ixz"),
inertia->Get<float>("iyz"));
link = link->GetNextElement("link");
}
sdf::ElementPtr joint = model->GetElement("joint");
while (joint) // TODO(dwitcpa): add support for translational joints
{
const string name(joint->Get<string>("name"));
const string parent(joint->GetElement("parent")->GetValue()->GetAsString());
const string child(joint->GetElement("child")->GetValue()->GetAsString());
link_ordering[parent] = child;
if (parent == string("world"))
{
joint = joint->GetNextElement("joint");
continue;
}
stringstream axis(joint->GetElement("axis")->GetElement("xyz")->GetValue()->GetAsString());
int axis_num = 0;
int cur;
while (axis >> cur)
{
if (cur != 0)
break;
axis_num++;
}
KDL::Joint::JointType joint_type = KDL::Joint::JointType::RotAxis;
if (axis_num == 0)
joint_type = KDL::Joint::JointType::RotX;
else if (axis_num == 1)
joint_type = KDL::Joint::JointType::RotY;
else if (axis_num == 2)
joint_type = KDL::Joint::JointType::RotZ;
links[child].joint = KDL::Joint(name, joint_type);
stringstream pose_string(joint->GetElement("pose")->GetValue()->GetAsString());
float x, y, z;
float roll, pitch, yaw;
pose_string >> x >> y >> z >> roll >> pitch >> yaw;
KDL::Rotation frame_rotation = KDL::Rotation::RPY(roll, pitch, yaw);
KDL::Vector frame_location(x, y, z);
links[child].joint_frame = KDL::Frame(frame_rotation, frame_location);
joint = joint->GetNextElement("joint");
}
string cur_link_name = "world";
KDL::Frame prev_frame(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(0, 0, 0));
KDL::Joint::JointType to_set(KDL::Joint::JointType::None);
while (link_ordering.find(cur_link_name) != link_ordering.end())
{
cur_link_name = link_ordering[cur_link_name];
// get the world coordinates of the joint
KDL::Frame joint_in_world = links[cur_link_name].link_frame * links[cur_link_name].joint_frame;
KDL::Frame prev_frame_inverse = prev_frame.Inverse();
KDL::Frame joint_relative_to_prev = prev_frame.Inverse() * joint_in_world;
KDL::Vector com_in_prev = joint_relative_to_prev * links[cur_link_name].com_location;
KDL::Joint::JointType next(links[cur_link_name].joint.getType());
KDL::RigidBodyInertia inertia(links[cur_link_name].mass, com_in_prev /*links[cur_link_name].com_location*/, links[cur_link_name].rotational_inertia);
KDL::Segment to_add(links[cur_link_name].link_name, KDL::Joint(links[cur_link_name].joint.getName(), to_set), joint_relative_to_prev, inertia);
to_set = next;
arm.addSegment(to_add);
prev_frame = joint_in_world;
}
arm.addSegment(KDL::Segment(string("pseudo_link"), KDL::Joint("pseudo_joint", to_set), KDL::Frame::Identity(), KDL::RigidBodyInertia()));
cout << "# of arm segments: " << arm.getNrOfSegments() << "\t# of arm joints: " << arm.getNrOfJoints() << endl;
fksolver.reset(new KDL::ChainFkSolverPos_recursive(arm));
iksolver.reset(new KDL::ChainIkSolverPos_LMA(arm));
idsolver.reset(new KDL::ChainIdSolver_RNE(arm, KDL::Vector(0, 0, -9.81)));
}
/*
* Using the provided joint positions, this calculates the pose of the end of the
* arm. Returns whether it was successful
*/
bool Arm::calculateForwardKinematics(KDL::JntArray joint_positions, KDL::Frame &end_effector_frame)
{
return fksolver->JntToCart(joint_positions, end_effector_frame);
}
bool Arm::calculateInverseKinematics(const KDL::JntArray &cur_configuration, const KDL::Frame &desired_end_effector_frame, KDL::JntArray &final_configuration)
{
return iksolver->CartToJnt(cur_configuration, desired_end_effector_frame, final_configuration);
}
int Arm::calculateInverseDynamics(const KDL::JntArray &pos, const KDL::JntArray &vel, const KDL::JntArray &accel, const KDL::Wrenches &f_external, KDL::JntArray &required_torque)
{
return idsolver->CartToJnt(pos, vel, accel, f_external, required_torque);
}
const KDL::Chain &Arm::getArm() const
{
return arm;
}
void Arm::print() const
{
for (const KDL::Segment &seg : arm.segments)
{
cout << seg.getName() << endl;
cout << seg.getFrameToTip() << endl;
cout << seg.getJoint().getTypeName() << endl;
cout << "Inertial frame:" << endl;
cout << "\tmass: " << seg.getInertia().getMass() << endl;
cout << '\t' << seg.getInertia().getCOG() << endl;
cout << endl;
}
}

128
rrrobot_ws/src/arm_control/src/arm_controller.cpp
View File

@@ -1,128 +0,0 @@
#include <arm.h>
#include <iostream>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Float64.h>
#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include <stdio.h>
#include <vector>
#include "ros/ros.h"
#include <simulation_env/arm_command.h>
#include <simulation_env/arm_angles.h>
#include <simulation_env/arm_command.h>
using std::cout;
using std::endl;
using std::vector;
using namespace KDL;
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
KDL::Chain chain = arm.getArm();
KDL::JntArray pos(arm.getArm().getNrOfJoints());
ros::Publisher shoulder_pivot_publisher;
ros::Publisher shoulder_joint_publisher;
ros::Publisher elbow_joint_publisher;
ros::Publisher wrist_pivot_publisher;
ros::Publisher wrist_joint_publisher;
ros::Publisher end_effector_pivot_publisher;
void shoulder_pivot_callback(const std_msgs::Float64 &cmd)
{
pos(0) = cmd.data;
}
void shoulder_joint_callback(const std_msgs::Float64 &cmd)
{
pos(1) = cmd.data;
}
void elbow_joint_callback(const std_msgs::Float64 &cmd)
{
pos(2) = cmd.data;
}
void wrist_pivot_callback(const std_msgs::Float64 &cmd)
{
pos(3) = cmd.data;
}
void wrist_joint_callback(const std_msgs::Float64 &cmd)
{
pos(4) = cmd.data;
}
void end_effector_pivot_callback(const std_msgs::Float64 &cmd)
{
pos(5) = cmd.data;
}
void fixAngles(const KDL::JntArray &current_angles, KDL::JntArray &desired_angles)
{
// TODO: go clockwise or counter-clockwise depending on which is shorter
}
void endEffectorPoseTargetCallback(const geometry_msgs::Pose &msg)
{
KDL::JntArray required_angles(arm.getArm().getNrOfJoints());
KDL::SetToZero(required_angles);
KDL::Frame desired(KDL::Rotation::Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w),
KDL::Vector(msg.position.x, msg.position.y, msg.position.z));
arm.calculateInverseKinematics(pos, desired, required_angles);
fixAngles(pos, required_angles);
std_msgs::Float64 shoulder_pivot_setpoint;
shoulder_pivot_setpoint.data = required_angles(0);
std_msgs::Float64 shoulder_joint_setpoint;
shoulder_joint_setpoint.data = required_angles(1);
std_msgs::Float64 elbow_joint_setpoint;
elbow_joint_setpoint.data = required_angles(2);
std_msgs::Float64 wrist_pivot_setpoint;
wrist_pivot_setpoint.data = required_angles(3);
std_msgs::Float64 wrist_joint_setpoint;
wrist_joint_setpoint.data = required_angles(4);
std_msgs::Float64 end_effector_setpoint;
end_effector_setpoint.data = required_angles(5);
shoulder_pivot_publisher.publish(shoulder_pivot_setpoint);
shoulder_joint_publisher.publish(shoulder_joint_setpoint);
elbow_joint_publisher.publish(elbow_joint_setpoint);
wrist_pivot_publisher.publish(wrist_pivot_setpoint);
wrist_joint_publisher.publish(wrist_joint_setpoint);
end_effector_pivot_publisher.publish(end_effector_setpoint);
}
int main(int argc, char **argv)
{
cout << "Starting arm controller..." << endl;
ros::init(argc, argv, "arm_control_node");
ros::NodeHandle nh;
ros::Subscriber shoulder_pivot_sub = nh.subscribe("/arm_node/arm_positions/shoulder_pivot_angle", 1000, &shoulder_pivot_callback);
ros::Subscriber shoulder_joint_sub = nh.subscribe("/arm_node/arm_positions/shoulder_joint_angle", 1000, &shoulder_joint_callback);
ros::Subscriber elbow_joint_sub = nh.subscribe("/arm_node/arm_positions/elbow_joint_angle", 1000, &elbow_joint_callback);
ros::Subscriber wrist_pivot_sub = nh.subscribe("/arm_node/arm_positions/wrist_pivot_angle", 1000, &wrist_pivot_callback);
ros::Subscriber wrist_joint_sub = nh.subscribe("/arm_node/arm_positions/wrist_joint_angle", 1000, &wrist_joint_callback);
ros::Subscriber end_effector_sub = nh.subscribe("/arm_node/arm_positions/end_effector_pivot_angle", 1000, &end_effector_pivot_callback);
ros::Subscriber end_effector_pose_setpoint_sub = nh.subscribe("/arm_node/arm_pose_setpoint", 1000, &endEffectorPoseTargetCallback);
KDL::SetToZero(pos);
shoulder_pivot_publisher = nh.advertise<std_msgs::Float64>("/arm_node/shoulder_pivot_setpoint", 1000);
shoulder_joint_publisher = nh.advertise<std_msgs::Float64>("/arm_node/shoulder_joint_setpoint", 1000);
elbow_joint_publisher = nh.advertise<std_msgs::Float64>("/arm_node/elbow_joint_setpoint", 1000);
wrist_pivot_publisher = nh.advertise<std_msgs::Float64>("/arm_node/wrist_pivot_setpoint", 1000);
wrist_joint_publisher = nh.advertise<std_msgs::Float64>("/arm_node/wrist_joint_setpoint", 1000);
end_effector_pivot_publisher = nh.advertise<std_msgs::Float64>("/arm_node/end_effector_pivot_setpoint", 1000);
ros::spin();
}

153
rrrobot_ws/src/arm_control/test/angles_subscriber.cpp
View File

@@ -1,153 +0,0 @@
#include <std_msgs/Float64.h>
#include "ros/ros.h"
#include <ros/rate.h>
#include <arm.h>
#include <iostream>
#include <vector>
#include <cmath>
using std::cout;
using std::endl;
using std::vector;
std_msgs::Float64 shoulder_pivot_angle;
std_msgs::Float64 shoulder_joint_angle;
std_msgs::Float64 elbow_joint_angle;
std_msgs::Float64 wrist_pivot_angle;
std_msgs::Float64 wrist_joint_angle;
std_msgs::Float64 end_effector_pivot_angle;
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
KDL::JntArray pos(arm.getArm().getNrOfJoints());
char received_angles = 0;
void shoulder_pivot_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
shoulder_pivot_angle = cmd;
pos(0) = cmd.data;
cout << "shoulder pivot: " << shoulder_pivot_angle.data << endl;
received_angles |= 0x01;
}
void shoulder_joint_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
shoulder_joint_angle = cmd;
pos(1) = cmd.data;
cout << "shoulder joint: " << shoulder_joint_angle.data << endl;
received_angles |= 0x02;
}
void elbow_joint_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
elbow_joint_angle = cmd;
pos(2) = cmd.data;
cout << "elbow joint: " << elbow_joint_angle.data << endl;
received_angles |= 0x04;
}
void wrist_pivot_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
wrist_pivot_angle = cmd;
pos(3) = cmd.data;
cout << "wrist pivot: " << wrist_pivot_angle.data << endl;
received_angles |= 0x08;
}
void wrist_joint_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
wrist_joint_angle = cmd;
pos(4) = cmd.data;
cout << "wrist joint: " << wrist_joint_angle.data << endl;
received_angles |= 0x10;
}
void end_effector_pivot_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
end_effector_pivot_angle = cmd;
pos(5) = cmd.data;
cout << "end effector pivot: " << end_effector_pivot_angle.data << endl;
received_angles |= 0x20;
}
void fix_angle(double cur_pos, double &desired)
{
while (desired > M_PI)
{
desired -= 2 * M_PI;
}
while (desired < -M_PI)
{
desired += 2 * M_PI;
}
double cur_pos_truncated = cur_pos; // % (M_PI);
while (cur_pos_truncated > M_PI)
{
cur_pos_truncated -= 2 * M_PI;
}
while (cur_pos_truncated < -M_PI)
{
cur_pos_truncated += 2 * M_PI;
}
// say desired = 1.5708
// 4 cases:
// 1. cur_pos = 0.2 --> set target as cur_pos + 1.3708
// 2. cur_pos = -0.2 --> set target as cur_pos + 1.7708
// 3. cur_pos = -2.5 --> set target as cur_pos + 2.21239
// 4. cur_pos = 2.5 --> set target as cur_pos + 0.9292
if ((cur_pos_truncated - desired) < M_PI && (cur_pos_truncated - desired) > 0)
{
desired = cur_pos - (cur_pos_truncated - desired);
}
else if ((desired - cur_pos_truncated) < M_PI && (desired - cur_pos_truncated) > 0)
{
desired = cur_pos + (desired - cur_pos_truncated);
}
else if ((cur_pos_truncated - desired) > -M_PI && (cur_pos_truncated - desired) < 0)
{
desired = cur_pos - (cur_pos_truncated - desired);
}
else if ((desired - cur_pos_truncated) > -M_PI && (desired - cur_pos_truncated) < 0)
{
desired = cur_pos + (desired - cur_pos_truncated);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "arm_control_demo");
ros::Subscriber shoulder_pivot_sub;
ros::Subscriber shoulder_joint_sub;
ros::Subscriber elbow_joint_sub;
ros::Subscriber wrist_pivot_sub;
ros::Subscriber wrist_joint_sub;
ros::Subscriber end_effector_sub;
ros::NodeHandle nh;
ros::Rate pub_rate(0.25); // publish every 10 seconds
ros::Rate quick(1);
shoulder_pivot_sub = nh.subscribe("/arm_node/arm_positions/shoulder_pivot_angle", 1000, &shoulder_pivot_callback);
shoulder_joint_sub = nh.subscribe("/arm_node/arm_positions/shoulder_joint_angle", 1000, &shoulder_joint_callback);
elbow_joint_sub = nh.subscribe("/arm_node/arm_positions/elbow_joint_angle", 1000, &elbow_joint_callback);
wrist_pivot_sub = nh.subscribe("/arm_node/arm_positions/wrist_pivot_angle", 1000, &wrist_pivot_callback);
wrist_joint_sub = nh.subscribe("/arm_node/arm_positions/wrist_joint_angle", 1000, &wrist_joint_callback);
end_effector_sub = nh.subscribe("/arm_node/arm_positions/end_effector_pivot_angle", 1000, &end_effector_pivot_callback);
ros::spin();
// while (ros::ok())
// {
// quick.sleep();
// ros::spinOnce();
// }
}

47
rrrobot_ws/src/arm_control/test/arm_movement_demo.cpp
View File

@@ -1,47 +0,0 @@
#include <geometry_msgs/Pose.h>
#include "ros/ros.h"
#include <ros/rate.h>
#include <arm.h>
#include <iostream>
#include <vector>
#include <cmath>
using std::cout;
using std::endl;
using std::vector;
int main(int argc, char **argv)
{
ros::init(argc, argv, "arm_control_demo");
ros::NodeHandle nh;
ros::Rate pub_rate(0.1); // publish every 10 seconds
ros::Publisher end_effector_setpoint_publisher = nh.advertise<geometry_msgs::Pose>("/arm_node/arm_pose_setpoint", 1000);
int state = 0;
vector<KDL::Frame> desired_positions;
desired_positions.push_back(KDL::Frame(KDL::Rotation::RPY(-2.35619, 0.7853, 0.0), KDL::Vector(1, 1, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -1, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -1, 2)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 1, 3)));
while (ros::ok())
{
const KDL::Frame &cur_setpoint = desired_positions[state];
geometry_msgs::Pose msg;
msg.position.x = cur_setpoint.p.x();
msg.position.y = cur_setpoint.p.y();
msg.position.z = cur_setpoint.p.z();
cur_setpoint.M.GetQuaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w);
end_effector_setpoint_publisher.publish(msg);
ros::spinOnce();
state = (state + 1) % desired_positions.size();
pub_rate.sleep();
}
}

206
rrrobot_ws/src/arm_control/test/test.cpp
View File

@@ -1,206 +0,0 @@
// Copyright (C) 2007 Francois Cauwe <francois at cauwe dot org>
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include <stdio.h>
#include <iostream>
#include <arm.h>
#include <fstream>
#include <string>
#include <vector>
using namespace KDL;
using std::cin;
using std::cout;
using std::endl;
using std::ofstream;
using std::string;
using std::vector;
int main(int argc, char **argv)
{
//Definition of a kinematic chain & add segments to the chain
Arm arm(/*"/home/rrrobot/rrrobot_src/src/gazebo_models/basic_arm/model.sdf"); /(*/ "/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
//KDL::Chain chain = arm.getArm();
// chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.0))));
// chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1))));
// chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1))));
// chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.480))));
// chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.645))));
// chain.addSegment(Segment(Joint(Joint::RotZ)));
// chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.120))));
// chain.addSegment(Segment(Joint(Joint::RotZ)));
// Create solver based on kinematic chain
//ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);
// Create joint array
unsigned int nj = arm.getArm().getNrOfJoints();
KDL::JntArray jointpositions = JntArray(nj);
for (int pos = 0; pos < nj; ++pos)
{
jointpositions(pos) = 0;
}
// // Assign some values to the joint positions for (unsigned int i = 0; i < nj; i++)
// for (int i = 0; i < nj; ++i)
// {
// float myinput;
// printf("Enter the position of joint %i: ", i);
// scanf("%e", &myinput);
// jointpositions(i) = (double)myinput;
// }
// // Create the frame that will contain the results
// KDL::Frame cartpos;
// // Calculate forward position kinematics
// bool kinematics_status;
// kinematics_status = fksolver.JntToCart(jointpositions, cartpos);
// if (kinematics_status >= 0)
// {
// // cout << segments << endl;
// std::cout << cartpos << std::endl;
// // printf("%s \n", "Succes, thanks KDL!");
// }
// else
// {
// // printf("%s \n", "Error: could not calculate forward kinematics :(");
// }
// while (1)
// {
// cout << "Enter the shoulder_pivot angle: ";
// cin >> jointpositions(0);
// cout << "Enter the shoulder_joint angle: ";
// cin >> jointpositions(1);
// cout << "Enter the elbow_joint angle: ";
// cin >> jointpositions(2);
// cout << "Enter the wrist_pivot angle: ";
// cin >> jointpositions(3);
// cout << "Enter the wrist_joint angle: ";
// cin >> jointpositions(4);
// cout << "Enter the end_effector_pivot angle: ";
// cin >> jointpositions(5);
// Create the frame that will contain the results
KDL::Frame cartpos;
ofstream f_raw("configurations.txt");
ofstream f_corrected("corrected_configurations.txt");
int which_joint;
cout << "Which joint should be moved (0: shoulder pivot, 5: end effector pivot): ";
cin >> which_joint;
for (double shoulder_pivot = 0.0; shoulder_pivot <= 6.28; shoulder_pivot += 0.1)
{
// Calculate forward position kinematics
bool kinematics_status;
// for (int segments = 0; segments <= chain.getNrOfSegments(); ++segments)
// {
// kinematics_status = fksolver.JntToCart(jointpositions, cartpos, segments);
// if (kinematics_status >= 0)
// {
// cout << segments << endl;
// std::cout << cartpos << std::endl;
// // printf("%s \n", "Succes, thanks KDL!");
// }
// else
// {
// // printf("%s \n", "Error: could not calculate forward kinematics :(");
// }
// }
jointpositions(which_joint) = shoulder_pivot;
kinematics_status = arm.calculateForwardKinematics(jointpositions, cartpos); //fksolver.JntToCart(jointpositions, cartpos);
cout << "Final position: " << endl;
cout << cartpos << endl;
KDL::Vector pos = cartpos.p;
f_raw << pos.x() << "," << pos.y() << "," << pos.z() << "\n";
KDL::Vector corrected = cartpos.M * cartpos.p;
f_corrected << corrected.x() << "," << corrected.y() << "," << corrected.z() << "\n";
}
// }
KDL::Frame desired(KDL::Vector(0.000767, -1.87014, 2.0658));
KDL::JntArray final_config(arm.getArm().getNrOfJoints());
bool error = arm.calculateInverseKinematics(jointpositions, desired, final_config);
if (error == false)
{
for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint)
{
cout << final_config(joint) << endl;
}
}
else
{
cout << "Failed to find configuration" << endl;
}
KDL::JntArray vel(arm.getArm().getNrOfJoints());
KDL::JntArray accel(arm.getArm().getNrOfJoints());
KDL::Wrenches ext_force(arm.getArm().getNrOfSegments());
KDL::JntArray required_force(arm.getArm().getNrOfJoints());
int error_val = arm.calculateInverseDynamics(jointpositions, vel, accel, ext_force, required_force);
if (error_val == 0)
{
for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint)
{
cout << required_force(joint) << endl;
}
}
else
{
cout << "Failed to find required torques, error #" << error_val << endl;
}
// record required torques for a variety of joint positions
KDL::JntArray pos(arm.getArm().getNrOfJoints());
for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint)
{
pos(joint) = 0;
}
ofstream f("required_" + std::to_string(which_joint) + "_torque.txt");
for (double angle = -3.14159; angle < 3.14159; angle += 0.1)
{
pos(which_joint) = angle;
arm.calculateInverseDynamics(pos, vel, accel, ext_force, required_force);
f << angle << "," << required_force(0) << "," << required_force(1) << "," << required_force(2) << "," << required_force(3) << "," << required_force(4) << "," << required_force(5) << "\n";
}
// KDL::JntArray pos(arm.getArm().getNrOfJoints());
KDL::JntArray required_angles(arm.getArm().getNrOfJoints());
KDL::SetToZero(pos);
KDL::SetToZero(required_angles);
vector<KDL::Frame> desired_positions;
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.5, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.4, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.3, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.2, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.1, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.0, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.1, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.2, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.3, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.4, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.5, 1)));
ofstream ik_f("inverse_kinematics.txt");
for (int state = 0; state < desired_positions.size(); ++state)
{
arm.calculateInverseKinematics(pos, desired_positions[state], required_angles);
ik_f << desired_positions[state].p.x() << "," << desired_positions[state].p.y() << "," << desired_positions[state].p.z() << ",";
ik_f << required_angles(0) << "," << required_angles(1) << "," << required_angles(2) << "," << required_angles(3) << "," << required_angles(4) << "," << required_angles(5) << "\n";
}
}

128
rrrobot_ws/src/arm_control/test/test_internal_arm_representation.cpp
View File

@@ -1,128 +0,0 @@
#include <arm.h>
#include "gtest/gtest.h"
#include <kdl/frames_io.hpp>
#include <kdl/chain.hpp>
#include <cmath>
const double ALLOWED_ERROR = 0.05;
double getDistance(const KDL::Vector &a, const KDL::Vector &b)
{
return sqrt(pow(a.x() - b.x(), 2) + pow(a.y() - b.y(), 2) + pow(a.z() - b.z(), 2));
}
void EXPECT_ROTATION_EQ(const KDL::Rotation &a, const KDL::Rotation &b)
{
EXPECT_TRUE(KDL::Equal(a, b, 0.005));
}
TEST(ArmRepresentationTests, shoulder_pivot)
{
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
// Create joint array
unsigned int nj = arm.getArm().getNrOfJoints();
KDL::JntArray jointpositions = KDL::JntArray(nj);
for (int pos = 0; pos < nj; ++pos)
{
jointpositions(pos) = 0;
}
// Create the frame that will contain the results
KDL::Frame cartpos;
KDL::Vector correct(0.000767, -1.87014, 2.0658);
jointpositions(0) = 0.0;
arm.calculateForwardKinematics(jointpositions, cartpos);
EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
jointpositions(0) = 2 * M_PI;
arm.calculateForwardKinematics(jointpositions, cartpos);
EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
jointpositions(0) = M_PI;
arm.calculateForwardKinematics(jointpositions, cartpos);
correct = KDL::Vector(0.000767, 1.87014, 2.0658);
EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
}
TEST(ArmRepresentationTests, elbow_pivot)
{
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
// Create joint array
unsigned int nj = arm.getArm().getNrOfJoints();
KDL::JntArray jointpositions = KDL::JntArray(nj);
for (int pos = 0; pos < nj; ++pos)
{
jointpositions(pos) = 0;
}
// Create the frame that will contain the results
KDL::Frame cartpos;
KDL::Vector correct(0.000767, -1.87014, 2.0658);
for (double pos = 0.0; pos <= M_2_PI; pos += 0.1)
{
jointpositions(3) = pos;
arm.calculateForwardKinematics(jointpositions, cartpos);
EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
}
}
TEST(ArmRepresentationTests, end_effector_pivot)
{
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
// Create joint array
unsigned int nj = arm.getArm().getNrOfJoints();
KDL::JntArray jointpositions = KDL::JntArray(nj);
for (int pos = 0; pos < nj; ++pos)
{
jointpositions(pos) = 0;
}
// Create the frame that will contain the results
KDL::Frame cartpos;
KDL::Vector correct(0.000767, -1.87014, 2.0658);
for (double pos = 0.0; pos <= M_2_PI; pos += 0.1)
{
jointpositions(5) = pos;
arm.calculateForwardKinematics(jointpositions, cartpos);
EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
}
}
TEST(ArmRepresentationTests, all_joints_rotated)
{
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
// Create joint array
unsigned int nj = arm.getArm().getNrOfJoints();
KDL::JntArray jointpositions = KDL::JntArray(nj);
jointpositions(0) = -12.0027;
jointpositions(1) = -1.86177;
jointpositions(2) = -0.991728;
jointpositions(3) = 39.5032;
jointpositions(4) = -1.45472;
jointpositions(5) = -25.4681;
// Create the frame that will contain the results
KDL::Frame cartpos;
KDL::Vector correct(-0.876742, 1.85761, 0.482547);
KDL::Rotation correct_rot = KDL::Rotation::RPY(-1.75917, -0.0162929, -0.823634);
arm.calculateForwardKinematics(jointpositions, cartpos);
EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
EXPECT_ROTATION_EQ(cartpos.M, correct_rot);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

97
rrrobot_ws/src/arm_control/test/test_pub_sub.cpp
View File

@@ -1,97 +0,0 @@
#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include <iostream>
using std::cout;
using std::endl;
void msg_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
cout << "received: " << cmd.data << endl;
}
/**
* This tutorial demonstrates simple sending of messages over the ROS system.
*/
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line.
* For programmatic remappings you can use a different version of init() which takes
* remappings directly, but for most command-line programs, passing argc and argv is
* the easiest way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
ros::init(argc, argv, "talker");
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
ros::NodeHandle n;
/**
* The advertise() function is how you tell ROS that you want to
* publish on a given topic name. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. After this advertise() call is made, the master
* node will notify anyone who is trying to subscribe to this topic name,
* and they will in turn negotiate a peer-to-peer connection with this
* node. advertise() returns a Publisher object which allows you to
* publish messages on that topic through a call to publish(). Once
* all copies of the returned Publisher object are destroyed, the topic
* will be automatically unadvertised.
*
* The second parameter to advertise() is the size of the message queue
* used for publishing messages. If messages are published more quickly
* than we can send them, the number here specifies how many messages to
* buffer up before throwing some away.
*/
ros::Publisher pub = n.advertise<std_msgs::Float64>("chatter", 1000);
ros::Subscriber sub = n.subscribe("chatter", 1000, &msg_callback);
ros::Rate loop_rate(10);
/**
* A count of how many messages we have sent. This is used to create
* a unique string for each message.
*/
int count = 0;
double data = -5e-5; //.1254987465413251e-5;
while (ros::ok())
{
/**
* This is a message object. You stuff it with data, and then publish it.
*/
std_msgs::Float64 msg;
msg.data = data;
cout << "Publishing: " << data << '\t' << msg.data << endl;
/**
* The publish() function is how you send messages. The parameter
* is the message object. The type of this object must agree with the type
* given as a template parameter to the advertise<>() call, as was done
* in the constructor above.
*/
pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
++count;
data += 1e-5;
if (data > 5.0)
{
data = -5.1254987465413251;
}
}
return 0;
}

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/arm_joint_and_link_definitions.jpg
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.9 MiB

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL
View File

Binary file not shown.

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL
View File

Binary file not shown.

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL
View File

Binary file not shown.

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL
View File

Binary file not shown.

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL
View File

Binary file not shown.

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL
View File

Binary file not shown.

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL
View File

Binary file not shown.

11
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.config
View File

@@ -1,11 +0,0 @@
<?xml version="1.0" ?>
<model>
<name>Fanuc_robot_arm</name>
<version>1.0</version>
<sdf version="1.6">model.sdf</sdf>
<author>
<name></name>
<email></email>
</author>
<description>This was created using parts from a CAD model on GrabCAD</description>
</model>

905
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf
View File

@@ -1,905 +0,0 @@
<?xml version='1.0'?>
<sdf version='1.6'>
<model name='Fanuc_robotic_arm'>
<link name='base'>
<pose frame=''>0 0 0 3.14159 -0 0</pose>
<inertial>
<mass>86.082</mass>
<inertia>
<ixx>4.3404</ixx>
<ixy>0</ixy>
<ixz>0.0020207</ixz>
<iyy>2.4866</iyy>
<iyz>0</iyz>
<izz>6.1574</izz>
</inertia>
<pose frame=''>-0.003895 -0.062844 -0.11068 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<link name='link_1'>
<pose frame=''>0.001715 0.003779 0.661683 1.5708 -0 0</pose>
<inertial>
<mass>266.27</mass>
<inertia>
<ixx>279.51</ixx>
<ixy>0</ixy>
<ixz>4.6477</ixz>
<iyy>43.386</iyy>
<iyz>34.265</iyz>
<izz>255.26</izz>
</inertia>
<pose frame=''>0.093634 -0.51398 -0.1614 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<link name='link_3'>
<pose frame=''>-0.00025 -0.94659 5.45119 1.5708 -0 0</pose>
<inertial>
<mass>63.612</mass>
<inertia>
<ixx>6.6124</ixx>
<ixy>0.099584</ixy>
<ixz>0</ixz>
<iyy>5.9295</iyy>
<iyz>0</iyz>
<izz>1.9517</izz>
</inertia>
<pose frame=''>0.00756 -3.4862 -0.40714 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<link name='link_2'>
<pose frame=''>-0.068455 0.275759 0.315754 -1.571 -0 0</pose>
<inertial>
<mass>60.795</mass>
<inertia>
<ixx>8.1939</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.76659</iyy>
<iyz>0</iyz>
<izz>8.0226</izz>
</inertia>
<pose frame=''>0.25344 -1.009 -0.57114 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<link name='link_4'>
<pose frame=''>-0.000257 -3.97477 2.07064 -1.5708 7e-06 3.14159</pose>
<inertial>
<mass>6.6264</mass>
<inertia>
<ixx>0.089718</ixx>
<ixy>0</ixy>
<ixz>0.017666</ixz>
<iyy>0.092881</iyy>
<iyz>3e-08</iyz>
<izz>0.028564</izz>
</inertia>
<pose frame=''>-0.028814 0 -2.454 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<link name='link_5'>
<pose frame=''>0.000743 -4.767 2.06498 -1.5708 7e-06 3.14159</pose>
<inertial>
<mass>5.1229</mass>
<inertia>
<ixx>0.038195</ixx>
<ixy>4e-07</ixy>
<ixz>0</ixz>
<iyy>0.066675</iyy>
<iyz>6e-07</iyz>
<izz>0.04821</izz>
</inertia>
<pose frame=''>-0.010371 1e-06 -3.0669 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<link name='link_6'>
<pose frame=''>-2.06426 -5.37015 4.13075 1.88426 1.57078 0.313465</pose>
<inertial>
<mass>0.72893</mass>
<inertia>
<ixx>0.00131</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0012978</iyy>
<iyz>0</iyz>
<izz>0.0024341</izz>
</inertia>
<pose frame=''>2.0657 2.0642 -3.5441 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<joint name='wrist_pivot' type='revolute'>
<parent>link_3</parent>
<child>link_4</child>
<pose frame=''>0 0 -2.7 0 -0 0</pose>
<axis>
<xyz>0 0 1</xyz>
<use_parent_model_frame>0</use_parent_model_frame>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-20000000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<joint name='shoulder_joint' type='revolute'>
<parent>link_1</parent>
<child>link_2</child>
<pose frame=''>0 -0.41 -0.61 0 -0 0</pose>
<axis>
<xyz>1 0 0</xyz>
<use_parent_model_frame>0</use_parent_model_frame>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-20000000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<joint name='elbow_joint' type='revolute'>
<parent>link_2</parent>
<child>link_3</child>
<pose frame=''>0 -3.65 -0.6 0 -0 0</pose>
<axis>
<xyz>1 0 0</xyz>
<use_parent_model_frame>0</use_parent_model_frame>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-20000000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<joint name='wrist_joint' type='revolute'>
<parent>link_4</parent>
<child>link_5</child>
<pose frame=''>0 0 -3.15 0 -0 0</pose>
<axis>
<xyz>1 0 0</xyz>
<use_parent_model_frame>0</use_parent_model_frame>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-20000000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<joint name='shoulder_pivot' type='revolute'>
<parent>base</parent>
<child>link_1</child>
<pose frame=''>0 0 0 0 -0 0</pose>
<axis>
<xyz>0 1 0</xyz>
<use_parent_model_frame>0</use_parent_model_frame>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-20000000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<joint name='end_effector_pivot' type='revolute'>
<parent>link_5</parent>
<child>link_6</child>
<pose frame=''>2.065 2.065 -3.5 0 -0 0</pose>
<axis>
<xyz>0 0 1</xyz>
<use_parent_model_frame>0</use_parent_model_frame>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-20000000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<joint name='world_fix' type='fixed'>
<parent>world</parent>
<child>base</child>
</joint>
<static>0</static>
<allow_auto_disable>0</allow_auto_disable>
<plugin name="joint_angles" filename="libarm_angles.so"/>
<plugin name="arm_control" filename="libarm_motors.so"/>
</model>
</sdf>

120
rrrobot_ws/src/rrrobot/CMakeLists.txt Normal file
View File

@@ -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)

163
rrrobot_ws/src/rrrobot/config/rrrobot.yaml Normal file
View File

@@ -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

36
rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml Normal file
View File

@@ -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']

28
rrrobot_ws/src/rrrobot/launch/rrrobot.launch Normal file
View File

@@ -0,0 +1,28 @@
<launch>
<arg name="verbose" default="false" />
<arg unless="$(arg verbose)" name="verbose_args" value="" />
<arg if="$(arg verbose)" name="verbose_args" value="--verbose" />
<arg name="state_logging" default="false" />
<arg unless="$(arg state_logging)" name="state_logging_args" value="" />
<arg if="$(arg state_logging)" name="state_logging_args" value="--state-logging=true" />
<arg name="no_gui" default="false" />
<arg unless="$(arg no_gui)" name="gui_args" value="" />
<arg if="$(arg no_gui)" name="gui_args" value="--no-gui" />
<arg name="fill_demo_shipment" default="false" />
<arg unless="$(arg fill_demo_shipment)" name="fill_demo_shipment_args" value="" />
<arg if="$(arg fill_demo_shipment)" name="fill_demo_shipment_args" value="--fill-demo-shipment" />
<node name="ariac_sim" pkg="osrf_gear" type="gear.py"
args="--development-mode
$(arg verbose_args)
$(arg state_logging_args)
$(arg gui_args)
$(arg fill_demo_shipment_args)
--visualize-sensor-views
-f /app/rrrobot_ws/src/rrrobot/config/rrrobot.yaml
/app/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml
" required="true" output="screen" />
</launch>

27
rrrobot_ws/src/rrrobot/package.xml Normal file
View File

@@ -0,0 +1,27 @@
<?xml version="1.0"?>
<package format="2">
<name>rrrobot</name>
<version>0.1.0</version>
<description>RRRobot package</description>
<maintainer email="balajsra@umich.edu">Sravan Balaji</maintainer>
<maintainer email="chenxgu@umich.edu">Chenxi Gu</maintainer>
<maintainer email="thejakej@umich.edu">Jake Johnson</maintainer>
<maintainer email="dwitcpa@umich.edu">Derek Witcpalek</maintainer>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>osrf_gear</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>std_srvs</depend>
<depend>tf2_geometry_msgs</depend>
<depend>trajectory_msgs</depend>
<depend>std_msgs</depend>
<depend>geometry_msgs</depend>
<depend>gazebo_ros</depend>
<depend>message_generation</depend>
<exec_depend>message_runtime</exec_depend>
</package>

5
rrrobot_ws/src/rrrobot/scripts/rrrobot_run.sh Normal file
View File

@@ -0,0 +1,5 @@
#!/bin/bash
sudo cp ../launch/rrrobot.launch /opt/ros/melodic/share/osrf_gear/launch
roslaunch osrf_gear rrrobot.launch

0
rrrobot_ws/src/gear/sample_run.sh → rrrobot_ws/src/rrrobot/scripts/sample_run.sh
View File

261
rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp Normal file
View File

@@ -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 <algorithm>
#include <vector>
#include <ros/ros.h>
#include <osrf_gear/LogicalCameraImage.h>
#include <osrf_gear/Order.h>
#include <osrf_gear/Proximity.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <std_srvs/Trigger.h>
#include <trajectory_msgs/JointTrajectory.h>
// %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<std_srvs::Trigger>("/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<trajectory_msgs::JointTrajectory>(
"/ariac/arm1/arm/command", 10);
arm_2_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(
"/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<osrf_gear::Order> 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)%

246
rrrobot_ws/src/simulation_env/CMakeLists.txt
View File

@@ -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)

13
rrrobot_ws/src/simulation_env/msg/arm_angles.msg
View File

@@ -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

6
rrrobot_ws/src/simulation_env/msg/arm_command.msg
View File

@@ -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

2
rrrobot_ws/src/simulation_env/msg/model_insertion.msg
View File

@@ -1,2 +0,0 @@
string model_name
geometry_msgs/Pose pose

73
rrrobot_ws/src/simulation_env/package.xml
View File

@@ -1,73 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>simulation_env</name>
<version>0.0.0</version>
<description>The simulation_env package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="root@todo.todo">root</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/simulation_env</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>gazebo_ros</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>gazebo_ros</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>gazebo_ros</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

133
rrrobot_ws/src/simulation_env/src/arm_angle_sensor_plugin.cpp
View File

@@ -1,133 +0,0 @@
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <memory>
#include <iostream>
#include <cmath>
#include "ros/ros.h"
#include "simulation_env/arm_angles.h"
#include <std_msgs/Float64.h>
using std::cout;
using std::endl;
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<simulation_env::arm_angles>("arm_positions", 1000);
shoulder_pivot_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/shoulder_pivot_angle", 1);
shoulder_joint_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/shoulder_joint_angle", 1);
elbow_joint_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/elbow_joint_angle", 1);
wrist_pivot_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/wrist_pivot_angle", 1);
wrist_joint_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/wrist_joint_angle", 1);
end_effector_pivot_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/end_effector_pivot_angle", 1);
std_msgs::Float64 shoulder_pivot_angle;
shoulder_pivot_angle.data = model->GetJoint("shoulder_pivot")->Position(); //msg.shoulder_pivot_angle;
std_msgs::Float64 shoulder_joint_angle;
shoulder_joint_angle.data = model->GetJoint("shoulder_joint")->Position(); //msg.shoulder_joint_angle;
std_msgs::Float64 elbow_joint_angle;
elbow_joint_angle.data = model->GetJoint("elbow_joint")->Position(); //msg.elbow_joint_angle;
std_msgs::Float64 wrist_pivot_angle;
wrist_pivot_angle.data = model->GetJoint("wrist_pivot")->Position(); //msg.wrist_pivot_angle;
std_msgs::Float64 wrist_joint_angle;
wrist_joint_angle.data = model->GetJoint("wrist_joint")->Position(); //msg.wrist_joint_angle;
std_msgs::Float64 end_effector_pivot_angle;
end_effector_pivot_angle.data = model->GetJoint("end_effector_pivot")->Position(); //msg.end_effector_pivot_angle;
// cout << "shoulder_pivot: " << shoulder_pivot_angle.data << endl;
// cout << "shoulder_joint: " << shoulder_joint_angle.data << endl;
// cout << "elbow_joint: " << elbow_joint_angle.data << endl;
// cout << "wrist_pivot: " << wrist_pivot_angle.data << endl;
// cout << "wrist_joint: " << wrist_joint_angle.data << endl;
// cout << "end_effector_pivot: " << end_effector_pivot_angle.data << endl;
}
private:
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<ros::NodeHandle> nh;
ros::Publisher publisher;
ros::Publisher shoulder_pivot_pub;
ros::Publisher shoulder_joint_pub;
ros::Publisher elbow_joint_pub;
ros::Publisher wrist_pivot_pub;
ros::Publisher wrist_joint_pub;
ros::Publisher end_effector_pivot_pub;
};
GZ_REGISTER_MODEL_PLUGIN(JointAngle)
} // namespace gazebo

117
rrrobot_ws/src/simulation_env/src/arm_motors.cpp
View File

@@ -1,117 +0,0 @@
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <memory>
#include <iostream>
#include "ros/ros.h"
#include "simulation_env/arm_command.h"
#include <std_msgs/Float64.h>
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<ros::NodeHandle> nh;
ros::Subscriber subscriber;
ros::Subscriber shoulder_pivot_sub;
ros::Subscriber shoulder_joint_sub;
ros::Subscriber elbow_joint_sub;
ros::Subscriber wrist_pivot_sub;
ros::Subscriber wrist_joint_sub;
ros::Subscriber end_effector_sub;
};
GZ_REGISTER_MODEL_PLUGIN(ArmControl)
} // namespace gazebo

78
rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp
View File

@@ -1,78 +0,0 @@
#include <ignition/math/Pose3.hh>
#include "gazebo/physics/physics.hh"
#include "gazebo/common/common.hh"
#include "gazebo/gazebo.hh"
#include "ros/ros.h"
#include <std_msgs/String.h>
#include "simulation_env/model_insertion.h"
#include <string>
#include <memory>
#include <iostream>
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<msgs::Factory>("~/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<ros::NodeHandle> nh;
ros::Subscriber subscriber;
};
// Register this plugin with the simulator
GZ_REGISTER_WORLD_PLUGIN(ModelInsertion)
} // namespace gazebo

70
rrrobot_ws/src/simulation_env/test/move_arm.cpp
View File

@@ -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<simulation_env::arm_command>("/arm_node/arm_commands", 1000);
ros::Subscriber sub = nh.subscribe("/arm_node/arm_positions", 1000, angle_callback);
ros::spin();
return 0;
}

34
rrrobot_ws/src/simulation_env/test/test_insert_object.cpp
View File

@@ -1,34 +0,0 @@
#include "simulation_env/model_insertion.h"
#include "ros/ros.h"
#include <iostream>
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<simulation_env::model_insertion>("/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();
}
}

20
rrrobot_ws/world/rrrobot.world
View File

@@ -1,20 +0,0 @@
<?xml version="1.0"?>
<sdf version="1.6">
<world name="default">
<!-- Ground Plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://fanuc_robotic_arm_with_gripper</uri>
</include>
<plugin name="insert_models" filename="libinsert_model.so"/>
</world>
</sdf>