From f942743ea5a0753c303b1d7863cf625048b32331 Mon Sep 17 00:00:00 2001 From: Wim Meeussen Date: Mon, 28 Feb 2011 15:02:49 -0800 Subject: [PATCH] initial copy --- arm_kinematics/CMakeLists.txt | 19 - arm_kinematics/Makefile | 1 - arm_kinematics/arm_kinematics.cpp | 362 ---------- arm_kinematics/mainpage.dox | 26 - arm_kinematics/manifest.xml | 19 - joint_state_publisher/CMakeLists.txt | 5 - joint_state_publisher/Makefile | 1 - joint_state_publisher/joint_state_publisher | 206 ------ joint_state_publisher/manifest.xml | 15 - joint_state_publisher/screenshot.png | Bin 11070 -> 0 bytes simmechanics_to_urdf/CMakeLists.txt | 30 - simmechanics_to_urdf/Makefile | 1 - simmechanics_to_urdf/convert.py | 732 -------------------- simmechanics_to_urdf/convertToBinaries.py | 18 - simmechanics_to_urdf/mainpage.dox | 26 - simmechanics_to_urdf/manifest.xml | 15 - 16 files changed, 1476 deletions(-) delete mode 100644 arm_kinematics/CMakeLists.txt delete mode 100644 arm_kinematics/Makefile delete mode 100644 arm_kinematics/arm_kinematics.cpp delete mode 100644 arm_kinematics/mainpage.dox delete mode 100644 arm_kinematics/manifest.xml delete mode 100644 joint_state_publisher/CMakeLists.txt delete mode 100644 joint_state_publisher/Makefile delete mode 100755 joint_state_publisher/joint_state_publisher delete mode 100644 joint_state_publisher/manifest.xml delete mode 100644 joint_state_publisher/screenshot.png delete mode 100644 simmechanics_to_urdf/CMakeLists.txt delete mode 100644 simmechanics_to_urdf/Makefile delete mode 100755 simmechanics_to_urdf/convert.py delete mode 100755 simmechanics_to_urdf/convertToBinaries.py delete mode 100644 simmechanics_to_urdf/mainpage.dox delete mode 100644 simmechanics_to_urdf/manifest.xml 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 cb8a6079a3fde6f7abd651698e4a7c98203dbba4..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 11070 zcmb`NWmsI%mZlF-NP-7R5u6YR!3qfwf;$9AkV1nyAxLm7oFKv7-6g@@-Q6{~yHhZA zZ%TT?5$W0V#F z0H6Y-#e|iek`Ggy-3Tw?Pb+B&OLG?GeOt3%eSO7xk&r;3Y#~DN96~Xqf|uqwQySUK znOEQbbWN)qnjIc9P6f%_cT7t#_d`(`U*TZ-c~XT3<8e_t_R=K{Y1_I#f!PO#s~Xs- zlZLi$djyOH&+E3&?-~Wu5=I8myV}sgM=?i`(R_AA>F|7g^@6#-tQfd3C_D zzQXr#PPvwwzZFyw*e^m|*5)Kt1kHm2ZN4@-o$B{=g@$GfGN2J4#jA61@$b+s)f_qbE~} zHhJWEB!DkpzkcQP5HadXpdKPJb~1o~75_Q#M3$D8N=iyN|y}f-bOHz}G*qh2t zKu1YwB7wt1Lqnsp*}1l-yPOFCTX2zfK=|eJ9vYqJZSR+x7)knJJn-QZ9PRfwY3m0s zV;DO7IJOfTb~|}c6hHU#T#dmF)g5DC$37es^V^E&!bZ`2zb!BMxj&CcQ+e7a!`)`~ zTin)`Dwaos30yShAxyY{(8olLdapXSgDP`kY?ScSQ~~$9i^|H%tE(%eJb5G`d3kwD zOG^=vcG%;2n!mq)e&CRC1qtA(H|g@lf72NNTC#GQ;-okWs=(VB*q^NI>@RPe*|{R1 zzt8}HjSLrPtf{!nTrkb~**c-T+^^~&z&(NtESU^2lI14%yMAaS5IjCg7dA;&LUE#(FBdGT&(6*adO}b6aDmOu%_#cMsUIDW zq8XGlG&Pqj9SJ+>gaDuf=XD}us$NLlC$5K*gUTh#nl!=NWA4;iHk2{W@2NF8rMsEu zJ7IL{eYMV-!*hpMaMoh@<@6bE1N`7|hN3qQocPwQ(BhcD0IBuj*LP;EA?&o$z8Jbb zx|Op7j&mm9Fz1>mG=cNFo79-dI5g#aviVAzvLwZF$4u+&VQz#HK6OQKb)NQAIV)5o z$%v0dza!6+tKJdcB{+9B$I!53DR{q9(o$ap@o!4THXHQ#)gPk4&cVUK&VJxT9U;0Yb|)9)|pX zOzuNV3%PzQTM6E*(hqehtWR~DdseXRBb}DC#Si;l-zijx zZHqb%U(0Psy=pcxHhzWWUtuyb9`wOlPEJlwF9Wg_<~<8yk-M83{^`!7*WhvaT^blF zTW((c(~%loVx`(#L3_U}N%v&&%hb;)6hG8;`DuFm*sarT$Qn*xq=;#3)Rq+B_;96q zTf>}`e9Q>D4Lp8oh!etyK)V}X^~<*CBLr&qM|gQ6H|7SiUePd1)o0|y&<-BNk5Hym ztq!5ZavCmHZC`x}MgiU$E$JEaC$92=| zB51*pw9cv5E=~O^8<#F5{?yWnOUehPl^6-!|M6#bYeUD6x|+eG!Bmmoi!WZxpm&qG3`ycnO-(7~%Jz+nSecsU7ZnjK zRg{<0$tUleIu#ZcPLj@A2g|i0QVSc~I}`ox<7vS5>dNnhs@qVY)+aVoq;`n9ZQ!>Q_4#&f% z^Q+RbC9bQiIDTVXH(xwKTb;b=~H+?uxuZ0PCwXgA<{i9J$+40jV+l; z#avKuFf;KdXXm5j?Ivp(UTWTV$7SEmc z`&YH_tg`>YZ!i>lg&h~h(Es+TCOd+?{;K8XEaTTVW4W(CR>VG!U#&E{rv^nVLYhym z1GZ*L9@=mz)c9N%D-&A37SA`BO+3%&a-_a53Lt z1wSIFK|Zdx)?X16EZ)`o6%;Q?dwF?@SnVh}xkNSt|C#+wo?#2F*S_+}JPKM`a(;DZ z{E-H>4NJYo6>sS28AlWhDDSpgQr`mGA9X12Pj)*m)V$BH9USc&8nJMNz(cW}v_*5C zJf9c*99Y3o5=NZ_%L6@VCywuHcu3llx!%3^2Ugv*4iDui(ShFN*qB$Z_rsmLCngT< zoZYa{Ju$KvSOgfx^u_S_meS1>1Om4O8c4Kl@FcH!$>rnXwg6A6f_SxWikT$NF>GN% zcp|5#rwGX)npbpmbo~DPyQ*s3$T?{aN+HaQ* zi|yGqj+flMy zzLZa8LNxKO(vM(?g+VnIFqY>9(WX#uXC?_psi{09UJpo3(1qxEqWZM?A`xSu5uhR2 z#!pU8X7r;QJGGtj_1M5&A?#m;Fz`WMc;;4CFUhOu;NTECzL9&kW`BRQxB;S)L}3hd zrr5?tzByhn6IgcEnrvov)@g+Qnn|bmtJ(>LI|Qr3c6*pvvpR%QP(H5;)08v02W>lB|xQQ zELiRVzoRSxlODvNtnZXPS;Oxx_h-srcG@n{nz#AXip*FmN{@fbpJhoH8Ep;Sr-f-u z3Al9Gdpx8lV92@Vq@n`3yFOLiA)o%JF~vro);#SAnRsGjaM5Ow3Ow%T&Tzr3>m?WL zeE-suk z9tn$X$}Pr|^QzMhe_Yy;xh!5MHKab4U3@g^*WbhHbIK}lT4sKux=p9h>dI>#2CLo{ zvVZyda-G8UpQ~^mjB+(u5LN!S_Gqu^_`BeIf9}#vlhud}0Ej-Od@e@cS-hUlMIkqL z;WmkQq0@Co}G?^$_>Bj&?l%yNJjew@CW;4*_hul7DW4k962ya|eDe z<%3QL&vg$q8*DyU$dsfOE9K_dsi;(zXIHIm`E{NCbmUIvdp+B+Y)c0)LSsz4I!NHKTwW8fFARuYvB7FR$-ZI8D7;SJa`HS4&!%KqL}vs&dA=>^?LucGwx zhnR)sxjNUGqo^hzJUPX5!^IrusB8xf5#HWZT){>6-=(UDUJ&+ec~VIvr{R?HnH0Xy zDDjxNzi^H@DB?4y*qO0Bhs#lxoli{m-7Bk!6A14Pzo#Hf`p6sxI!P0pI=I&2Q6E8$ zKg-*Ue|@g6%R<2BeX82RDM-(c7*EZgUGgt7X(gdalz^vYnPt|GZrYzE5zW8LinmVu z6L$Oh3}WF$6%PS3gYd36M3J;o*Roi%>R26j^`IwgzFHL7X341Ie{FYp79R4Kqr{(8 zD};@$0~xTZ8R#i!bT6n*{fSNigonC>8y&nizyQ*_S1xO7A6uQK-q-yxVN_;*L8tgL zo|**u`zq?L%KeG2hLwG(+2-^9jAa=ARX1sgt;hQ=g%>?&Gi=1BG?(YeLRj!Yci}t4;LSgwW>ElV?JRrc&0{4 zvikB;Z@bBJ;|hh=txb1fS~c>j7swI{T5511d#2`|$C)0X0-{Inoo(>PVS1cD!I%U; zo$olT%IpQ-f8a1U;1<+**h(W_!1HWzINu9+QZ9J(`{a0w83u1UdZ?D9n|py65=(dT zMTXM^^?pdrHO4csLJrWA>O?-hzw>K0WB>90;}; zOza~qJf1eQwB$O<`ie#x)@tHoe>6>`lfLC&W2~|ZzILps(aNZV_+W|r%*-z@FGox) zOW{!A>Leg{cYJ(2J|3)hZWz%jE%=CmMhEh}zDbeKTR9H<>v+cygt$Mlo~p`9j8Me1 zoopa<7I-4G(z$tglGl@I>pY3MM#N~CXcLrbu`yNI_#&Lwc;G=#pZ^ddH3(Q1yei#7 z0dK(Uw7D_hRlp<;p(E=_kj>4`rd=zui}AH)atBdUd?dpuoB1fB5*i}(TbEN$ecHT= zmkcL$8x=_kh6+mg*@8h`G`07mF7xu1TS5y$*0W~gcakhzjP4sw^(=H<4D&V5^B!ao zAq%}sz?(`cGMd@w3qICBIj0_PlG=ep^xioQ58WmwQ4N!}f*@)YC?`Po3JI6OodBWSC{8C{-?cL?@Q+iO3JsokZ#yHF(>3f_z8ct@>iHtHk^Idp zZs^7b#9jPV`$XukNbz@4CmCMBL|UrVE=Jf+M#s;ubsL!+q$?H4NveycX)At4T&K)| zC1v?0_@`rnks`hxI5$7vRO&2*LEtD4#ybgW`fI)R?#?BseMKouL*htlJv}=+yRy34 z!NI}*k(foK*MiH$&=AYK8zPE<2rilrH6!D#3&o$Wv@^WT7I)Xa*?cwxV7=eU6?x7)Lahlh;e0jhH2 zp~PRL-2DQ+-t_SJN2*3P_Q^W%*ANMVorO>Xh)YO#7%^ypaQeSc2m^Eb5X=z%R=%x~e!Uz;X{etGZ>Ec^3!vY?Qz zjDgK{uRUib_ig{rKu(&;cBg)r$IP+Z2&Gl+7cE}b9-pwI`nSg}&OOU%d`l2mP!;Qw z*nh((a=E(1bh@=&q~E*FEg9-~xdV}xClp*CD)`Y08^8sW*Ru^XCB33cI%L1|(020-7x~Vp%TyD*_lVqtZ|fQ3%yKGrY&6dc z&L({tpWaGHbKc(Gsy$#MbM^Ot455V7a`VeWspdlT*r$~}IX$#xUf<&BtF3T%&#%&WN^Gm}NcK)1- z^30ewC8*r2OresE4Qm?-D#Sqg^g)8{E~lLn>w2dky0bu8OqBEq2dC|}w33f-q21#j*s0HNrL8V=V;m85 zI+VsD7bin&hv_pIXoCT`BsPG7RENr1pw;vjcY&`p*GD%#>&iGZU>xkQV!P%Or|q;h z1ADlKHMAxxm7C@=_gkGvB1JHO-!;AMQ<&O92htac7V?wXp6P$fW<6jm`p~Fn?DFF* z-sowM>1pk~E$lQ07ie`nV!dX*Njc2nhpC0X;(_64K^pUl;?O;BWQ&jh5&{k;K*+Jd z&0LhR>QapF1n?Ar;*QiWBqs9#oeNA6rbldwG`3R4{VnbhX4l>PB}^-}2fw&Awu_VX zk#x!F1s3fqeUg7a##i)=%^?(e?4i~*|sLHZgf zKgp_b>Z78Tf#;|8cCD8$p9lP@&^BAs84HqEmo==I>%5FF_%KuAW?_a?>2`6pWgyRY z-JRb7k+rq8H8V5Q)?WUyZDgJB!PXdIdLc6Rql&UJ2qS-EE7AhkF*9{AsM5Xosf@|d z3QFjR{ujiM_ASueTzA|6@l3T`-`5nLzT^s^$L9?oG4&qQONBk&w#m%FJbH3xa(yCbEOrVW%p(HPgmhfhfYeRZ-sAa-PP6A5sq%<(U$at9b#s( zvLP1dk*(4&^Sv({*c^j+%j=H3DIP6?K`Xm?ZxN=E4cW~2I1LYCzN;pC@xO&JB4`s* zQz@2|V4>@#iLvVcC`Dnq>JEmMmBd5*AT*9u-M0T+={k*5R$Uwu6Zfpel)31(iP8TH zTLcEwqE_zE{5xC3HeKK{#qanUsUt|jnx|84Jf{WOTfS4rH+;(69-%yUCmupyUSjpw z%yQy>^9`W=D^7SN`tn*LK)??ng?PyNn6HKKK+=)rIau$!_0(hOWK9Tymwqa`-+~El z32S1dp9*nW#UiBi)T>LrXP4a(Om}7HxsR#{Xo@$s7)CYXL^0Mo&ELZ7Zwl^Hp-j4T$Pxt4V*c!T61T5 zl$iZ(NHK8dCJ@#`pprj3I~!5AD(?TA+Y4d%q$;+YgVCGMvLqikOB>dL%9mbad!vzg zo$!60sI)j{Ln6Wy#S`;t2&%~f-3#*tkmUYmVhpVk(|fx~Ef4RS6C5aCo6p%1jqk|L z%VX4uRKmxSjx}etv##j^vK96N9>9E;%BCEuVz+=pVuTcecRyTcsvJ z_#FSl%YWGtsTT%!J|~@Y8ZCe6sg|ANc=Zv$A>mZHG&;J9U`Hqi26b>(WLzA*rh-4J z&ok1{^;e{%$93DO2or-8Yi!OJ>_1m$yB*7{*@?@5DQ05E=JR~yoF(qt?b+t-?X74^ zoUq|g;(JnV%V_scpnn4y1H*hAWXHCUzW}K|l zK!dTymKW>C_?<>|lJ{cyey7n-52Y(f99ey{n9-VCzCqnjQPuh=fR?_!(_r;|>2W!>1nS}G+s9Ep*-W!nwsywu>3c7W=+aUk}3 zM#Z2XC65-3jonrmwQK@JcL7uOLmxpz#=#at2IAP-@3}jbEz?A-X0S*g$W5ydDxzzp$y)5LNttd;zziJW?>w z%T(74SsVA*5&P<*lF9ah6-`3M5T{S}7GVb*F4yM8Y`TUvol5K{$0#uY#p|5`4WSOd z3r(hcF7PeJVI;#^jD9t1d#3m;66v!OK`U2+=LbS)JH9R^ZTpFWwcoc6Rhtl?Tpa3Mm_$_G!VnOTOd zYz#^|*^Z%D#j;dW1F*xbDcp}W$}JyM4~xyi1-zG3Vq34L59Z6W9NX+ww+ZDyC-&16 zcXp2IO=+sPuhPw(9KX+PV}CL+$s>}N&ytHlm$!Sz&z2&&GPhBCbvLJkSso?%3vGBw zLDi#pE+CIHLLT9p0fs5J8`XvMD2sy=I)&c|aiGJ6XuG2SgY+(%v+e!ARQFquHw53^ z!r-1PIQNMue|iX&D@)2Q zIB#Zo5QEK%L}s6fE-9eNP0&jBla)>KN-nP~35;J?1-0Ujm8#5bs;D?|Fo9>CFRn?MWvsP*loxj)kea- zP&o{($xPT25DDD3WgH6rE76k~>~vtw)2Mxnp`P%iw z^R9oK1z>>_;n~qV()bpu6b)gf+^AJqT0 z%14eD{-1T&qt4@Zd{4PR+Q|B;FVQ>AFY^SAyCl%T0#Mg+!ZGMnRX(&+C{R zKi~fEBEPl$oKJ3|yr~DI%!DP3hkEpGIrPN=`YD<538G6w_ARjeOTTFHU2p{~)~KWE zQ~YndEeIkg{c#)0AkoO|Ie=p}kukC+;?sXC#IK*n8X0kTj3!<%!u8O|^j_-!S0O$O zh6c>UGXmP5xD4ZE=^B<;1ndz@`VfRZKK81|2GNZn%xT%gF~)0i{1p&_Xe>=nPY(|d zkFK1FVW`vkQ3hJb=u@_J?t z5lxF2=-q|t|nyB2-tC;|64W5q2R#`g3r zrK)AB6C%#dPi7jMulS-b)1g{-tUOqX%Z4uz&AU&xT(yhsCw#2U+@zcm_$lAgr>3hg z=4qf=&$Y(;>0Zb@WJIenI(?aYTcok4vs;QqDpSupry^dom7&8vQD>O3^Wmb`$X)EM zLV>oW?-zaj%!R|)l*q7VUn%Iy=h!Xj_k^Zj7090o>*C(L>r9{6%qLdMbFkyfPmLiT zkjds_Jg3tXDku)eW0ad)HM9wW(hf*GJ2~R=O<&CT_%)R_*PXisxU{TX|^yS zIu)8aF7kvdsvE2@Bs)5t1troDzFT&>J++k$C1Vrh#TMmT<&o#giNo#l zRmY(;C8_#RoLI=DXOxqFr#!@xKUM1?nNQ~HnVB0cZAt_pjFV)f*Fc@LIp)p~%eNv? zCKoM6pUOA6Yf{m`w#aSrg>+H{$AS-p_?v zF89SsWfA*Gkyuh41a(w@!5d?ZijhcQ#89abR)}u&F5>~(WIi5TB)I>DuEeO-J}7JA zAv(1GW-7o?V=bnqoB|bjfHKHSg{IU=HC9sn#h<^?B*0;A*@`rJ$!I#8r-L5)^r~;9 zuY8bRjJ(DaUU5h4-B^pcMxacR7x8$V4<{)vxuqKMX9n~--S(#B47)XrJWzCfe1k1of!`vA)N{`d0SgeW)BDKx8LKP=AuM`c?F`ZJGbqB5t1n(L>9 zEL^>wv=^@?7=jOqrx_CDrq9Uw-;ehf7E(cj(>c^InvO^N{RhR3a0paF*62uNQu-Ht z=CMmkS&BmfL#ab+MQmC?dQErLAIlEm7W?1wK0ff_N2x%@h()~vo-y+z@Y%RT?2F~R zo~0<}!v!I;Y!DZ~RHh<9BPH#57Nw+_%!+a>JK?OIA%61VAF#w)14=vgYNAc5>bvm{ zC2Hp|-xO1SpC44d@A)Y2c;gJLHhzzhTtIZ`ZQkg#lv4Ex(ZD9g!UTAy{|3ySItuVC zDNGP0>HeX!HW6HQI6)3!NZ@!k%=?sdU8_}USSFrkx9iNJlRTru$ajX5h5I zT~d;`f2BGewMJ7gJ098tA!{+x3*B3R7hy z9I*D%S|h3z=6>;ISZ^|}8%yU4rN)sAZokr6W*R~DWiM>LOrz3T#y^1;qaBai{Oq7F zcADem#Flmv`KK2k;l%G*iuzLCLLi|PZUYQo03Rew6)Q{^6+w%!@of^5$R)PfMcPNj z+e6+TlN!6PRM~h^_|^4ikVwljx`z7k)yyn*iV{sxooCJWvkg|K+|0G-_n7)lv6ml* z$!}|`vrd~1Pit6ikFKA^iZ&J*UpF+6@2&i~;-)MJ(t0@Aa}MQLc+}1%FxS=0Gl#w{ z3*Gt2X-TrbA>;gBw9d-mjY2=toJIBNdJiTHA=JFQyw5Q)S+yH^zDV~6!J!&Ys;Uv} z^6IpOr&vKjG=Uz`O)y-}=6Wu>za-XPIoa9DQ0SZm7ec^mm6etA)#86giUs8rrmMVT zjw*RWJC(~|OZY@nWgoj%D0M)W6IvMNlAYHsL-PWo%d%G<`{z>lRL0&L6_w^a)TzFb zVy6BGKNXwG=Pw?r%ct8tpyWkd>;(;lu@`=z$P&dEXs&%qv<*&lP-S-Jopxa?cH^1p zCaq~6@w9DW`uu32b;`iINAXu9mqv9^sGs}~sqaYH>>s=!sU16U>hleZaH)afM0+Uw z?IqPcBT=LZEpMAqiqdFs;j5VP3XpUTDpX4V!FaCk43+e!E|ePR?k^ou{lg`kM<)EKaK6?kPkE@>L(crYu#pCD z4$yU}sBKvyFU%mw{8Y{uN|;qnMJ#cW0gX>bVK!4UB4^O*ytxc`yqluu-TNxF$s;o_ zS*Yyc;$iX613f?cFUA*#@%q!;Rnq5hKQoM|Gy zm}wjZnqmH4&S+oxPSTwGx5rXpH(HQG)`6T|qPKiTw0l5#Y-gl7gJhLT%;}D@>f1Sq z=Sk_W>c*YSoh95^F;4ibY8DLi^$m%@REVQcQIU}>+Ku383M{bfg%=)s(okPtAFZ^^ z5;hQ?kTAEnhy{Rz<7IO%F6^KNzHK37pl=9HI!MJ1^1*&~t2u@ph6No)oRC0z+ASJR zG>Y>#Y|dsAO(MmjYsbHm7PI+{1Qd%eG*CV)j2dJ31gaG$9Uqo{Mb6X(sHkC#0OwP_ z+uL)$HZgEQOC1`J{C>c 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 - - - - - - -