diff --git a/arm_kinematics/CMakeLists.txt b/arm_kinematics/CMakeLists.txt deleted file mode 100644 index 63cf1cf..0000000 --- a/arm_kinematics/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -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 deleted file mode 100644 index b75b928..0000000 --- a/arm_kinematics/Makefile +++ /dev/null @@ -1 +0,0 @@ -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 deleted file mode 100644 index 375f99e..0000000 --- a/arm_kinematics/arm_kinematics.cpp +++ /dev/null @@ -1,362 +0,0 @@ -// 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::Stamped transform_root; - 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()); - } - } - - //Convert F to our root_frame - try { - tf_listener.transformPose(root_name, transform, transform_root); - } catch (...) { - ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str()); - response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; - return false; - } - - KDL::Frame F_dest; - tf::TransformTFToKDL(transform_root, 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 deleted file mode 100644 index 6f761b9..0000000 --- a/arm_kinematics/mainpage.dox +++ /dev/null @@ -1,26 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b arm_kinematics is ... - - - - -\section codeapi Code API - - - - -*/ diff --git a/arm_kinematics/manifest.xml b/arm_kinematics/manifest.xml deleted file mode 100644 index d1f5b45..0000000 --- a/arm_kinematics/manifest.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - A generic package for computing both forward and backward kinematics for arms. - Developed as an alternative to pr2_arm_kinematics for people not using the PR2. - - David Lu!! - BSD - - http://ros.org/wiki/arm_kinematics - - - - - - - - - - diff --git a/joint_state_publisher/CMakeLists.txt b/joint_state_publisher/CMakeLists.txt deleted file mode 100644 index 172441e..0000000 --- a/joint_state_publisher/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -rosbuild_init() - diff --git a/joint_state_publisher/Makefile b/joint_state_publisher/Makefile deleted file mode 100644 index b75b928..0000000 --- a/joint_state_publisher/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/joint_state_publisher/joint_state_publisher b/joint_state_publisher/joint_state_publisher deleted file mode 100755 index ae5aeb2..0000000 --- a/joint_state_publisher/joint_state_publisher +++ /dev/null @@ -1,206 +0,0 @@ -#!/usr/bin/python - -import roslib; roslib.load_manifest('joint_state_publisher') -import rospy -import wx -import xml.dom.minidom -from sensor_msgs.msg import JointState -from math import pi -from threading import Thread - -RANGE = 10000 - -def get_param(name, value=None): - private = "~%s" % name - if rospy.has_param(private): - return rospy.get_param(private) - elif rospy.has_param(name): - return rospy.get_param(name) - else: - return value - -class JointStatePublisher(): - def __init__(self): - description = get_param('robot_description') - robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0] - self.free_joints = {} - self.joint_list = [] # for maintaining the original order of the joints - self.dependent_joints = get_param("dependent_joints", {}) - - # Find all non-fixed 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 = -pi - maxval = pi - else: - limit = child.getElementsByTagName('limit')[0] - minval = float(limit.getAttribute('lower')) - maxval = float(limit.getAttribute('upper')) - - if name in self.dependent_joints: - continue - if minval > 0 or maxval < 0: - zeroval = (maxval + minval)/2 - else: - zeroval = 0 - - joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'value':zeroval } - self.free_joints[name] = joint - self.joint_list.append(name) - - use_gui = get_param("use_gui", False) - - if use_gui: - app = wx.App() - self.gui = JointStatePublisherGui("Joint State Publisher", self) - self.gui.Show() - Thread(target=app.MainLoop).start() - else: - self.gui = None - - source_list = get_param("source_list", []) - self.sources = [] - for source in source_list: - self.sources.append(rospy.Subscriber(source, JointState, self.source_cb)) - - self.pub = rospy.Publisher('joint_states', JointState) - - def source_cb(self, msg): - for i in range(len(msg.name)): - name = msg.name[i] - position = msg.position[i] - if name in self.free_joints: - joint = self.free_joints[name] - joint['value'] = position - if self.gui is not None: - self.gui.update_sliders() - - - def loop(self): - hz = get_param("rate", 10) # 10hz - r = rospy.Rate(hz) - - # Publish Joint States - while not rospy.is_shutdown(): - msg = JointState() - msg.header.stamp = rospy.Time.now() - - # Add Free Joints - for (name,joint) in self.free_joints.items(): - msg.name.append(str(name)) - msg.position.append(joint['value']) - - # Add Dependent Joints - for (name,param) in self.dependent_joints.items(): - parent = param['parent'] - baseval = self.free_joints[parent]['value'] - value = baseval * param.get('factor', 1) - - msg.name.append(str(name)) - msg.position.append(value) - - self.pub.publish(msg) - - r.sleep() - -class JointStatePublisherGui(wx.Frame): - def __init__(self, title, jsp): - wx.Frame.__init__(self, None, -1, title, (-1, -1)); - self.jsp = jsp - self.joint_map = {} - 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 self.jsp.joint_list: - joint = self.jsp.free_joints[name] - - if joint['min'] == joint['max']: - continue - - 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) - slider = wx.Slider(panel, -1, RANGE/2, 0, RANGE, - style= wx.SL_AUTOTICKS | wx.SL_HORIZONTAL) - slider.SetFont(font) - box.Add(slider, 1, wx.EXPAND) - - self.joint_map[name] = {'slidervalue':0, 'display':display, - 'slider':slider, 'joint':joint} - - ### Buttons ### - self.ctrbutton = wx.Button(panel, 1, 'Center') - self.Bind(wx.EVT_SLIDER, self.sliderUpdate) - - wx.EVT_BUTTON(self, 1, self.center_event) - - box.Add(self.ctrbutton, 0, wx.EXPAND) - - panel.SetSizer(box) - self.center() - box.Fit(self) - self.update_values() - - - def update_values(self): - for (name,joint_info) in self.joint_map.items(): - purevalue = joint_info['slidervalue'] - joint = joint_info['joint'] - value = self.sliderToValue(purevalue, joint) - joint['value'] = value - self.update_sliders() - - def update_sliders(self): - for (name,joint_info) in self.joint_map.items(): - joint = joint_info['joint'] - joint_info['slidervalue'] = self.valueToSlider(joint['value'], joint) - joint_info['slider'].SetValue(joint_info['slidervalue']) - joint_info['display'].SetValue("%.2f"%joint['value']) - - def center_event(self, event): - self.center() - - def center(self): - rospy.loginfo("Centering") - for (name,joint_info) in self.joint_map.items(): - joint = joint_info['joint'] - joint_info['slidervalue'] = self.valueToSlider(joint['zero'], joint) - self.update_values() - - def sliderUpdate(self, event): - for (name,joint_info) in self.joint_map.items(): - joint_info['slidervalue'] = joint_info['slider'].GetValue() - self.update_values() - - def valueToSlider(self, value, joint): - return (value - joint['min']) * float(RANGE) / (joint['max'] - joint['min']) - - def sliderToValue(self, slider, joint): - pctvalue = slider / float(RANGE) - return joint['min'] + (joint['max']-joint['min']) * pctvalue - - -if __name__ == '__main__': - try: - rospy.init_node('joint_state_publisher') - jsp = JointStatePublisher() - jsp.loop() - - except rospy.ROSInterruptException: pass - diff --git a/joint_state_publisher/manifest.xml b/joint_state_publisher/manifest.xml deleted file mode 100644 index 5447c5d..0000000 --- a/joint_state_publisher/manifest.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - A node for publishing joint angles, either through a gui, or with - default values. - - David Lu!! - BSD - - http://ros.org/wiki/joint_state_publisher - - - - - - diff --git a/joint_state_publisher/screenshot.png b/joint_state_publisher/screenshot.png deleted file mode 100644 index cb8a607..0000000 Binary files a/joint_state_publisher/screenshot.png and /dev/null differ diff --git a/simmechanics_to_urdf/CMakeLists.txt b/simmechanics_to_urdf/CMakeLists.txt deleted file mode 100644 index f8f1c9c..0000000 --- a/simmechanics_to_urdf/CMakeLists.txt +++ /dev/null @@ -1,30 +0,0 @@ -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 deleted file mode 100644 index b75b928..0000000 --- a/simmechanics_to_urdf/Makefile +++ /dev/null @@ -1 +0,0 @@ -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 deleted file mode 100755 index 412590e..0000000 --- a/simmechanics_to_urdf/convert.py +++ /dev/null @@ -1,732 +0,0 @@ -#!/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 deleted file mode 100755 index 8dd6877..0000000 --- a/simmechanics_to_urdf/convertToBinaries.py +++ /dev/null @@ -1,18 +0,0 @@ -#!/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 deleted file mode 100644 index f438819..0000000 --- a/simmechanics_to_urdf/mainpage.dox +++ /dev/null @@ -1,26 +0,0 @@ -/** -\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 deleted file mode 100644 index 89bf15d..0000000 --- a/simmechanics_to_urdf/manifest.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - Converts SimMechanics XML to URDF - - David Lu!! - BSD - - http://ros.org/wiki/simmechanics_to_urdf - - - - - - -