From ee43b9f90f8284ff60b5c6128c260e7e9fa4bf9f Mon Sep 17 00:00:00 2001 From: brian Date: Wed, 9 Apr 2014 10:04:57 -0600 Subject: [PATCH 1/2] Added on-the-fly point streaming via the joint_command topic. Added a simple Carteian jogging package that demonstrates the added capability. --- motoman/CHANGELOG.rst | 4 + motoman/package.xml | 2 +- motoman_config/cfg/sia10d_mesh.xml | 120 +++++++- .../joint_trajectory_interface.h | 9 +- .../joint_trajectory_streamer.h | 11 +- .../joint_trajectory_streamer.h | 8 +- .../joint_trajectory_interface.cpp | 7 + .../joint_trajectory_streamer.cpp | 103 ++++++- .../src/joint_trajectory_streamer.cpp | 67 +++++ motoman_jogger/CMakeLists.txt | 165 +++++++++++ motoman_jogger/msg/Deltas.msg | 1 + motoman_jogger/package.xml | 62 +++++ motoman_jogger/src/keyboard_teleop_node.cpp | 81 ++++++ motoman_jogger/src/motoman_jogger.cpp | 249 +++++++++++++++++ motoman_jogger/src/motoman_jogger.cpp.bck | 258 ++++++++++++++++++ motoman_jogger/srv/set_velocity.srv | 3 + motoman_mh5_support/CHANGELOG.rst | 4 + motoman_mh5_support/package.xml | 2 +- motoman_sia10d_support/CHANGELOG.rst | 4 + motoman_sia10d_support/package.xml | 2 +- motoman_sia20d_moveit_config/CHANGELOG.rst | 5 + motoman_sia20d_moveit_config/package.xml | 2 +- motoman_sia20d_support/CHANGELOG.rst | 4 + motoman_sia20d_support/package.xml | 2 +- motoman_sia5d_support/CHANGELOG.rst | 5 + motoman_sia5d_support/package.xml | 2 +- 26 files changed, 1167 insertions(+), 15 deletions(-) create mode 100644 motoman_jogger/CMakeLists.txt create mode 100644 motoman_jogger/msg/Deltas.msg create mode 100644 motoman_jogger/package.xml create mode 100644 motoman_jogger/src/keyboard_teleop_node.cpp create mode 100644 motoman_jogger/src/motoman_jogger.cpp create mode 100644 motoman_jogger/src/motoman_jogger.cpp.bck create mode 100644 motoman_jogger/srv/set_velocity.srv diff --git a/motoman/CHANGELOG.rst b/motoman/CHANGELOG.rst index db6b7a7c..10d96ee2 100644 --- a/motoman/CHANGELOG.rst +++ b/motoman/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package motoman ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.3 (2014-02-07) +------------------ +* No changes + 0.3.2 (2014-01-31) ------------------ * Added build dependency on roslaunch to support packages diff --git a/motoman/package.xml b/motoman/package.xml index 24543ee3..075b3fe5 100644 --- a/motoman/package.xml +++ b/motoman/package.xml @@ -1,6 +1,6 @@ motoman - 0.3.2 + 0.3.3 The motoman stack constains libraries, configuration files, and ROS nodes for controlling a Motoman robot from ROS-Industrial Shaun Edwards BSD diff --git a/motoman_config/cfg/sia10d_mesh.xml b/motoman_config/cfg/sia10d_mesh.xml index a005330e..42c13f7f 100644 --- a/motoman_config/cfg/sia10d_mesh.xml +++ b/motoman_config/cfg/sia10d_mesh.xml @@ -181,5 +181,123 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h index ed489132..00cfbb11 100644 --- a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h +++ b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h @@ -69,7 +69,7 @@ class JointTrajectoryInterface public: /** - * \brief Default constructor. + * \brief Default constructor. */ JointTrajectoryInterface() : default_joint_pos_(0.0), default_vel_ratio_(0.1), default_duration_(10.0) {}; @@ -180,7 +180,7 @@ class JointTrajectoryInterface * \param[in] pt trajectory point data, in order/count expected by robot connection * \param[out] rbt_velocity computed velocity scalar for robot message (if needed by robot) * - * \return true on success, false otherwise + * \return true on success, false otherwise */ virtual bool calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity); @@ -212,6 +212,8 @@ class JointTrajectoryInterface * \param msg JointTrajectory message from ROS trajectory-planner */ virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg); + + virtual void jointCommandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg); /** * \brief Callback function registered to ROS stopMotion service @@ -245,6 +247,9 @@ class JointTrajectoryInterface SmplMsgConnection* connection_; ros::Subscriber sub_cur_pos_; // handle for joint-state topic subscription ros::Subscriber sub_joint_trajectory_; // handle for joint-trajectory topic subscription + + ros::Subscriber sub_joint_command_; // handle for joint-trajectory topic subscription + ros::ServiceServer srv_joint_trajectory_; // handle for joint-trajectory service ros::ServiceServer srv_stop_motion_; // handle for stop_motion service std::vector all_joint_names_; diff --git a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h index 9f68121a..5d4af53d 100644 --- a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h +++ b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h @@ -34,6 +34,7 @@ #include #include "motoman_driver/industrial_robot_client/joint_trajectory_interface.h" +#include namespace industrial_robot_client { @@ -48,7 +49,7 @@ namespace TransferStates { enum TransferState { - IDLE = 0, STREAMING =1 //,STARTING, //, STOPPING + IDLE = 0, STREAMING =1, POINT_STREAMING = 2 //,STARTING, //, STOPPING }; } typedef TransferStates::TransferState TransferState; @@ -74,7 +75,7 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface /** * \brief Default constructor * - * \param min_buffer_size minimum number of points as required by robot implementation + * \param min_buffer_size minimum number of points as required by robot implementation */ JointTrajectoryStreamer(int min_buffer_size = 1) : min_buffer_size_(min_buffer_size) {}; @@ -96,6 +97,8 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface ~JointTrajectoryStreamer(); virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg); + + virtual void jointCommandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg); virtual bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector* msgs); @@ -109,11 +112,13 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface boost::thread* streaming_thread_; boost::mutex mutex_; - int current_point_; + int current_point_, streaming_sequence; std::vector current_traj_; TransferState state_; ros::Time streaming_start_; int min_buffer_size_; + + std::queue streaming_queue_; }; } //joint_trajectory_streamer diff --git a/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h b/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h index 513c9dc2..db68142a 100644 --- a/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h +++ b/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h @@ -71,7 +71,7 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer /** * \brief Default constructor * - * \param robot_id robot group # on this controller (for multi-group systems) + * \param robot_id robot group # on this controller (for multi-group systems) */ MotomanJointTrajectoryStreamer(int robot_id=-1) : JointTrajectoryStreamer(1), robot_id_(robot_id) {} @@ -110,7 +110,7 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer protected: static const double pos_stale_time_ = 1.0; // max time since last "current position" update, for validation (sec) - static const double start_pos_tol_ = 1e-4; // max difference btwn start & current position, for validation (rad) + static const double start_pos_tol_ = 5e-4; // max difference btwn start & current position, for validation (rad) int robot_id_; MotomanMotionCtrl motion_ctrl_; @@ -120,6 +120,10 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer static bool VectorToJointData(const std::vector &vec, industrial::joint_data::JointData &joints); + + double time_of_last; + double time_since_last; + static const double point_streaming_timeout = 3.0; }; } //joint_trajectory_streamer diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp index 5a815b3f..df033fc9 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp +++ b/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp @@ -104,6 +104,8 @@ bool JointTrajectoryInterface::init(SmplMsgConnection* connection, const std::ve this->srv_joint_trajectory_ = this->node_.advertiseService("joint_path_command", &JointTrajectoryInterface::jointTrajectoryCB, this); this->sub_joint_trajectory_ = this->node_.subscribe("joint_path_command", 0, &JointTrajectoryInterface::jointTrajectoryCB, this); this->sub_cur_pos_ = this->node_.subscribe("joint_states", 1, &JointTrajectoryInterface::jointStateCB, this); + + this->sub_joint_command_ = this->node_.subscribe("joint_command", 0, &JointTrajectoryInterface::jointCommandCB, this); return true; } @@ -148,6 +150,11 @@ void JointTrajectoryInterface::jointTrajectoryCB(const trajectory_msgs::JointTra send_to_robot(robot_msgs); } +void JointTrajectoryInterface::jointCommandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg) +{ + //ROS_INFO("Yessir?"); +} + bool JointTrajectoryInterface::trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr& traj, std::vector* msgs) { msgs->clear(); diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp index b08ae4b0..8d868010 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp +++ b/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp @@ -99,6 +99,74 @@ void JointTrajectoryStreamer::jointTrajectoryCB(const trajectory_msgs::JointTraj send_to_robot(new_traj_msgs); } +void JointTrajectoryStreamer::jointCommandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg) +{ + // read current state value (should be atomic) + int state = this->state_; + + ROS_DEBUG("Current state is: %d", state); + + //If current state is idle, set to POINT_STREAMING + if (TransferStates::IDLE == state) + { + this->mutex_.lock(); + this->state_ = TransferStates::POINT_STREAMING; + this->current_point_ = 0; + this->streaming_sequence = 0; + this->streaming_start_ = ros::Time::now(); + this->mutex_.unlock(); + state = TransferStates::POINT_STREAMING; + ROS_INFO("First joint point received. Starting on-the-fly streaming."); + } + + //if current state is POINT_STREAMING, process incoming point. + if (TransferStates::POINT_STREAMING == state) + { + if (msg->points.empty()) + { + ROS_INFO("Empty point received, cancelling current trajectory"); + return; + } + + //Else, Push point into queue + SimpleMessage message; + trajectory_msgs::JointTrajectoryPoint rbt_pt, xform_pt; + + // select / reorder joints for sending to robot + if (!select(msg->joint_names, msg->points[0], this->all_joint_names_, &rbt_pt)) + return; + + // transform point data (e.g. for joint-coupling) + if (!transform(rbt_pt, &xform_pt)) + return; + + // convert trajectory point to ROS message + if (!create_message(this->streaming_sequence, xform_pt, &message)) + return; + + //Points get pushed into queue here. They will be popped in the Streaming Thread and sent to controller. + this->mutex_.lock(); + this->streaming_queue_.push(message); + this->streaming_sequence++; + this->mutex_.unlock(); + } + + //Else, cannot splice. Cancel current motion. + else + { + if (msg->points.empty()) + ROS_INFO("Empty trajectory received, canceling current trajectory"); + else + ROS_ERROR("Trajectory splicing not yet implemented, stopping current motion."); + + this->mutex_.lock(); + trajectoryStop(); + this->mutex_.unlock(); + return; + } +} + + bool JointTrajectoryStreamer::send_to_robot(const std::vector& messages) { ROS_INFO("Loading trajectory, setting state to streaming"); @@ -199,8 +267,41 @@ void JointTrajectoryStreamer::streamingThread() ROS_WARN("Failed sent joint point, will try again"); break; + case TransferStates::POINT_STREAMING: + ROS_INFO("I'm streaming, sir."); + //if no points in queue, streaming complete, set to idle. + if(this->streaming_queue_.empty()) + { + ROS_INFO("Point streaming complete, setting state to IDLE"); + this->state_ = TransferStates::IDLE; + break; + } + //if not connected, reconnect. + if (!this->connection_->isConnected()) + { + ROS_DEBUG("Robot disconnected. Attempting reconnect..."); + connectRetryCount = 5; + break; + } + //otherwise, send point to robot. + tmpMsg = this->streaming_queue_.front(); + this->streaming_queue_.pop(); + msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST, + ReplyTypes::INVALID, tmpMsg.getData()); // set commType=REQUEST + + ROS_DEBUG("Sending joint trajectory point"); + if (this->connection_->sendAndReceiveMsg(msg, reply, false)) + { + ROS_INFO("Point[%d] sent to controller", this->current_point_); + this->current_point_++; + } + else + ROS_WARN("Failed sent joint point, will try again"); + + break; + //consider checking for controller point starvation here. use a timer to check if the state is popping in and out of POINT_STREAMING mode, indicating something is trying to send streaming points, but is doing so too slowly. It may, in fact, not matter other than motion won't be smooth. default: - ROS_ERROR("Joint trajectory streamer: unknown state"); + ROS_ERROR("Joint trajectory streamer: unknown state, %d", this->state_); this->state_ = TransferStates::IDLE; break; } diff --git a/motoman_driver/src/joint_trajectory_streamer.cpp b/motoman_driver/src/joint_trajectory_streamer.cpp index 85763639..f79fe8f3 100644 --- a/motoman_driver/src/joint_trajectory_streamer.cpp +++ b/motoman_driver/src/joint_trajectory_streamer.cpp @@ -235,8 +235,75 @@ void MotomanJointTrajectoryStreamer::streamingThread() break; } } + + break; + case TransferStates::POINT_STREAMING: + //if no points in queue, streaming complete, set to idle. + if(this->streaming_queue_.empty()) + { + if(this->time_since_last < this->point_streaming_timeout) + { + time_since_last = ros::Time::now().toSec() - time_of_last; + ros::Duration(0.005).sleep(); + //ROS_INFO("Time since last point: %f", time_since_last); + break; + } + else + { + ROS_INFO("Point streaming complete, setting state to IDLE"); + this->state_ = TransferStates::IDLE; + break; + } + } + //if not connected, reconnect. + if (!this->connection_->isConnected()) + { + ROS_DEBUG("Robot disconnected. Attempting reconnect..."); + connectRetryCount = 5; + break; + } + //otherwise, send point to robot. + tmpMsg = this->streaming_queue_.front(); + msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST, + ReplyTypes::INVALID, tmpMsg.getData()); // set commType=REQUEST + + ROS_INFO("Sending joint trajectory point"); + if (this->connection_->sendAndReceiveMsg(msg, reply, false)) + { + MotionReplyMessage reply_status; + if (!reply_status.init(reply)) + { + ROS_ERROR("Aborting point stream operation."); + this->state_ = TransferStates::IDLE; + break; + } + if (reply_status.reply_.getResult() == MotionReplyResults::SUCCESS) + { + ROS_INFO("Point[%d] sent to controller", this->current_point_); + this->current_point_++; + time_since_last = 0.0; + time_of_last = ros::Time::now().toSec(); + this->streaming_queue_.pop(); + } + else if (reply_status.reply_.getResult() == MotionReplyResults::BUSY) + { + ROS_INFO("silently resending."); + break; // silently retry sending this point + } + else + { + ROS_ERROR_STREAM("Aborting point stream operation. Failed to send point" + << " (#" << this->current_point_ << "): " + << MotomanMotionCtrl::getErrorString(reply_status.reply_)); + this->state_ = TransferStates::IDLE; + break; + } + } + else + ROS_WARN("Failed sent joint point, will try again"); break; + default: ROS_ERROR("Joint trajectory streamer: unknown state"); this->state_ = TransferStates::IDLE; diff --git a/motoman_jogger/CMakeLists.txt b/motoman_jogger/CMakeLists.txt new file mode 100644 index 00000000..3214e734 --- /dev/null +++ b/motoman_jogger/CMakeLists.txt @@ -0,0 +1,165 @@ +cmake_minimum_required(VERSION 2.8.3) +project(motoman_jogger) + +## 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 + moveit_ros_manipulation + moveit_ros_move_group + moveit_ros_planning_interface + roscpp + message_generation +) + +## 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 and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * add a run_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 + Deltas.msg +# Message2.msg + ) + +## Generate services in the 'srv' folder + add_service_files( + FILES + set_velocity.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 # Or other packages containing msgs + ) + +################################### +## 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 you 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 motoman_jogger +# CATKIN_DEPENDS moveit_ros_manipulation moveit_ros_move_group roscpp +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp library +# add_library(motoman_jogger +# src/${PROJECT_NAME}/motoman_jogger.cpp +# ) + +## Declare a cpp executable + add_executable(motoman_jogger_node src/motoman_jogger.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(motoman_jogger_node motoman_jogger_generate_messages_cpp) + +## Specify libraries to link a library or executable target against + target_link_libraries(motoman_jogger_node + ${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 and/or libraries for installation +# install(TARGETS motoman_jogger motoman_jogger_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 +# # 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_motoman_jogger.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/motoman_jogger/msg/Deltas.msg b/motoman_jogger/msg/Deltas.msg new file mode 100644 index 00000000..e8ffff6e --- /dev/null +++ b/motoman_jogger/msg/Deltas.msg @@ -0,0 +1 @@ +float64[] deltas diff --git a/motoman_jogger/package.xml b/motoman_jogger/package.xml new file mode 100644 index 00000000..80202373 --- /dev/null +++ b/motoman_jogger/package.xml @@ -0,0 +1,62 @@ + + + motoman_jogger + 0.0.0 + The motoman_jogger package + + + + + brian + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + moveit_ros_manipulation + moveit_ros_move_group + moveit_ros_planning_interface + roscpp + message_generation + moveit_ros_manipulation + moveit_ros_move_group + moveit_ros_planning_interface + roscpp + + + + + + + + + + + diff --git a/motoman_jogger/src/keyboard_teleop_node.cpp b/motoman_jogger/src/keyboard_teleop_node.cpp new file mode 100644 index 00000000..6943cea6 --- /dev/null +++ b/motoman_jogger/src/keyboard_teleop_node.cpp @@ -0,0 +1,81 @@ +#include +#include +#include +#include +#include + +int main(int argc, char** argv) +{ + + ros::init(argc, argv, "keyboard_teleop"); + ros::NodeHandle n; + + ros::Publisher delta_pub = n.advertise("cartesian_jogging_deltas", 1); + motoman_jogger::Deltas deltas; + ROS_INFO("Keyboard teleop online."); + + //system("stty raw"); + + while(ros::ok) + { + //Clear the deltas. + deltas.deltas.clear(); + deltas.deltas.resize(6,0); + initscr(); + //nodelay(stdscr, TRUE); + noecho(); + cbreak(); + + char command = getch(); + //ros::Duration(0.1).sleep(); + //std::cout << command << std::endl; + switch(command) + { + case '1': + deltas.deltas.at(0) = 1.0; + break; + case '2': + deltas.deltas.at(1) = 1.0; + break; + case '3': + deltas.deltas.at(2) = 1.0; + break; + case '4': + deltas.deltas.at(3) = 1.0; + break; + case '5': + deltas.deltas.at(4) = 1.0; + break; + case '6': + deltas.deltas.at(5) = 1.0; + break; + case 'q': + deltas.deltas.at(0) = -1.0; + break; + case 'w': + deltas.deltas.at(1) = -1.0; + break; + case 'e': + deltas.deltas.at(2) = -1.0; + break; + case 'r': + deltas.deltas.at(3) = -1.0; + break; + case 't': + deltas.deltas.at(4) = -1.0; + break; + case 'y': + deltas.deltas.at(5) = -1.0; + break; + case ERR: + break; + default: + break; + } + delta_pub.publish(deltas); + ros::spinOnce(); + //system("stty cooked"); + } + + return(0); +} diff --git a/motoman_jogger/src/motoman_jogger.cpp b/motoman_jogger/src/motoman_jogger.cpp new file mode 100644 index 00000000..416b7566 --- /dev/null +++ b/motoman_jogger/src/motoman_jogger.cpp @@ -0,0 +1,249 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +sensor_msgs::JointState current_joints; +std::map joint_name_map; +robot_state::RobotStatePtr kinematic_state; +trajectory_msgs::JointTrajectory dummy_traj; +trajectory_msgs::JointTrajectoryPoint point; +const robot_state::JointModelGroup* joint_model_group; +ros::Time start_time, time_last, last_callback; +ros::Publisher streaming_pub; +motoman_jogger::Deltas deltas; +double jogging_velocity; +double max_translation_acc, translation_acc; +double max_rotation_acc, rotation_acc; + +void joint_state_cb(sensor_msgs::JointState msg) +{ + current_joints = msg; +} + +Eigen::MatrixXd invert(Eigen::MatrixXd J) +{ + return(J.transpose()*(J*J.transpose()).inverse()); +} + +void cartesian_delta_cb(motoman_jogger::Deltas new_deltas) +{ + last_callback = ros::Time::now(); + //Check valid deltas: + if(new_deltas.deltas.size() != 6) + { + ROS_ERROR("Cartesian jogging deltas must be of size 6. Ignoring message."); + return; + } + deltas = new_deltas; +} + +bool set_vel_cb(motoman_jogger::set_velocity::Request &req, motoman_jogger::set_velocity::Response &res) +{ + if(req.speed < 0 || req.speed > 1.0) + { + ROS_WARN("Invalid jogging speed requested. Value must be on interval [0,1]."); + res.result = false; + } + else + { + jogging_velocity = req.speed; + translation_acc = req.speed*max_translation_acc; + rotation_acc = req.speed*max_rotation_acc; + res.result = true; + } + return res.result; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "motoman_jogger"); + ros::NodeHandle n; + + streaming_pub = n.advertise("joint_command", 10); + ros::Subscriber cart_delta_sub = n.subscribe("cartesian_jogging_deltas", 1, cartesian_delta_cb); + + //Set up joint subscriber + ros::Subscriber joint_sub = n.subscribe("joint_states", 1, joint_state_cb); + moveit::planning_interface::MoveGroup arm(argv[1]); + + //Set up server to set velocity + ros::ServiceServer set_vel_server = n.advertiseService("set_jogging_velocity", set_vel_cb); + + double dt = 0.1; + double move_timeout = 2.0; + jogging_velocity = 1.0; + + double cart_translation_vel = .75; + max_translation_acc = .015; + translation_acc = max_translation_acc; + double k_translation = 0.1; + double b_translation = 0.95; + + double cart_rotation_vel = 1.5; + max_rotation_acc = 0.05; + rotation_acc = max_rotation_acc; + double k_rotation = 0.1; + double b_rotation = 0.95; + + double jacobian_condition = 1.0; + + Eigen::VectorXd d_X(6), d_theta(7), cart_acc(6), cart_vel(6); + Eigen::VectorXd vel_err(6), err_dot(6); + Eigen::MatrixXd J(6,7); + Eigen::Vector3d ref_point(0.0, 0.0, 0.0); + + //initialize Cartesian delta vector; + cart_acc.setZero(); + cart_vel.setZero(); + d_X.setZero(); + + + //Get set up for kinematics: + robot_model_loader::RobotModelLoader model_loader("robot_description"); + robot_model::RobotModelPtr kinematic_model = model_loader.getModel(); + ROS_INFO("Model Frame: %s", kinematic_model->getModelFrame().c_str()); + + kinematic_state = boost::shared_ptr(new robot_state::RobotState(kinematic_model)); + kinematic_state->setToDefaultValues(); + joint_model_group = kinematic_model->getJointModelGroup("sia10"); + + + const std::vector &joint_names = joint_model_group->getJointModelNames(); + std::vector joint_values; + kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); + ros::topic::waitForMessage("joint_states"); + + //Initialize jogging start. + dummy_traj.joint_names = current_joints.name; + for(unsigned int joint=0; joint<7; joint++) + { + point.positions.push_back(current_joints.position.at(joint)); + point.velocities.push_back(0); + } + point.time_from_start = ros::Duration(0.0); + dummy_traj.points.push_back(point); + streaming_pub.publish(dummy_traj); + + start_time = ros::Time::now(); + time_last = ros::Time::now(); + deltas.deltas.resize(6,0); + + while(ros::ok) + { + if((ros::Time::now() - last_callback).toSec() > .1) + { + for(unsigned int i=0; i<6; i++) + { + deltas.deltas.at(i) = 0.0; + } + } + + //Time since last point: + //ros::topic::waitForMessage("joint_states"); + dt = (ros::Time::now() - time_last).toSec(); + time_last = ros::Time::now(); + kinematic_state->setVariableValues(current_joints); + + //Compute command velocity + Eigen::VectorXd vel_command(6); + double sum_squares = 0; + for(unsigned int i=0; i<3; i++) + { + sum_squares = sum_squares + deltas.deltas.at(i)*deltas.deltas.at(i); + } + double delta_mag = pow(sum_squares, 0.5); + for(unsigned int i=0; i<3; i++) + { + if(delta_mag > 0.0) + vel_command[i] = deltas.deltas.at(i)/delta_mag*jogging_velocity*cart_translation_vel; + else + vel_command[i] = 0.0; + } + + sum_squares = 0; + for(unsigned int i=3; i<6; i++) + { + sum_squares = sum_squares + deltas.deltas.at(i)*deltas.deltas.at(i); + } + delta_mag = pow(sum_squares, 0.5); + for(unsigned int i=3; i<6; i++) + { + if(delta_mag > 0.0) + vel_command[i] = deltas.deltas.at(i)/delta_mag*jogging_velocity*cart_rotation_vel; + else + vel_command[i] = 0.0; + } + //std::cout << "vel_command norm: " << vel_command.norm() << std::endl; + //Compute velocity error. + err_dot = 1/dt*((vel_command - cart_vel)-vel_err); + vel_err = vel_command - cart_vel; + + double err_mag = vel_err.norm(); + //std::cout << "vel_error norm: " << vel_err.squaredNorm() << std::endl; + + //Compute acceleration + if(err_mag <= 0.015) + { + cart_vel = vel_command; + cart_acc.setZero(); + } + else + { + for(unsigned int i=0; i<3; i++) + { + cart_acc[i] = vel_err.normalized()[i]*translation_acc/dt; + } + for(unsigned int i=3; i<6; i++) + { + cart_acc[i] = vel_err.normalized()[i]*rotation_acc/dt; + } + cart_vel = (cart_vel + dt*cart_acc); + } + + if(cart_vel.norm() == 0) + cart_vel.setZero(); + + d_X = cart_vel*dt; + //std::cout << "accel:\n" << cart_acc << std::endl; + //std::cout << "err_dot:\n" << err_dot << std::endl; + //std::cout << "vel:\n" << cart_vel << std::endl; + //std::cout <<" velocity norm: " << cart_vel.norm() <getJacobian(joint_model_group, kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),ref_point, J); + d_theta = invert(J)*d_X; + + //check condition number: + jacobian_condition = J.norm()*invert(J).norm(); + //std::cout << "Jacobian condition: " << jacobian_condition << std::endl; + if(jacobian_condition > 39) + { + ROS_ERROR("Cannot do Cartesian Jogging due to ill-conditioned Jacobian"); + return 0; + } + + //std::cout << "d_theta:\n" << d_theta << std::endl; + for(unsigned int j=0; j<7; j++) + { + point.positions.at(j) = current_joints.position.at(j) + d_theta[j]; + point.velocities.at(j) = d_theta[j]/dt; + } + + point.time_from_start = ros::Time::now() - start_time; + dummy_traj.points.at(0) = point; + streaming_pub.publish(dummy_traj); + ros::Duration(0.01).sleep(); + + ros::spinOnce(); + } + ros::shutdown(); + return 1; +} diff --git a/motoman_jogger/src/motoman_jogger.cpp.bck b/motoman_jogger/src/motoman_jogger.cpp.bck new file mode 100644 index 00000000..bac24a28 --- /dev/null +++ b/motoman_jogger/src/motoman_jogger.cpp.bck @@ -0,0 +1,258 @@ +#include +#include +#include +#include +#include +#include +#include + +sensor_msgs::JointState current_joints; +std::map joint_name_map; +robot_state::RobotStatePtr kinematic_state; +trajectory_msgs::JointTrajectory dummy_traj; +trajectory_msgs::JointTrajectoryPoint point; +const robot_state::JointModelGroup* joint_model_group; +ros::Time start_time, time_last, last_callback; +ros::Publisher streaming_pub; +motoman_jogger::Deltas deltas; + +double dt, cart_translation_vel, cart_rotation_vel, move_timeout, max_translation_acc, max_rotation_acc; + +Eigen::VectorXd d_X, d_theta, cart_acc, cart_vel; +Eigen::MatrixXd J; +Eigen::Vector3d ref_point; + +void joint_state_cb(sensor_msgs::JointState msg) +{ + current_joints = msg; +} + +Eigen::MatrixXd invert(Eigen::MatrixXd J) +{ + return(J.transpose()*(J*J.transpose()).inverse()); +} + +void cartesian_delta_cb(motoman_jogger::Deltas new_deltas) +{ + last_callback = ros::Time::now(); + //Check valid deltas: + if(new_deltas.deltas.size() != 6) + { + ROS_ERROR("Cartesian jogging deltas must be of size 6. Ignoring message."); + return; + } + deltas = new_deltas; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "motoman_jogger"); + ros::NodeHandle n; + + streaming_pub = n.advertise("joint_command", 10); + ros::Subscriber cart_delta_sub = n.subscribe("cartesian_jogging_deltas", 1, cartesian_delta_cb); + + //Set up joint subscriber + ros::Subscriber joint_sub = n.subscribe("joint_states", 1, joint_state_cb); + + moveit::planning_interface::MoveGroup arm(argv[1]); + dt = 0.1; + cart_translation_vel = 1.0; + cart_rotation_vel = 2.0; + move_timeout = 2.0; + max_rotation_acc = 10.0; + max_translation_acc = 10.0; + + d_X.resize(6); + d_theta.resize(6); + cart_acc.resize(6); + cart_vel.resize(6); + J.resize(6,7); + ref_point.setZero(3); + + //initialize Cartesian delta vector; + for(unsigned int i=0; i<6; i++) + { + d_X[i] = 0.0; + cart_vel[i] = 0.0; + cart_acc[i] = 0.0; + } + + //Get set up for kinematics: + robot_model_loader::RobotModelLoader model_loader("robot_description"); + robot_model::RobotModelPtr kinematic_model = model_loader.getModel(); + ROS_INFO("Model Frame: %s", kinematic_model->getModelFrame().c_str()); + + kinematic_state = boost::shared_ptr(new robot_state::RobotState(kinematic_model)); + kinematic_state->setToDefaultValues(); + joint_model_group = kinematic_model->getJointModelGroup("sia10"); + + + const std::vector &joint_names = joint_model_group->getJointModelNames(); + std::vector joint_values; + kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); + ros::topic::waitForMessage("joint_states"); + + //Initialize jogging start. + dummy_traj.joint_names = current_joints.name; + for(unsigned int joint=0; joint<7; joint++) + { + point.positions.push_back(current_joints.position.at(joint)); + point.velocities.push_back(0); + } + point.time_from_start = ros::Duration(0.0); + dummy_traj.points.push_back(point); + streaming_pub.publish(dummy_traj); + + start_time = ros::Time::now(); + time_last = ros::Time::now(); + deltas.deltas.resize(6,0); + + while(ros::ok) + { + if((ros::Time::now() - last_callback).toSec() > .15) + { + for(unsigned int i=0; i<6; i++) + { + deltas.deltas.at(i) = 0.0; + } + } + //This loop should do all of the computation and sending of joints. + //The callback will just change the deltas. + //If time passes since last time the callback got called, deltas reset to zero. + //Time since last point: + dt = (ros::Time::now() - time_last).toSec(); + time_last = ros::Time::now(); + + //Compute commanded velocity, store in deltas. + double sum_squares = 0; + for(unsigned int i=0; i<3; i++) + { + sum_squares = sum_squares + deltas.deltas.at(i)*deltas.deltas.at(i); + } + double delta_mag = pow(sum_squares, 0.5); + for(unsigned int i=0; i<3; i++) + { + if(delta_mag > 0.0) + deltas.deltas.at(i) = deltas.deltas.at(i)/delta_mag*cart_translation_vel; + else + deltas.deltas.at(i) = 0.0; + } + + sum_squares = 0; + for(unsigned int i=3; i<6; i++) + { + sum_squares = sum_squares + deltas.deltas.at(i)*deltas.deltas.at(i); + } + delta_mag = pow(sum_squares, 0.5); + for(unsigned int i=3; i<6; i++) + { + if(delta_mag > 0.0) + deltas.deltas.at(i) = deltas.deltas.at(i)/delta_mag*cart_rotation_vel; + else + deltas.deltas.at(i) = 0.0; + } + //std::cout << "deltas:\n" << deltas << std::endl; + //Command velocity now in deltas. Compute the acceleration vector. + sum_squares = 0; + for(unsigned int i=0; i<3; i++) + { + sum_squares = sum_squares + pow(deltas.deltas.at(i) - cart_vel[i], 2); + } + double err_mag = pow(sum_squares, 0.5); //acc mag here is actually mag of velocity error. + //scale acc_magnitude to be proportional near zero velocity error, but quickly saturating to max. + double acc_mag = 0.0; + if(err_mag > .05*2*cart_translation_vel) + acc_mag = max_translation_acc; + else + acc_mag = err_mag*max_translation_acc/(.05*2*cart_translation_vel); + for(unsigned int i=0; i<3; i++) + { + if(acc_mag > 0.0) //proportional throttle + cart_acc[i] = (deltas.deltas.at(i)-cart_vel[i])/err_mag*acc_mag/dt; + else + cart_acc[i] = 0.0; + } + + sum_squares = 0; + for(unsigned int i=3; i<6; i++) + { + sum_squares = sum_squares + pow(deltas.deltas.at(i) - cart_vel[i], 2); + } + acc_mag = pow(sum_squares, 0.5); + if(acc_mag > max_rotation_acc) + acc_mag = max_rotation_acc; + for(unsigned int i=3; i<6; i++) + { + if(acc_mag > 0.0) //proportional throttle. + cart_acc[i] = (deltas.deltas.at(i)-cart_vel[i])/dt; + else + cart_acc[i] = 0.0; + } + std::cout << "acceleration:\n" << cart_acc << std::endl; + + //TODO: If dt gets too large, it's possible that new command will overshoot at every iteration. Can fix this with a magnitude check, but may not be necessary. + + //Compute velocities and Cartesian deltas. + sum_squares = 0; + for(unsigned int i=0; i<3; i++) + { + cart_vel[i] = cart_vel[i] + cart_acc[i]*dt; + sum_squares += pow(cart_vel[i], 2); + } + double vel_mag = pow(sum_squares,0.5); + for(unsigned int i=0; i<3; i++) + { + if(vel_mag > 0.0){ + d_X[i] = cart_vel[i]*dt; + } + else + { + cart_vel[i] = 0.0; + d_X[i] = 0.0; + } + } + + sum_squares = 0; + for(unsigned int i=3; i<6; i++) + { + cart_vel[i] = cart_vel[i] + cart_acc[i]*dt; + sum_squares += pow(cart_vel[i], 2); + } + vel_mag = pow(sum_squares,0.5); + for(unsigned int i=3; i<6; i++) + { + if(vel_mag > 0.0){ + d_X[i] = cart_vel[i]*dt; + } + else + { + cart_vel[i] = 0.0; + d_X[i] = 0.0; + } + } + std::cout << "velocity:\n" << cart_vel << std::endl; + //std::cout << "d_X:\n" << d_X << std::endl; + ros::topic::waitForMessage("joint_states"); + + //Get the Jacobian + kinematic_state->setVariableValues(current_joints); + kinematic_state->getJacobian(joint_model_group, kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),ref_point, J); + d_theta = invert(J)*d_X; + std::cout << "d_theta:\n" << d_theta << std::endl; + for(unsigned int j=0; j<7; j++) + { + point.positions.at(j) = current_joints.position.at(j) + d_theta[j]; + point.velocities.at(j) = d_theta[j]/dt; + } + + point.time_from_start = ros::Time::now() - start_time; + dummy_traj.points.at(0) = point; + streaming_pub.publish(dummy_traj); + //ros::Duration(0.01).sleep(); + + ros::spinOnce(); + } + ros::shutdown(); + return 1; +} diff --git a/motoman_jogger/srv/set_velocity.srv b/motoman_jogger/srv/set_velocity.srv new file mode 100644 index 00000000..613b25c4 --- /dev/null +++ b/motoman_jogger/srv/set_velocity.srv @@ -0,0 +1,3 @@ +float32 speed +--- +bool result diff --git a/motoman_mh5_support/CHANGELOG.rst b/motoman_mh5_support/CHANGELOG.rst index 43c5d9c0..e165eab9 100644 --- a/motoman_mh5_support/CHANGELOG.rst +++ b/motoman_mh5_support/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package motoman_mh5_support ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.3 (2014-02-07) +------------------ +* No changes + 0.3.2 (2014-01-31) ------------------ * Added build dependency on roslaunch to address missing roslaunch check missing macro diff --git a/motoman_mh5_support/package.xml b/motoman_mh5_support/package.xml index f7fb6fd9..58a59d23 100644 --- a/motoman_mh5_support/package.xml +++ b/motoman_mh5_support/package.xml @@ -1,7 +1,7 @@ motoman_mh5_support - 0.3.2 + 0.3.3

ROS Industrial support for the Motoman mh5 (and variants). diff --git a/motoman_sia10d_support/CHANGELOG.rst b/motoman_sia10d_support/CHANGELOG.rst index 96d8415f..7d5196aa 100644 --- a/motoman_sia10d_support/CHANGELOG.rst +++ b/motoman_sia10d_support/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package motoman_sia10d_support ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.3 (2014-02-07) +------------------ +* No changes + 0.3.2 (2014-01-31) ------------------ * Added build dependency on roslaunch to address missing roslaunch check missing macro diff --git a/motoman_sia10d_support/package.xml b/motoman_sia10d_support/package.xml index 26bd192d..170fd9b2 100644 --- a/motoman_sia10d_support/package.xml +++ b/motoman_sia10d_support/package.xml @@ -1,7 +1,7 @@ motoman_sia10d_support - 0.3.2 + 0.3.3

ROS Industrial support for the Motoman sia10d (and variants). diff --git a/motoman_sia20d_moveit_config/CHANGELOG.rst b/motoman_sia20d_moveit_config/CHANGELOG.rst index aa6fde8c..5f74297f 100644 --- a/motoman_sia20d_moveit_config/CHANGELOG.rst +++ b/motoman_sia20d_moveit_config/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package motoman_sia20d_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.3 (2014-02-07) +------------------ +* Path fix for industrial_core/`#22 `_ moveit patch +* Contributors: Shaun Edwards + 0.3.2 (2014-01-31) ------------------ * No changes diff --git a/motoman_sia20d_moveit_config/package.xml b/motoman_sia20d_moveit_config/package.xml index f6d8ec52..ba76e94e 100644 --- a/motoman_sia20d_moveit_config/package.xml +++ b/motoman_sia20d_moveit_config/package.xml @@ -1,7 +1,7 @@ motoman_sia20d_moveit_config - 0.3.2 + 0.3.3 An automatically generated package with all the configuration and launch files for using the motoman_sia20d with the MoveIt Motion Planning Framework diff --git a/motoman_sia20d_support/CHANGELOG.rst b/motoman_sia20d_support/CHANGELOG.rst index 6e23af18..7f5e275d 100644 --- a/motoman_sia20d_support/CHANGELOG.rst +++ b/motoman_sia20d_support/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package motoman_sia20d_support ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.3 (2014-02-07) +------------------ +* No changes + 0.3.2 (2014-01-31) ------------------ * Added build dependency on roslaunch to address missing roslaunch check missing macro diff --git a/motoman_sia20d_support/package.xml b/motoman_sia20d_support/package.xml index ec255de5..dbe0ce23 100644 --- a/motoman_sia20d_support/package.xml +++ b/motoman_sia20d_support/package.xml @@ -1,7 +1,7 @@ motoman_sia20d_support - 0.3.2 + 0.3.3

ROS Industrial support for the Motoman sia20d (and variants). diff --git a/motoman_sia5d_support/CHANGELOG.rst b/motoman_sia5d_support/CHANGELOG.rst index 2f6a26e4..00f321b4 100644 --- a/motoman_sia5d_support/CHANGELOG.rst +++ b/motoman_sia5d_support/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package motoman_sia5d_support ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.3 (2014-02-07) +------------------ +* sia5d: remove incorrect safety ctrlr tags. Fix `#29 `_. +* Contributors: gavanderhoorn + 0.3.2 (2014-01-31) ------------------ * Added build dependency on roslaunch to address missing roslaunch check missing macro diff --git a/motoman_sia5d_support/package.xml b/motoman_sia5d_support/package.xml index aba4c8f3..10270b1a 100644 --- a/motoman_sia5d_support/package.xml +++ b/motoman_sia5d_support/package.xml @@ -1,7 +1,7 @@ motoman_sia5d_support - 0.3.2 + 0.3.3

ROS Industrial support for the Motoman sia5d (and variants). From edea90c2125c5bcf01d92a2b8eb94b8ab2f441a9 Mon Sep 17 00:00:00 2001 From: brian Date: Wed, 9 Apr 2014 10:22:46 -0600 Subject: [PATCH 2/2] reverted sia10d_mesh.xml to original state. --- motoman_config/cfg/sia10d_mesh.xml | 119 ----------------------------- 1 file changed, 119 deletions(-) diff --git a/motoman_config/cfg/sia10d_mesh.xml b/motoman_config/cfg/sia10d_mesh.xml index 42c13f7f..51d5529b 100644 --- a/motoman_config/cfg/sia10d_mesh.xml +++ b/motoman_config/cfg/sia10d_mesh.xml @@ -181,123 +181,4 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -