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