commit a331a40d1779929602050955e6a106741f794eb7 Author: probablydavid Date: Thu Aug 19 21:10:56 2010 +0000 Initial URDF Tools Package diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..28105dd --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of +# directories (or patterns, but directories should suffice) that should +# be excluded from the distro. This is not the place to put things that +# should be ignored everywhere, like "build" directories; that happens in +# rosbuild/rosbuild.cmake. Here should be listed packages that aren't +# ready for inclusion in a distro. +# +# This list is combined with the list in rosbuild/rosbuild.cmake. Note +# that CMake 2.6 may be required to ensure that the two lists are combined +# properly. CMake 2.4 seems to have unpredictable scoping rules for such +# variables. +#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) + +rosbuild_make_distribution(0.1.0) diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..a818cca --- /dev/null +++ b/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake_stack.mk \ No newline at end of file diff --git a/arm_kinematics/CMakeLists.txt b/arm_kinematics/CMakeLists.txt new file mode 100644 index 0000000..63cf1cf --- /dev/null +++ b/arm_kinematics/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +rosbuild_add_executable(arm_kinematics arm_kinematics.cpp) diff --git a/arm_kinematics/Makefile b/arm_kinematics/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/arm_kinematics/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/arm_kinematics/arm_kinematics.cpp b/arm_kinematics/arm_kinematics.cpp new file mode 100644 index 0000000..d854f47 --- /dev/null +++ b/arm_kinematics/arm_kinematics.cpp @@ -0,0 +1,353 @@ +// bsd license blah blah +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +using std::string; + +static const std::string IK_SERVICE = "get_ik"; +static const std::string FK_SERVICE = "get_fk"; +static const std::string IK_INFO_SERVICE = "get_ik_solver_info"; +static const std::string FK_INFO_SERVICE = "get_fk_solver_info"; + +class Kinematics { + public: + Kinematics(); + bool init(); + + private: + ros::NodeHandle nh, nh_private; + std::string root_name, tip_name; + KDL::JntArray joint_min, joint_max; + KDL::Chain chain; + unsigned int num_joints; + + KDL::ChainFkSolverPos_recursive* fk_solver; + KDL::ChainIkSolverPos_NR_JL *ik_solver_pos; + KDL::ChainIkSolverVel_pinv* ik_solver_vel; + + ros::ServiceServer ik_service,ik_solver_info_service; + ros::ServiceServer fk_service,fk_solver_info_service; + + tf::TransformListener tf_listener; + + kinematics_msgs::KinematicSolverInfo info; + + bool loadModel(const std::string xml); + bool readJoints(urdf::Model &robot_model); + int getJointIndex(const std::string &name); + int getKDLSegmentIndex(const std::string &name); + + /** + * @brief This is the basic IK service method that will compute and return an IK solution. + * @param A request message. See service definition for GetPositionIK for more information on this message. + * @param The response message. See service definition for GetPositionIK for more information on this message. + */ + bool getPositionIK(kinematics_msgs::GetPositionIK::Request &request, + kinematics_msgs::GetPositionIK::Response &response); + + /** + * @brief This is the basic kinematics info service that will return information about the kinematics node. + * @param A request message. See service definition for GetKinematicSolverInfo for more information on this message. + * @param The response message. See service definition for GetKinematicSolverInfo for more information on this message. + */ + bool getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request, + kinematics_msgs::GetKinematicSolverInfo::Response &response); + + /** + * @brief This is the basic kinematics info service that will return information about the kinematics node. + * @param A request message. See service definition for GetKinematicSolverInfo for more information on this message. + * @param The response message. See service definition for GetKinematicSolverInfo for more information on this message. + */ + bool getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request, + kinematics_msgs::GetKinematicSolverInfo::Response &response); + + /** + * @brief This is the basic forward kinematics service that will return information about the kinematics node. + * @param A request message. See service definition for GetPositionFK for more information on this message. + * @param The response message. See service definition for GetPositionFK for more information on this message. + */ + bool getPositionFK(kinematics_msgs::GetPositionFK::Request &request, + kinematics_msgs::GetPositionFK::Response &response); +}; + + + +Kinematics::Kinematics(): nh_private ("~") { +} + +bool Kinematics::init() { + // Get URDF XML + std::string urdf_xml, full_urdf_xml; + nh.param("urdf_xml",urdf_xml,std::string("robot_description")); + nh.searchParam(urdf_xml,full_urdf_xml); + ROS_DEBUG("Reading xml file from parameter server"); + std::string result; + if (!nh.getParam(full_urdf_xml, result)) { + ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str()); + return false; + } + + // Get Root and Tip From Parameter Service + if (!nh_private.getParam("root_name", root_name)) { + ROS_FATAL("GenericIK: No root name found on parameter server"); + return false; + } + if (!nh_private.getParam("tip_name", tip_name)) { + ROS_FATAL("GenericIK: No tip name found on parameter server"); + return false; + } + + // Load and Read Models + if (!loadModel(result)) { + ROS_FATAL("Could not load models!"); + return false; + } + + // Get Solver Parameters + int maxIterations; + double epsilon; + + nh_private.param("maxIterations", maxIterations, 1000); + nh_private.param("epsilon", epsilon, 1e-2); + + // Build Solvers + fk_solver = new KDL::ChainFkSolverPos_recursive(chain); + ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain); + ik_solver_pos = new KDL::ChainIkSolverPos_NR_JL(chain, joint_min, joint_max, + *fk_solver, *ik_solver_vel, maxIterations, epsilon); + + ROS_INFO("Advertising services"); + fk_service = nh_private.advertiseService(FK_SERVICE,&Kinematics::getPositionFK,this); + ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::getPositionIK,this); + ik_solver_info_service = nh_private.advertiseService(IK_INFO_SERVICE,&Kinematics::getIKSolverInfo,this); + fk_solver_info_service = nh_private.advertiseService(FK_INFO_SERVICE,&Kinematics::getFKSolverInfo,this); + + return true; +} + +bool Kinematics::loadModel(const std::string xml) { + urdf::Model robot_model; + KDL::Tree tree; + + if (!robot_model.initString(xml)) { + ROS_FATAL("Could not initialize robot model"); + return -1; + } + if (!kdl_parser::treeFromString(xml, tree)) { + ROS_ERROR("Could not initialize tree object"); + return false; + } + if (!tree.getChain(root_name, tip_name, chain)) { + ROS_ERROR("Could not initialize chain object"); + return false; + } + + if (!readJoints(robot_model)) { + ROS_FATAL("Could not read information about the joints"); + return false; + } + + return true; +} + +bool Kinematics::readJoints(urdf::Model &robot_model) { + num_joints = 0; + // get joint maxs and mins + boost::shared_ptr link = robot_model.getLink(tip_name); + boost::shared_ptr joint; + + while (link && link->name != root_name) { + joint = robot_model.getJoint(link->parent_joint->name); + if (!joint) { + ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str()); + return false; + } + if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) { + ROS_INFO( "adding joint: [%s]", joint->name.c_str() ); + num_joints++; + } + link = robot_model.getLink(link->getParent()->name); + } + + joint_min.resize(num_joints); + joint_max.resize(num_joints); + info.joint_names.resize(num_joints); + info.limits.resize(num_joints); + + link = robot_model.getLink(tip_name); + unsigned int i = 0; + while (link && i < num_joints) { + joint = robot_model.getJoint(link->parent_joint->name); + if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) { + ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() ); + + float lower, upper; + int hasLimits; + if ( joint->type != urdf::Joint::CONTINUOUS ) { + lower = joint->limits->lower; + upper = joint->limits->upper; + hasLimits = 1; + } else { + lower = -M_PI; + upper = M_PI; + hasLimits = 0; + } + int index = num_joints - i -1; + + joint_min.data[index] = lower; + joint_max.data[index] = upper; + info.joint_names[index] = joint->name; + info.limits[index].joint_name = joint->name; + info.limits[index].has_position_limits = hasLimits; + info.limits[index].min_position = lower; + info.limits[index].max_position = upper; + i++; + } + link = robot_model.getLink(link->getParent()->name); + } + return true; +} + + +int Kinematics::getJointIndex(const std::string &name) { + for (unsigned int i=0; i < info.joint_names.size(); i++) { + if (info.joint_names[i] == name) + return i; + } + return -1; +} + +int Kinematics::getKDLSegmentIndex(const std::string &name) { + int i=0; + while (i < (int)chain.getNrOfSegments()) { + if (chain.getSegment(i).getName() == name) { + return i+1; + } + i++; + } + return -1; +} + +bool Kinematics::getPositionIK(kinematics_msgs::GetPositionIK::Request &request, + kinematics_msgs::GetPositionIK::Response &response) { + + geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped; + tf::Stamped transform; + tf::poseStampedMsgToTF( pose_msg_in, transform ); + + //Do the IK + KDL::JntArray jnt_pos_in; + KDL::JntArray jnt_pos_out; + jnt_pos_in.resize(num_joints); + for (unsigned int i=0; i < num_joints; i++) { + int tmp_index = getJointIndex(request.ik_request.ik_seed_state.joint_state.name[i]); + if (tmp_index >=0) { + jnt_pos_in(tmp_index) = request.ik_request.ik_seed_state.joint_state.position[i]; + } else { + ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.ik_seed_state.joint_state.name[i].c_str()); + } + } + + // populate F_dest from tf::Transform parameter + KDL::Frame F_dest; + tf::TransformTFToKDL(transform, F_dest); + + int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out); + + if (ik_valid >= 0) { + response.solution.joint_state.name = info.joint_names; + response.solution.joint_state.position.resize(num_joints); + for (unsigned int i=0; i < num_joints; i++) { + response.solution.joint_state.position[i] = jnt_pos_out(i); + ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i)); + } + response.error_code.val = response.error_code.SUCCESS; + return true; + } else { + ROS_DEBUG("An IK solution could not be found"); + response.error_code.val = response.error_code.NO_IK_SOLUTION; + return true; + } +} + +bool Kinematics::getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request, + kinematics_msgs::GetKinematicSolverInfo::Response &response) { + response.kinematic_solver_info = info; + return true; +} + +bool Kinematics::getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request, + kinematics_msgs::GetKinematicSolverInfo::Response &response) { + response.kinematic_solver_info = info; + return true; +} + +bool Kinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request, + kinematics_msgs::GetPositionFK::Response &response) { + KDL::Frame p_out; + KDL::JntArray jnt_pos_in; + geometry_msgs::PoseStamped pose; + tf::Stamped tf_pose; + + jnt_pos_in.resize(num_joints); + for (unsigned int i=0; i < num_joints; i++) { + int tmp_index = getJointIndex(request.robot_state.joint_state.name[i]); + if (tmp_index >=0) + jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i]; + } + + response.pose_stamped.resize(request.fk_link_names.size()); + response.fk_link_names.resize(request.fk_link_names.size()); + + bool valid = true; + for (unsigned int i=0; i < request.fk_link_names.size(); i++) { + int segmentIndex = getKDLSegmentIndex(request.fk_link_names[i]); + ROS_DEBUG("End effector index: %d",segmentIndex); + ROS_DEBUG("Chain indices: %d",chain.getNrOfSegments()); + if (fk_solver->JntToCart(jnt_pos_in,p_out,segmentIndex) >=0) { + tf_pose.frame_id_ = root_name; + tf_pose.stamp_ = ros::Time(); + tf::PoseKDLToTF(p_out,tf_pose); + try { + tf_listener.transformPose(request.header.frame_id,tf_pose,tf_pose); + } catch (...) { + ROS_ERROR("Could not transform FK pose to frame: %s",request.header.frame_id.c_str()); + response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; + return false; + } + tf::poseStampedTFToMsg(tf_pose,pose); + response.pose_stamped[i] = pose; + response.fk_link_names[i] = request.fk_link_names[i]; + response.error_code.val = response.error_code.SUCCESS; + } else { + ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str()); + response.error_code.val = response.error_code.NO_FK_SOLUTION; + valid = false; + } + } + return true; +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "arm_kinematics"); + Kinematics k; + if (k.init()<0) { + ROS_ERROR("Could not initialize kinematics node"); + return -1; + } + + ros::spin(); + return 0; +} + diff --git a/arm_kinematics/mainpage.dox b/arm_kinematics/mainpage.dox new file mode 100644 index 0000000..6f761b9 --- /dev/null +++ b/arm_kinematics/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b arm_kinematics is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/arm_kinematics/manifest.xml b/arm_kinematics/manifest.xml new file mode 100644 index 0000000..1c73d59 --- /dev/null +++ b/arm_kinematics/manifest.xml @@ -0,0 +1,20 @@ + + + + arm_kinematics + + + David Lu!! + BSD + + http://ros.org/wiki/arm_kinematics + + + + + + + + + + diff --git a/easy_state_publisher/CMakeLists.txt b/easy_state_publisher/CMakeLists.txt new file mode 100644 index 0000000..a351fcd --- /dev/null +++ b/easy_state_publisher/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +rosbuild_add_executable(easy_state_publisher src/easy_state_publisher.cpp) +rosbuild_add_executable(easy_tf_publisher src/easy_tf_publisher.cpp) + diff --git a/easy_state_publisher/Makefile b/easy_state_publisher/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/easy_state_publisher/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/easy_state_publisher/mainpage.dox b/easy_state_publisher/mainpage.dox new file mode 100644 index 0000000..48fd4ab --- /dev/null +++ b/easy_state_publisher/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b easy_state_publisher is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/easy_state_publisher/manifest.xml b/easy_state_publisher/manifest.xml new file mode 100644 index 0000000..3083440 --- /dev/null +++ b/easy_state_publisher/manifest.xml @@ -0,0 +1,16 @@ + + + Easy State Publisher publishes JointState and/or TF messages for a given robot model + + David Lu!! + BSD + + http://ros.org/wiki/easy_state_publisher + + + + + + + + diff --git a/easy_state_publisher/src/easy_state_publisher.cpp b/easy_state_publisher/src/easy_state_publisher.cpp new file mode 100644 index 0000000..784ad52 --- /dev/null +++ b/easy_state_publisher/src/easy_state_publisher.cpp @@ -0,0 +1,58 @@ +#include +#include +#include +#include +#include +#include + +int main(int argc, char** argv) { + ros::init(argc, argv, "easy_state_publisher"); + ros::NodeHandle n; + ros::Publisher m_joint_pub = n.advertise + ("joint_states", 1); + ros::Rate loop_rate(2); + + urdf::Model my_model; + if (!my_model.initParam("/robot_description")) { + ROS_ERROR("Failed to parse urdf robot model"); + return false; + } + + typedef std::map > joint_map; + typedef std::map position_map; + joint_map joints = my_model.joints_; + position_map positions; + + int count = 0; + for (joint_map::const_iterator it = joints.begin(); it != joints.end(); ++it) { + std::string name = it->first; + urdf::Joint* joint = it->second.get(); + if (joint->type!=urdf::Joint::FIXED) { + positions[name] = 0; + count++; + ROS_INFO("%s", name.c_str()); + } + } + + sensor_msgs::JointState joint_state; + joint_state.name.resize(count); + joint_state.position.resize(count); + joint_state.velocity.resize(count); + int i = 0; + for (position_map::const_iterator it = positions.begin(); it != positions.end(); ++it) { + joint_state.name[i++] = it->first.c_str(); + } + + while (ros::ok()) { + + joint_state.header.stamp = ros::Time::now(); + m_joint_pub.publish(joint_state); + + + // This will adjust as needed per iteration + loop_rate.sleep(); + } + + + return 0; +} diff --git a/easy_state_publisher/src/easy_tf_publisher.cpp b/easy_state_publisher/src/easy_tf_publisher.cpp new file mode 100644 index 0000000..72e4a6b --- /dev/null +++ b/easy_state_publisher/src/easy_tf_publisher.cpp @@ -0,0 +1,50 @@ +#include +#include +#include +#include +#include +#include +#include + +int main(int argc, char** argv) { + ros::init(argc, argv, "easy_state_publisher"); + ros::NodeHandle n; + ros::Rate loop_rate(30); + + urdf::Model my_model; + if (!my_model.initParam("/robot_description")) { + ROS_ERROR("Failed to parse urdf robot model"); + return false; + } + KDL::Tree my_tree; + if (!kdl_parser::treeFromUrdfModel(my_model, my_tree)) { + ROS_ERROR("Failed to construct kdl tree"); + return false; + } + robot_state_publisher::RobotStatePublisher* publisher = new robot_state_publisher::RobotStatePublisher(my_tree); + + typedef std::map > joint_map; + + std::map positions; + joint_map joints = my_model.joints_; + + for (joint_map::const_iterator it = joints.begin(); it != joints.end(); ++it) { + std::string name = it->first; + urdf::Joint* joint = it->second.get(); + if (joint->type==urdf::Joint::REVOLUTE || joint->type==urdf::Joint::CONTINUOUS || + joint->type==urdf::Joint::PRISMATIC) + positions[name] = 0; + } + while (ros::ok()) { + publisher->publishTransforms(positions, ros::Time::now()); + + // Process a round of subscription messages + ros::spinOnce(); + + // This will adjust as needed per iteration + loop_rate.sleep(); + } + + + return 0; +} diff --git a/joint_controller/CMakeLists.txt b/joint_controller/CMakeLists.txt new file mode 100644 index 0000000..172441e --- /dev/null +++ b/joint_controller/CMakeLists.txt @@ -0,0 +1,5 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +rosbuild_init() + diff --git a/joint_controller/Makefile b/joint_controller/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/joint_controller/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/joint_controller/joint_controller b/joint_controller/joint_controller new file mode 100755 index 0000000..0673965 --- /dev/null +++ b/joint_controller/joint_controller @@ -0,0 +1,132 @@ +#!/usr/bin/python + +import roslib; roslib.load_manifest('joint_controller') +import rospy +import wx +import xml.dom.minidom +from sensor_msgs.msg import JointState + +RANGE = 1000 + +class JointController(wx.Frame): + def __init__(self, parent, id, title): + self.dependent_joints = rospy.get_param("dependent_joints", {}) + (self.free_joints, self.fixed_joints, ordered_joints) = self.getFreeJoints() + + wx.Frame.__init__(self, parent, id, title, (-1, -1)); + panel = wx.Panel(self, wx.ID_ANY); + box = wx.BoxSizer(wx.VERTICAL) + font = wx.Font(9, wx.SWISS, wx.NORMAL, wx.BOLD) + + ### Sliders ### + for name in ordered_joints: + joint = self.free_joints[name] + row = wx.GridSizer(1,2) + label = wx.StaticText(panel, -1, name) + label.SetFont(font) + row.Add(label, 1, wx.ALIGN_CENTER_VERTICAL) + + display = wx.TextCtrl (panel, value=str(0), style=wx.TE_READONLY | wx.ALIGN_RIGHT) + + row.Add(display, flag= wx.ALIGN_RIGHT| wx.ALIGN_CENTER_VERTICAL) + box.Add(row, 1, wx.EXPAND) + slide = wx.Slider(panel, -1, RANGE/2, 0, RANGE, style= wx.SL_AUTOTICKS | wx.SL_HORIZONTAL) + slide.SetFont(font) + box.Add(slide, 1, wx.EXPAND) + + joint['display'] = display + joint['slider'] = slide + + ### Buttons ### + self.ctrbutton = wx.Button(panel, 1, 'Center') + self.Bind(wx.EVT_SLIDER, self.sliderUpdate) + + wx.EVT_BUTTON(self, 1, self.center) + + box.Add(self.ctrbutton, 0, wx.EXPAND) + + panel.SetSizer(box) + self.Center() + + self.pub = rospy.Publisher('joint_states', JointState) + + box.Fit(self) + self.publish() + + def getFreeJoints(self): + description = rospy.get_param("robot_description") + robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0] + joints = {} + fixed_joints = {} + ordered_joints = [] + + for child in robot.childNodes: + if child.nodeType is child.TEXT_NODE: + continue + if child.localName == 'joint': + jtype = child.getAttribute('type') + if jtype == 'fixed': + continue + name = child.getAttribute('name') + if jtype == 'continuous': + minval = -3.1415 + maxval = 3.1415 + else: + limit = child.getElementsByTagName('limit')[0] + minval = float(limit.getAttribute('lower')) + maxval = float(limit.getAttribute('upper')) + if minval == maxval: + fixed_joints[name] = minval + continue + if name in self.dependent_joints: + continue + zeroval = (int)(-minval * RANGE / (maxval - minval)) + joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'slidervalue':zeroval, 'value':0 } + joints[name] = joint + ordered_joints.append(name) + return (joints, fixed_joints, ordered_joints) + + def center(self, event): + rospy.loginfo("Centering") + for (name,joint) in self.free_joints.items(): + joint['slidervalue'] = joint['zero'] + self.publish() + + def sliderUpdate(self, event): + for (name,joint) in self.free_joints.items(): + joint['slidervalue'] = joint['slider'].GetValue() + self.publish() + + def publish(self): + msg = JointState() + msg.header.stamp = rospy.Time.now() + + for (name,joint) in self.free_joints.items(): + msg.name.append(str(name)) + purevalue = joint['slidervalue'] + pctvalue = purevalue / float( RANGE) + value = joint['min'] + (joint['max']-joint['min']) * pctvalue + joint['value'] = value + msg.position.append(value) + joint['slider'].SetValue(purevalue) + joint['display'].SetValue("%.2f"%value) + for (name,value) in self.fixed_joints.items(): + msg.name.append(str(name)) + msg.position.append(value) + for (name,param) in self.dependent_joints.items(): + msg.name.append(str(name)) + parent = param['parent'] + baseval = self.free_joints[parent]['value'] + value = baseval * param.get('factor', 1) + msg.position.append(value) + self.pub.publish(msg) + +if __name__ == '__main__': + try: + rospy.init_node('joint_controller') + app = wx.App() + gui = JointController(None, -1, "Joint Controller") + gui.Show() + app.MainLoop() + except rospy.ROSInterruptException: pass + diff --git a/joint_controller/manifest.xml b/joint_controller/manifest.xml new file mode 100644 index 0000000..97d6fbd --- /dev/null +++ b/joint_controller/manifest.xml @@ -0,0 +1,14 @@ + + + A node for inputting joint angles directly + + David Lu!! + BSD + + http://ros.org/wiki/joint_controller + + + + + + diff --git a/simmechanics_to_urdf/CMakeLists.txt b/simmechanics_to_urdf/CMakeLists.txt new file mode 100644 index 0000000..f8f1c9c --- /dev/null +++ b/simmechanics_to_urdf/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) diff --git a/simmechanics_to_urdf/Makefile b/simmechanics_to_urdf/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/simmechanics_to_urdf/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/simmechanics_to_urdf/convert.py b/simmechanics_to_urdf/convert.py new file mode 100755 index 0000000..412590e --- /dev/null +++ b/simmechanics_to_urdf/convert.py @@ -0,0 +1,732 @@ +#!/usr/bin/python + +import roslib; roslib.load_manifest('simmechanics_to_urdf') +import rospy +import sys +import tf +from tf.transformations import euler_from_quaternion, quaternion_from_matrix +import xml.dom.minidom +from xml.dom.minidom import Document +import threading +import math +import numpy +import yaml + +# Conversion Factors +INCH2METER = 0.0254 +SLUG2KG = 14.5939029 +SLUGININ2KGMM = .009415402 +MM2M = .001 + +# Special Reference Frame(s) +WORLD = "WORLD" + +# Arbitrary List of colors to give pieces different looks +COLORS =[("green", "0 1 0 1"), ("black", "0 0 0 1"), ("red", "1 0 0 1"), + ("blue", "0 0 1 1"), ("yellow", "1 1 0 1"), ("pink", "1 0 1 1"), + ("cyan", "0 1 1 1"), ("green", "0 1 0 1"), ("white", "1 1 1 1"), + ("dblue", "0 0 .8 1"), ("dgreen", ".1 .8 .1 1"), ("gray", ".5 .5 .5 1")] + +class Converter: + def __init__(self): + # initialize member variables + self.links = {} + self.frames = {} + self.joints = {} + self.names = {} + self.colormap = {} + self.colorindex = 0 + self.usedcolors = {} + + # Start the Transform Manager + self.tfman = TransformManager() + self.tfman.start() + + # Extra Transforms for Debugging + self.tfman.add([0,0,0], [0.70682518,0,0,0.70682518], "ROOT", WORLD) # rotate so Z axis is up + + def convert(self, filename, configfile, mode): + self.mode = mode + + # Parse the configuration file + self.parseConfig(configfile) + + # Parse the input file + self.parse(xml.dom.minidom.parse(filename)) + self.buildTree(self.root) + + # Create the output + self.result = Document() + self.output(self.root) + + # output the output + if mode == "xml": + print self.result.toprettyxml() + if mode == "graph": + print self.graph() + if mode == "groups": + print self.groups(root) + + def parseConfig(self, configFile): + """Parse the Configuration File, if it exists. + Set the fields the default if the config does + not set them """ + if configFile == None: + configuration = {} + else: + configuration = yaml.load(file(configFile, 'r')) + if configuration == None: + configuration = {} + + self.freezeList = [] + self.redefinedjoints = {} + + self.root = configuration.get('root', None) + self.extrajoints = configuration.get('extrajoints', {}) + self.filenameformat = configuration.get('filenameformat', "%s") + self.forcelowercase = configuration.get('forcelowercase', False) + self.scale = configuration.get('scale', None) + self.freezeAll = configuration.get('freezeAll', False) + self.baseframe = configuration.get('baseframe', WORLD) + + # Get lists converted to strings + self.removeList = [ str(e) for e in configuration.get('remove', []) ] + self.freezeList = [ str(e) for e in configuration.get('freeze', []) ] + + # Get map with key converted to strings + jointmap = configuration.get('redefinedjoints', {}) + for x in jointmap.keys(): + self.redefinedjoints[str(x)] = jointmap[x] + + # Add Extra Frames + for frame in configuration.get('moreframes', []): + self.tfman.add(frame['offset'], frame['orientation'], frame['parent'], frame['child']) + + + def parse(self, element): + """Recursively goes through all XML elements + and branches off for important elements""" + name = element.localName + # Grab name from root element AND recursively parse + if name == "PhysicalModelingXMLFile": + dict = getDictionary(element) + self.name = dict['name'] + + if name == "Body": + self.parseLink(element) + elif name == "SimpleJoint": + self.parseJoint(element) + elif name == "Ground": + dict = getDictionary(element) + self.parseFrames(dict['frame'], "GROUND") + else: + for child in element.childNodes: + self.parse(child) + + def parseLink(self, link): + """Parse the important bits of a link element""" + linkdict = getDictionary(link) + uid = self.getName(linkdict['name']) + linkdict['neighbors'] = [] + linkdict['children'] = [] + linkdict['jointmap'] = {} + + # Save the frames for separate parsing + frames = linkdict['frames'] + linkdict['frames'] = None + + # Save the color if it exists + if 'MaterialProp' in linkdict: + colorelement = linkdict['MaterialProp'][1] + color = colorelement.childNodes[0].data + linkdict['MaterialProp'] = None + linkdict['color'] = color.replace(",", " ") + " 1" + + + self.links[uid] = linkdict + self.parseFrames(frames, uid) + + # Save First Actual Element as Root, if not defined already + if self.root == None and "geometryFileName" in linkdict: + self.root = uid + + def parseFrames(self, frames, parent): + """Parse the frames from xml""" + for frame in frames: + if frame.nodeType is frame.TEXT_NODE: + continue + fdict = getDictionary(frame) + fid = str(frame.getAttribute("ref")) + fdict['parent'] = parent + + offset = getlist(fdict['position']) + units = fdict['positionUnits'] + for i in range(0, len(offset)): + offset[i] = convert(offset[i], units) + + orientation = getlist(fdict['orientation']) + quat = matrixToQuaternion(orientation) + + # If the frame does not have a reference number, + # use the name plus a suffix (for CG or CS1... + # otherwise ignore the frame + if fid == "": + name = fdict['name'] + if name == "CG": + fid = parent + "CG" + elif name == "CS1": + fid = parent + "CS1" + else: + continue + + self.tfman.add(offset, quat, WORLD, fid) + self.frames[fid] = fdict + + def parseJoint(self, element): + """Parse the joint from xml""" + dict = getDictionary(element) + joint = {} + joint['name'] = dict['name'] + uid = self.getName(joint['name']) + + frames = element.getElementsByTagName("Frame") + joint['parent'] = str(frames[0].getAttribute("ref")) + joint['child'] = str(frames[1].getAttribute("ref")) + type = element.getElementsByTagName("Primitive") + + # If there multiple elements, assume a fixed joint + if len(type)==1: + pdict = getDictionary(type[0]) + joint['type'] = pdict['name'] + joint['axis'] = pdict['axis'] + if joint['type'] == 'weld': + joint['type'] = 'fixed' + else: + joint['type'] = 'fixed' + + # Ignore joints on the remove list + if joint['parent'] in self.removeList: + return + + # Force joints to be fixed on the freezeList + if joint['parent'] in self.freezeList or self.freezeAll: + joint['type'] = 'fixed' + + # Redefine specified joints on redefined list + if joint['parent'] in self.redefinedjoints.keys(): + jdict = self.redefinedjoints[joint['parent']] + if 'name' in jdict: + uid = jdict['name'] + + # Default to continuous joints + joint['type'] = jdict.get('type', 'continuous') + + if 'axis' in jdict: + joint['axis'] = jdict['axis'] + if 'limits' in jdict: + joint['limits'] = jdict['limits'] + + self.joints[uid] = joint + + def buildTree(self, root): + """Reduce the graph structure of links and joints to a tree + by breadth first search. Then construct new coordinate frames + from new tree structure""" + + # Create a list of all neighboring links at each link + for jid in self.joints: + jointdict = self.joints[jid] + if "Root" in jointdict['name']: + continue + pid = self.getLinkNameByFrame(jointdict['parent']) + cid = self.getLinkNameByFrame(jointdict['child']) + parent = self.links[pid] + child = self.links[cid] + + parent['neighbors'].append(cid) + parent['jointmap'][cid] = jid + child['neighbors'].append(pid) + child['jointmap'][pid] = jid + + # Add necessary information for any user-defined joints + for (name, extrajoint) in self.extrajoints.items(): + pid = extrajoint['pid'] + cid = extrajoint['cid'] + jorigin = extrajoint['jorigin'] + newframe = name + "_frame" + + self.links[pid]['neighbors'].append(cid) + self.links[pid]['jointmap'][cid] = name + self.links[cid]['neighbors'].append(pid) + self.links[cid]['jointmap'][pid] = name + self.joints[name] = {'name': name, 'parent': jorigin, 'child': newframe} + for (k,v) in extrajoint['attributes'].items(): + self.joints[name][k] = v + self.frames[jorigin] = {'parent': pid} + self.frames[newframe] = {'parent': cid} + + # Starting with designated root node, perform BFS to + # create the tree + queue = [ root ] + self.links[root]['parent'] = "GROUND" + while len(queue) > 0: + id = queue.pop(0) + link = self.links[id] + for n in link['neighbors']: + nbor = self.links[n] + # if a neighbor has not been visited yet, + # add it as a child node + if not 'parent' in nbor: + nbor['parent'] = id + queue.append(n) + link['children'].append(n) + + # build new coordinate frames + for id in self.links: + link = self.links[id] + if not 'parent' in link: + continue + parentid = link['parent'] + if parentid == "GROUND": + ref = self.baseframe + else: + joint = self.joints[link['jointmap'][parentid]] + ref = joint['parent'] + # The root of each link is the offset to the joint + # and the rotation of the CS1 frame + (off1, rot1) = self.tfman.get(WORLD, ref) + (off2, rot2) = self.tfman.get(WORLD, id + "CS1") + self.tfman.add(off1, rot2, WORLD, "X" + id) + + + def output(self, rootid): + """Creates the URDF from the parsed document. + Makes the document and starts the recursive build process""" + doc = self.result; + self.root = doc.createElement("robot") + doc.appendChild(self.root) + self.root.setAttribute("name", self.name) + self.outputLink(rootid) + self.processLink(rootid) + + def processLink(self, id): + """ Creates the output for the specified node's + child links, the connecting joints, then + recursively processes each child """ + link = self.links[id] + for cid in link['children']: + jid = link['jointmap'][cid] + + self.outputLink(cid) + self.outputJoint(jid, id) + self.processLink(cid) + + def outputLink(self, id): + """ Creates the URDF output for a single link """ + doc = self.result + linkdict = self.links[id] + if linkdict['name'] == "RootPart": + return + + link = doc.createElement("link") + link.setAttribute("name", id) + + visual = doc.createElement("visual") + inertial = doc.createElement("inertial") + collision = doc.createElement("collision") + link.appendChild(visual) + link.appendChild(inertial) + link.appendChild(collision) + + # Define Geometry + geometry = doc.createElement("geometry") + mesh = doc.createElement("mesh") + filename = linkdict['geometryFileName'] + if self.forcelowercase: + filename = filename.lower() + + mesh.setAttribute("filename", self.filenameformat % filename) + if self.scale != None: + mesh.setAttribute("scale", self.scale) + geometry.appendChild(mesh) + visual.appendChild(geometry) + collision.appendChild(geometry.cloneNode(1)) + + # Define Inertial Frame + mass = doc.createElement("mass") + units = linkdict['massUnits'] + massval = convert(float(linkdict['mass']), units) + mass.setAttribute("value", str(massval)) + inertial.appendChild(mass) + + inertia = doc.createElement("inertia") + matrix = getlist(linkdict["inertia"]) + units = linkdict['inertiaUnits'] + + for i in range(0,len(matrix)): + matrix[i] = convert(matrix[i], units) + + inertia.setAttribute("ixx", str(matrix[0])) + inertia.setAttribute("ixy", str(matrix[1])) + inertia.setAttribute("ixz", str(matrix[2])) + inertia.setAttribute("iyy", str(matrix[4])) + inertia.setAttribute("iyz", str(matrix[5])) + inertia.setAttribute("izz", str(matrix[8])) + inertial.appendChild(inertia) + + # Inertial origin is the center of gravity + iorigin = doc.createElement("origin") + (off, rot) = self.tfman.get("X" + id, id+"CG") + rpy = list(euler_from_quaternion(rot)) + iorigin.setAttribute("xyz", " ".join(map(str,zero(off)))) + iorigin.setAttribute("rpy", " ".join(map(str,zero(rpy)))) + inertial.appendChild(iorigin) + + # Create link's origin + origin = doc.createElement("origin") + + # Visual offset is difference between origin and CS1 + (off, rot) = self.tfman.get("X" + id, id+"CS1") + rpy = list(euler_from_quaternion(rot)) + origin.setAttribute("xyz", " ".join(map(str,zero(off)))) + origin.setAttribute("rpy", " ".join(map(str,zero(rpy)))) + visual.appendChild(origin) + collision.appendChild(origin.cloneNode(1)) + + # Define Material + material = doc.createElement("material") + + # Use specified color, if exists. Otherwise, get random color + if 'color' in linkdict: + cname = "%s_color"%id + rgba = linkdict['color'] + else: + (cname, rgba) = self.getColor(linkdict['name']) + material.setAttribute("name", cname) + visual.appendChild(material) + + # If color has already been output, only output name + if not cname in self.usedcolors: + color = doc.createElement("color") + color.setAttribute("rgba", rgba) + material.appendChild(color) + self.usedcolors[cname] = True + + self.root.appendChild(link) + + def getColor(self, s): + """ Gets a two element list containing a color name, + and it's rgba. The color selected is based on the mesh name. + If already seen, returns the saved color + Otherwise, returns the next available color""" + if s in self.colormap: + return self.colormap[s] + color = COLORS[self.colorindex] + self.colormap[s] = color + self.colorindex = (self.colorindex + 1) % len(COLORS) + return color + + def outputJoint(self, id, parentname): + """ Outputs URDF for a single joint """ + doc = self.result + jointdict = self.joints[id] + + if "Root" in jointdict['name']: + return + + joint = doc.createElement("joint") + joint.setAttribute('name', id) + + # Define the parent and child + pid = self.getLinkNameByFrame(jointdict['parent']) + cid = self.getLinkNameByFrame(jointdict['child']) + + # If the original joint was reversed while building the tree, + # swap the two ids + if parentname != pid: + cid = pid + pid = parentname + + parent = doc.createElement("parent") + child = doc.createElement("child") + parent.setAttribute('link', pid) + child.setAttribute('link', cid) + joint.appendChild(parent) + joint.appendChild(child) + + # Define joint type + jtype = jointdict['type'] + joint.setAttribute('type', jtype) + if 'limits' in jointdict: + limit = doc.createElement("limit") + for (k,v) in jointdict['limits'].items(): + limit.setAttribute(str(k), str(v)) + joint.appendChild(limit) + if 'axis' in jointdict and jtype != 'fixed': + axis = doc.createElement("axis") + xyz = jointdict['axis'].replace(',', ' ') + axis.setAttribute('xyz', xyz) + joint.appendChild(axis) + + # Define the origin + origin = doc.createElement("origin") + (off, rot) = self.tfman.get("X" + pid, "X" + cid) + rpy = list(euler_from_quaternion(rot)) + off = zero(off) + rpy = zero(rpy) + origin.setAttribute("xyz", " ".join(map(str,off))) + origin.setAttribute("rpy", " ".join(map(str,rpy))) + joint.appendChild(origin) + + self.root.appendChild(joint) + + def getName(self, basename): + """Return a unique name of the format + basenameD where D is the lowest number + to make the name unique""" + index = 1 + name = basename + str(index) + while name in self.names: + index = index + 1 + name = basename + str(index) + self.names[name] = 1 + return name + + def getLinkNameByFrame(self, key): + """Gets the link name from the frame object""" + return self.frames[key]['parent'] + + def graph(self): + """For debugging purposes, output a graphviz + representation of the tree structure, noting + which joints have been reversed and which have + been removed""" + graph = "digraph proe {\n" + for jkey in self.joints: + joint = self.joints[jkey] + pref = joint['parent'] + cref = joint['child'] + label = pref + ":" + cref + pkey = self.getLinkNameByFrame(pref) + ckey = self.getLinkNameByFrame(cref) + case = 'std' + if pkey != "GROUND": + parent = self.links[pkey] + if not ckey in parent['children']: + child = self.links[ckey] + if pkey in child['children']: + case = 'rev' + else: + case = 'not' + pkey = pkey.replace("-", "_") + ckey = ckey.replace("-", "_") + + if (case == 'std' or case == 'rev') and (joint['type'] != "fixed"): + style = " penwidth=\"5\"" + else: + style = ""; + + if case == 'std': + s = pkey + " -> " + ckey + " [ label = \""+label+"\""; + elif case == 'not': + s = pkey + " -> " + ckey + " [ label = \""+label+"\" color=\"yellow\"" + elif case == 'rev': + s = ckey + " -> " + pkey + " [ label = \""+label+"\" color=\"blue\"" + s = s + style + "];" + + if not "Root" in s and "-> SCR_" not in s: + graph = graph + s + "\n" + return graph + "}\n" + + def groups(self, root): + """ For planning purposes, print out lists of + all the links between the different joints""" + self.groups = {} + self.makeGroup(root, "BASE") + s = "" + for key in self.groups.keys(): + s = s + key + ":\n\t" + ids = self.groups[key] + for id in ids: + s = s+id + " " + s = s + "\n\n" + return s + + def makeGroup(self, id, gid): + """ Helper function for recursively gathering + groups of joints. """ + if gid in self.groups: + idlist = self.groups[gid] + idlist.append(id) + else: + idlist = [id] + self.groups[gid] = idlist + link = self.links[id] + for child in link['children']: + jid = link['jointmap'][child] + joint = self.joints[jid] + if joint['type'] == 'weld': + ngid = gid + else: + ngid = jid + + self.makeGroup(child, ngid) + +def getDictionary(tag): + """Builds a dictionary from the specified xml tag + where each child of the tag is entered into the dictionary + with the name of the child tag as the key, and the contents + as the value. Also removes quotes from quoted values""" + x = {} + for child in tag.childNodes: + if child.nodeType is child.TEXT_NODE: + continue + key = str(child.localName) + if len(child.childNodes) == 1: + data = str(child.childNodes[0].data) + if data[0] == '"' and data[-1] == '"': + if len(data) != 2: + x[key] = data[1:-1] + else: + x[key] = data + else: + data = child.childNodes + x[key] = data + return x + +def getlist(string): + """Splits a string of comma delimited floats to + a list of floats""" + slist = string.split(",") + flist = [] + for s in slist: + flist.append(float(s)) + return flist + + +def convert(value, units): + """Convert value from the specified units to mks units""" + if units == 'kg' or units == 'm' or units == 'kg*m^2': + return value + elif units == 'slug*in^2': + return value * SLUGININ2KGMM + elif units == 'slug': + return value * SLUG2KG + elif units == 'in': + return value * INCH2METER + elif units == 'mm': + return value * MM2M + else: + raise Exception("unsupported mass unit: %s" % units) + +def matrixToQuaternion(matrix): + """Concert 3x3 rotation matrix into a quaternion""" + (R11, R12, R13, R21, R22, R23, R31, R32, R33) = matrix + # Build 4x4 matrix + M = [[R11, R21, R31, 0], + [R12, R22, R32, 0], + [R13, R23, R33, 0], + [0, 0, 0, 1]] + A = numpy.array(M) + [w,x,y,z] = quaternion_from_matrix(A) + return [w,x,y,z] + +def quaternion_to_rpy(quat): + """Convert quaternion into roll pitch yaw list (in degrees)""" + rpy = list(euler_from_quaternion(quat)) + for i in range(0, len(rpy)): + rpy[i] = rpy[i]*180/math.pi + return rpy + +def zero(arr): + """Converts any numbers less than 1e-7 to 0 in the array""" + for i in range(0,len(arr)): + if math.fabs(arr[i]) < 1e-7: + arr[i] = 0 + return arr + + +class TransformManager(threading.Thread): + """Bridge to the ROS tf package. Constantly broadcasts + all tfs, and retrieves them on demand.""" + + def __init__(self): + threading.Thread.__init__(self) + self.running = True + self.tfs = [] + self.listener = tf.TransformListener() + + def run(self): + """Broadcasts tfs until shutdown""" + rate = rospy.Rate(10.0) + br = tf.TransformBroadcaster() + while self.running: + for transform in self.tfs: + br.sendTransform(transform['trans'], + transform['rot'], + rospy.Time.now(), + transform['child'], + transform['parent']) + rate.sleep() + + def add(self, trans, rot, parent, child): + """Adds a new transform to the set""" + tf = {'trans':trans, 'rot':rot, 'child':child, 'parent':parent} + self.tfs.append(tf) + + def get(self, parent, child): + """Attempts to retrieve a transform""" + attempts = 0 + rate = rospy.Rate(10.0) + while attempts < 50: + try: + (off,rpy) = self.listener.lookupTransform(parent, child, rospy.Time(0)) + return [list(off), list(rpy)] + except (tf.LookupException, tf.ConnectivityException): + attempts+=1 + rate.sleep() + continue + + raise Exception("Attempt to transform %s exceeded attempt limit" % (parent + "/" + child)) + + def printTransform(self, parent, child): + """Attempts to print the specified transform""" + (off, rot) = self.get(parent, child) + rpy = quaternion_to_rpy(rot) + + s = parent + "-->" + child + l = (30 - len(s))*" " + print "%s%s[%+.5f %+.5f %+.5f] \t [%+3.3f %+.3f %+.3f] \t [%+.6f %+.6f %+.6f %+.6f] " \ + % (s, l, off[0], off[1], off[2], rpy[0], rpy[1], rpy[2], rot[0], rot[1], rot[2], rot[3]) + + def kill(self): + """Stops thread from running""" + self.running = False + +if __name__ == '__main__': + argc = len(sys.argv) + if argc == 3: + filename = sys.argv[1] + config = None + mode = sys.argv[2] + elif argc == 4: + filename = sys.argv[1] + config = sys.argv[2] + mode = sys.argv[3] + else: + print "Usage: " + sys.argv[0] + " {XML filename} [configfile] {tf|xml|graph|groups|none}" + sys.exit(-1) + try: + rospy.init_node('convert') + con = Converter() + try: + con.convert(filename, config, mode) + while mode == "tf" and not rospy.is_shutdown(): + None + except Exception: + raise + finally: + con.tfman.kill() + + except rospy.ROSInterruptException: pass + diff --git a/simmechanics_to_urdf/convertToBinaries.py b/simmechanics_to_urdf/convertToBinaries.py new file mode 100755 index 0000000..8dd6877 --- /dev/null +++ b/simmechanics_to_urdf/convertToBinaries.py @@ -0,0 +1,18 @@ +#!/usr/bin/python +import os +import sys +import subprocess + +if __name__ == '__main__': + if len(sys.argv) != 2: + print "Usage: " + sys.argv[0] + " [directory]" + sys.exit(-1) + path= sys.argv[1] + dirList=os.listdir(path) + for fname in dirList: + path1 = path + fname + path2 = path + fname + "b" + cmd = "rosrun ivcon ivcon " + path1 + " " + path2 + proc = subprocess.Popen([cmd], stdout=subprocess.PIPE, shell=True) + (out, err) = proc.communicate() + print err diff --git a/simmechanics_to_urdf/mainpage.dox b/simmechanics_to_urdf/mainpage.dox new file mode 100644 index 0000000..f438819 --- /dev/null +++ b/simmechanics_to_urdf/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b simmechanics_to_urdf is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/simmechanics_to_urdf/manifest.xml b/simmechanics_to_urdf/manifest.xml new file mode 100644 index 0000000..89bf15d --- /dev/null +++ b/simmechanics_to_urdf/manifest.xml @@ -0,0 +1,15 @@ + + + Converts SimMechanics XML to URDF + + David Lu!! + BSD + + http://ros.org/wiki/simmechanics_to_urdf + + + + + + + diff --git a/stack.xml b/stack.xml new file mode 100644 index 0000000..6fcf8c4 --- /dev/null +++ b/stack.xml @@ -0,0 +1,9 @@ + + urdf_tools + Maintained by David Lu!! + BSD + + http://ros.org/wiki/urdf_tools + + +