Initial URDF Tools Package
This commit is contained in:
probablydavid
commit
a331a40d17
|
|
@ -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)
|
||||||
|
|
@ -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)
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
include $(shell rospack find mk)/cmake.mk
|
||||||
|
|
@ -0,0 +1,353 @@
|
||||||
|
// bsd license blah blah
|
||||||
|
#include <cstring>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <tf/transform_listener.h>
|
||||||
|
#include <tf_conversions/tf_kdl.h>
|
||||||
|
#include <tf/transform_datatypes.h>
|
||||||
|
#include <kdl_parser/kdl_parser.hpp>
|
||||||
|
#include <kdl/jntarray.hpp>
|
||||||
|
#include <kdl/chainiksolverpos_nr_jl.hpp>
|
||||||
|
#include <kdl/chainiksolvervel_pinv.hpp>
|
||||||
|
#include <kdl/chainfksolverpos_recursive.hpp>
|
||||||
|
#include <kinematics_msgs/GetPositionFK.h>
|
||||||
|
#include <kinematics_msgs/GetPositionIK.h>
|
||||||
|
#include <kinematics_msgs/GetKinematicSolverInfo.h>
|
||||||
|
#include <kinematics_msgs/KinematicSolverInfo.h>
|
||||||
|
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<const urdf::Link> link = robot_model.getLink(tip_name);
|
||||||
|
boost::shared_ptr<const urdf::Joint> 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<tf::Pose> 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> 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -0,0 +1,26 @@
|
||||||
|
/**
|
||||||
|
\mainpage
|
||||||
|
\htmlinclude manifest.html
|
||||||
|
|
||||||
|
\b arm_kinematics is ...
|
||||||
|
|
||||||
|
<!--
|
||||||
|
Provide an overview of your package.
|
||||||
|
-->
|
||||||
|
|
||||||
|
|
||||||
|
\section codeapi Code API
|
||||||
|
|
||||||
|
<!--
|
||||||
|
Provide links to specific auto-generated API documentation within your
|
||||||
|
package that is of particular interest to a reader. Doxygen will
|
||||||
|
document pretty much every part of your code, so do your best here to
|
||||||
|
point the reader to the actual API.
|
||||||
|
|
||||||
|
If your codebase is fairly large or has different sets of APIs, you
|
||||||
|
should use the doxygen 'group' tag to keep these APIs together. For
|
||||||
|
example, the roscpp documentation has 'libros' group.
|
||||||
|
-->
|
||||||
|
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
@ -0,0 +1,20 @@
|
||||||
|
<package>
|
||||||
|
<description brief="arm_kinematics">
|
||||||
|
|
||||||
|
arm_kinematics
|
||||||
|
|
||||||
|
</description>
|
||||||
|
<author>David Lu!!</author>
|
||||||
|
<license>BSD</license>
|
||||||
|
<review status="unreviewed" notes=""/>
|
||||||
|
<url>http://ros.org/wiki/arm_kinematics</url>
|
||||||
|
<depend package="roscpp"/>
|
||||||
|
<depend package="tf"/>
|
||||||
|
<depend package="sensor_msgs"/>
|
||||||
|
<depend package="tf_conversions"/>
|
||||||
|
<depend package="kdl_parser"/>
|
||||||
|
<depend package="kinematics_msgs"/>
|
||||||
|
|
||||||
|
</package>
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -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)
|
||||||
|
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
include $(shell rospack find mk)/cmake.mk
|
||||||
|
|
@ -0,0 +1,26 @@
|
||||||
|
/**
|
||||||
|
\mainpage
|
||||||
|
\htmlinclude manifest.html
|
||||||
|
|
||||||
|
\b easy_state_publisher is ...
|
||||||
|
|
||||||
|
<!--
|
||||||
|
Provide an overview of your package.
|
||||||
|
-->
|
||||||
|
|
||||||
|
|
||||||
|
\section codeapi Code API
|
||||||
|
|
||||||
|
<!--
|
||||||
|
Provide links to specific auto-generated API documentation within your
|
||||||
|
package that is of particular interest to a reader. Doxygen will
|
||||||
|
document pretty much every part of your code, so do your best here to
|
||||||
|
point the reader to the actual API.
|
||||||
|
|
||||||
|
If your codebase is fairly large or has different sets of APIs, you
|
||||||
|
should use the doxygen 'group' tag to keep these APIs together. For
|
||||||
|
example, the roscpp documentation has 'libros' group.
|
||||||
|
-->
|
||||||
|
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
@ -0,0 +1,16 @@
|
||||||
|
<package>
|
||||||
|
<description brief="Easy State Publisher publishes JointState and/or TF messages for a given robot model">
|
||||||
|
Easy State Publisher publishes JointState and/or TF messages for a given robot model
|
||||||
|
</description>
|
||||||
|
<author>David Lu!!</author>
|
||||||
|
<license>BSD</license>
|
||||||
|
<review status="unreviewed" notes=""/>
|
||||||
|
<url>http://ros.org/wiki/easy_state_publisher</url>
|
||||||
|
<depend package="roscpp"/>
|
||||||
|
<depend package="robot_state_publisher"/>
|
||||||
|
<depend package="urdf" />
|
||||||
|
<depend package="kdl_parser"/>
|
||||||
|
<depend package="sensor_msgs"/>
|
||||||
|
</package>
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,58 @@
|
||||||
|
#include <string>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <sensor_msgs/JointState.h>
|
||||||
|
#include <urdf/model.h>
|
||||||
|
#include <urdf/joint.h>
|
||||||
|
#include <map>
|
||||||
|
|
||||||
|
int main(int argc, char** argv) {
|
||||||
|
ros::init(argc, argv, "easy_state_publisher");
|
||||||
|
ros::NodeHandle n;
|
||||||
|
ros::Publisher m_joint_pub = n.advertise
|
||||||
|
<sensor_msgs::JointState>("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<std::string, boost::shared_ptr<urdf::Joint> > joint_map;
|
||||||
|
typedef std::map<std::string, double> 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;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,50 @@
|
||||||
|
#include <string>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <robot_state_publisher/robot_state_publisher.h>
|
||||||
|
#include <urdf/model.h>
|
||||||
|
#include <urdf/joint.h>
|
||||||
|
#include <kdl_parser/kdl_parser.hpp>
|
||||||
|
#include <map>
|
||||||
|
|
||||||
|
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<std::string, boost::shared_ptr<urdf::Joint> > joint_map;
|
||||||
|
|
||||||
|
std::map<std::string, double> 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;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,5 @@
|
||||||
|
cmake_minimum_required(VERSION 2.4.6)
|
||||||
|
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
|
||||||
|
|
||||||
|
rosbuild_init()
|
||||||
|
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
include $(shell rospack find mk)/cmake.mk
|
||||||
|
|
@ -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
|
||||||
|
|
||||||
|
|
@ -0,0 +1,14 @@
|
||||||
|
<package>
|
||||||
|
<description brief="A node for inputting joint angles directly">
|
||||||
|
A node for inputting joint angles directly
|
||||||
|
</description>
|
||||||
|
<author>David Lu!!</author>
|
||||||
|
<license>BSD</license>
|
||||||
|
<review status="unreviewed" notes=""/>
|
||||||
|
<url>http://ros.org/wiki/joint_controller</url>
|
||||||
|
<depend package="rospy" />
|
||||||
|
<depend package="sensor_msgs" />
|
||||||
|
<rosdep name="wxpython" />
|
||||||
|
</package>
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -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})
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
include $(shell rospack find mk)/cmake.mk
|
||||||
|
|
@ -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
|
||||||
|
|
||||||
|
|
@ -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
|
||||||
|
|
@ -0,0 +1,26 @@
|
||||||
|
/**
|
||||||
|
\mainpage
|
||||||
|
\htmlinclude manifest.html
|
||||||
|
|
||||||
|
\b simmechanics_to_urdf is ...
|
||||||
|
|
||||||
|
<!--
|
||||||
|
Provide an overview of your package.
|
||||||
|
-->
|
||||||
|
|
||||||
|
|
||||||
|
\section codeapi Code API
|
||||||
|
|
||||||
|
<!--
|
||||||
|
Provide links to specific auto-generated API documentation within your
|
||||||
|
package that is of particular interest to a reader. Doxygen will
|
||||||
|
document pretty much every part of your code, so do your best here to
|
||||||
|
point the reader to the actual API.
|
||||||
|
|
||||||
|
If your codebase is fairly large or has different sets of APIs, you
|
||||||
|
should use the doxygen 'group' tag to keep these APIs together. For
|
||||||
|
example, the roscpp documentation has 'libros' group.
|
||||||
|
-->
|
||||||
|
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
@ -0,0 +1,15 @@
|
||||||
|
<package>
|
||||||
|
<description brief="Converts SimMechanics XML to URDF">
|
||||||
|
Converts SimMechanics XML to URDF
|
||||||
|
</description>
|
||||||
|
<author>David Lu!!</author>
|
||||||
|
<license>BSD</license>
|
||||||
|
<review status="unreviewed" notes=""/>
|
||||||
|
<url>http://ros.org/wiki/simmechanics_to_urdf</url>
|
||||||
|
<depend package="rospy"/>
|
||||||
|
<depend package="sensor_msgs"/>
|
||||||
|
<depend package="tf"/>
|
||||||
|
<rosdep name="python-yaml"/>
|
||||||
|
</package>
|
||||||
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue