initial copy
This commit is contained in:
Wim Meeussen
parent
a3c35dc515
commit
e1d5756fac
|
|
@ -1,19 +0,0 @@
|
|||
cmake_minimum_required(VERSION 2.4.6)
|
||||
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
|
||||
|
||||
# Set the build type. Options are:
|
||||
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
|
||||
# Debug : w/ debug symbols, w/o optimization
|
||||
# Release : w/o debug symbols, w/ optimization
|
||||
# RelWithDebInfo : w/ debug symbols, w/ optimization
|
||||
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
|
||||
#set(ROS_BUILD_TYPE RelWithDebInfo)
|
||||
|
||||
rosbuild_init()
|
||||
|
||||
#set the default path for built executables to the "bin" directory
|
||||
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
|
||||
#set the default path for built libraries to the "lib" directory
|
||||
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
|
||||
|
||||
rosbuild_add_executable(arm_kinematics arm_kinematics.cpp)
|
||||
|
|
@ -1 +0,0 @@
|
|||
include $(shell rospack find mk)/cmake.mk
|
||||
|
|
@ -1,362 +0,0 @@
|
|||
// 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::Stamped<tf::Pose> transform_root;
|
||||
tf::poseStampedMsgToTF( pose_msg_in, transform );
|
||||
|
||||
//Do the IK
|
||||
KDL::JntArray jnt_pos_in;
|
||||
KDL::JntArray jnt_pos_out;
|
||||
jnt_pos_in.resize(num_joints);
|
||||
for (unsigned int i=0; i < num_joints; i++) {
|
||||
int tmp_index = getJointIndex(request.ik_request.ik_seed_state.joint_state.name[i]);
|
||||
if (tmp_index >=0) {
|
||||
jnt_pos_in(tmp_index) = request.ik_request.ik_seed_state.joint_state.position[i];
|
||||
} else {
|
||||
ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.ik_seed_state.joint_state.name[i].c_str());
|
||||
}
|
||||
}
|
||||
|
||||
//Convert F to our root_frame
|
||||
try {
|
||||
tf_listener.transformPose(root_name, transform, transform_root);
|
||||
} catch (...) {
|
||||
ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str());
|
||||
response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
|
||||
return false;
|
||||
}
|
||||
|
||||
KDL::Frame F_dest;
|
||||
tf::TransformTFToKDL(transform_root, F_dest);
|
||||
|
||||
int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out);
|
||||
|
||||
if (ik_valid >= 0) {
|
||||
response.solution.joint_state.name = info.joint_names;
|
||||
response.solution.joint_state.position.resize(num_joints);
|
||||
for (unsigned int i=0; i < num_joints; i++) {
|
||||
response.solution.joint_state.position[i] = jnt_pos_out(i);
|
||||
ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
|
||||
}
|
||||
response.error_code.val = response.error_code.SUCCESS;
|
||||
return true;
|
||||
} else {
|
||||
ROS_DEBUG("An IK solution could not be found");
|
||||
response.error_code.val = response.error_code.NO_IK_SOLUTION;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
bool Kinematics::getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
|
||||
kinematics_msgs::GetKinematicSolverInfo::Response &response) {
|
||||
response.kinematic_solver_info = info;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Kinematics::getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
|
||||
kinematics_msgs::GetKinematicSolverInfo::Response &response) {
|
||||
response.kinematic_solver_info = info;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Kinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
|
||||
kinematics_msgs::GetPositionFK::Response &response) {
|
||||
KDL::Frame p_out;
|
||||
KDL::JntArray jnt_pos_in;
|
||||
geometry_msgs::PoseStamped pose;
|
||||
tf::Stamped<tf::Pose> 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;
|
||||
}
|
||||
|
||||
|
|
@ -1,26 +0,0 @@
|
|||
/**
|
||||
\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.
|
||||
-->
|
||||
|
||||
|
||||
*/
|
||||
|
|
@ -1,19 +0,0 @@
|
|||
<package>
|
||||
<description brief="A generic package for computing Arm Kinematics">
|
||||
A generic package for computing both forward and backward kinematics for arms.
|
||||
Developed as an alternative to pr2_arm_kinematics for people not using the PR2.
|
||||
</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>
|
||||
|
||||
|
||||
|
|
@ -1,30 +0,0 @@
|
|||
cmake_minimum_required(VERSION 2.4.6)
|
||||
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
|
||||
|
||||
# Set the build type. Options are:
|
||||
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
|
||||
# Debug : w/ debug symbols, w/o optimization
|
||||
# Release : w/o debug symbols, w/ optimization
|
||||
# RelWithDebInfo : w/ debug symbols, w/ optimization
|
||||
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
|
||||
#set(ROS_BUILD_TYPE RelWithDebInfo)
|
||||
|
||||
rosbuild_init()
|
||||
|
||||
#set the default path for built executables to the "bin" directory
|
||||
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
|
||||
#set the default path for built libraries to the "lib" directory
|
||||
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
|
||||
|
||||
#uncomment if you have defined messages
|
||||
#rosbuild_genmsg()
|
||||
#uncomment if you have defined services
|
||||
#rosbuild_gensrv()
|
||||
|
||||
#common commands for building c++ executables and libraries
|
||||
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
|
||||
#target_link_libraries(${PROJECT_NAME} another_library)
|
||||
#rosbuild_add_boost_directories()
|
||||
#rosbuild_link_boost(${PROJECT_NAME} thread)
|
||||
#rosbuild_add_executable(example examples/example.cpp)
|
||||
#target_link_libraries(example ${PROJECT_NAME})
|
||||
|
|
@ -1 +0,0 @@
|
|||
include $(shell rospack find mk)/cmake.mk
|
||||
|
|
@ -1,732 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
|
||||
import roslib; roslib.load_manifest('simmechanics_to_urdf')
|
||||
import rospy
|
||||
import sys
|
||||
import tf
|
||||
from tf.transformations import euler_from_quaternion, quaternion_from_matrix
|
||||
import xml.dom.minidom
|
||||
from xml.dom.minidom import Document
|
||||
import threading
|
||||
import math
|
||||
import numpy
|
||||
import yaml
|
||||
|
||||
# Conversion Factors
|
||||
INCH2METER = 0.0254
|
||||
SLUG2KG = 14.5939029
|
||||
SLUGININ2KGMM = .009415402
|
||||
MM2M = .001
|
||||
|
||||
# Special Reference Frame(s)
|
||||
WORLD = "WORLD"
|
||||
|
||||
# Arbitrary List of colors to give pieces different looks
|
||||
COLORS =[("green", "0 1 0 1"), ("black", "0 0 0 1"), ("red", "1 0 0 1"),
|
||||
("blue", "0 0 1 1"), ("yellow", "1 1 0 1"), ("pink", "1 0 1 1"),
|
||||
("cyan", "0 1 1 1"), ("green", "0 1 0 1"), ("white", "1 1 1 1"),
|
||||
("dblue", "0 0 .8 1"), ("dgreen", ".1 .8 .1 1"), ("gray", ".5 .5 .5 1")]
|
||||
|
||||
class Converter:
|
||||
def __init__(self):
|
||||
# initialize member variables
|
||||
self.links = {}
|
||||
self.frames = {}
|
||||
self.joints = {}
|
||||
self.names = {}
|
||||
self.colormap = {}
|
||||
self.colorindex = 0
|
||||
self.usedcolors = {}
|
||||
|
||||
# Start the Transform Manager
|
||||
self.tfman = TransformManager()
|
||||
self.tfman.start()
|
||||
|
||||
# Extra Transforms for Debugging
|
||||
self.tfman.add([0,0,0], [0.70682518,0,0,0.70682518], "ROOT", WORLD) # rotate so Z axis is up
|
||||
|
||||
def convert(self, filename, configfile, mode):
|
||||
self.mode = mode
|
||||
|
||||
# Parse the configuration file
|
||||
self.parseConfig(configfile)
|
||||
|
||||
# Parse the input file
|
||||
self.parse(xml.dom.minidom.parse(filename))
|
||||
self.buildTree(self.root)
|
||||
|
||||
# Create the output
|
||||
self.result = Document()
|
||||
self.output(self.root)
|
||||
|
||||
# output the output
|
||||
if mode == "xml":
|
||||
print self.result.toprettyxml()
|
||||
if mode == "graph":
|
||||
print self.graph()
|
||||
if mode == "groups":
|
||||
print self.groups(root)
|
||||
|
||||
def parseConfig(self, configFile):
|
||||
"""Parse the Configuration File, if it exists.
|
||||
Set the fields the default if the config does
|
||||
not set them """
|
||||
if configFile == None:
|
||||
configuration = {}
|
||||
else:
|
||||
configuration = yaml.load(file(configFile, 'r'))
|
||||
if configuration == None:
|
||||
configuration = {}
|
||||
|
||||
self.freezeList = []
|
||||
self.redefinedjoints = {}
|
||||
|
||||
self.root = configuration.get('root', None)
|
||||
self.extrajoints = configuration.get('extrajoints', {})
|
||||
self.filenameformat = configuration.get('filenameformat', "%s")
|
||||
self.forcelowercase = configuration.get('forcelowercase', False)
|
||||
self.scale = configuration.get('scale', None)
|
||||
self.freezeAll = configuration.get('freezeAll', False)
|
||||
self.baseframe = configuration.get('baseframe', WORLD)
|
||||
|
||||
# Get lists converted to strings
|
||||
self.removeList = [ str(e) for e in configuration.get('remove', []) ]
|
||||
self.freezeList = [ str(e) for e in configuration.get('freeze', []) ]
|
||||
|
||||
# Get map with key converted to strings
|
||||
jointmap = configuration.get('redefinedjoints', {})
|
||||
for x in jointmap.keys():
|
||||
self.redefinedjoints[str(x)] = jointmap[x]
|
||||
|
||||
# Add Extra Frames
|
||||
for frame in configuration.get('moreframes', []):
|
||||
self.tfman.add(frame['offset'], frame['orientation'], frame['parent'], frame['child'])
|
||||
|
||||
|
||||
def parse(self, element):
|
||||
"""Recursively goes through all XML elements
|
||||
and branches off for important elements"""
|
||||
name = element.localName
|
||||
# Grab name from root element AND recursively parse
|
||||
if name == "PhysicalModelingXMLFile":
|
||||
dict = getDictionary(element)
|
||||
self.name = dict['name']
|
||||
|
||||
if name == "Body":
|
||||
self.parseLink(element)
|
||||
elif name == "SimpleJoint":
|
||||
self.parseJoint(element)
|
||||
elif name == "Ground":
|
||||
dict = getDictionary(element)
|
||||
self.parseFrames(dict['frame'], "GROUND")
|
||||
else:
|
||||
for child in element.childNodes:
|
||||
self.parse(child)
|
||||
|
||||
def parseLink(self, link):
|
||||
"""Parse the important bits of a link element"""
|
||||
linkdict = getDictionary(link)
|
||||
uid = self.getName(linkdict['name'])
|
||||
linkdict['neighbors'] = []
|
||||
linkdict['children'] = []
|
||||
linkdict['jointmap'] = {}
|
||||
|
||||
# Save the frames for separate parsing
|
||||
frames = linkdict['frames']
|
||||
linkdict['frames'] = None
|
||||
|
||||
# Save the color if it exists
|
||||
if 'MaterialProp' in linkdict:
|
||||
colorelement = linkdict['MaterialProp'][1]
|
||||
color = colorelement.childNodes[0].data
|
||||
linkdict['MaterialProp'] = None
|
||||
linkdict['color'] = color.replace(",", " ") + " 1"
|
||||
|
||||
|
||||
self.links[uid] = linkdict
|
||||
self.parseFrames(frames, uid)
|
||||
|
||||
# Save First Actual Element as Root, if not defined already
|
||||
if self.root == None and "geometryFileName" in linkdict:
|
||||
self.root = uid
|
||||
|
||||
def parseFrames(self, frames, parent):
|
||||
"""Parse the frames from xml"""
|
||||
for frame in frames:
|
||||
if frame.nodeType is frame.TEXT_NODE:
|
||||
continue
|
||||
fdict = getDictionary(frame)
|
||||
fid = str(frame.getAttribute("ref"))
|
||||
fdict['parent'] = parent
|
||||
|
||||
offset = getlist(fdict['position'])
|
||||
units = fdict['positionUnits']
|
||||
for i in range(0, len(offset)):
|
||||
offset[i] = convert(offset[i], units)
|
||||
|
||||
orientation = getlist(fdict['orientation'])
|
||||
quat = matrixToQuaternion(orientation)
|
||||
|
||||
# If the frame does not have a reference number,
|
||||
# use the name plus a suffix (for CG or CS1...
|
||||
# otherwise ignore the frame
|
||||
if fid == "":
|
||||
name = fdict['name']
|
||||
if name == "CG":
|
||||
fid = parent + "CG"
|
||||
elif name == "CS1":
|
||||
fid = parent + "CS1"
|
||||
else:
|
||||
continue
|
||||
|
||||
self.tfman.add(offset, quat, WORLD, fid)
|
||||
self.frames[fid] = fdict
|
||||
|
||||
def parseJoint(self, element):
|
||||
"""Parse the joint from xml"""
|
||||
dict = getDictionary(element)
|
||||
joint = {}
|
||||
joint['name'] = dict['name']
|
||||
uid = self.getName(joint['name'])
|
||||
|
||||
frames = element.getElementsByTagName("Frame")
|
||||
joint['parent'] = str(frames[0].getAttribute("ref"))
|
||||
joint['child'] = str(frames[1].getAttribute("ref"))
|
||||
type = element.getElementsByTagName("Primitive")
|
||||
|
||||
# If there multiple elements, assume a fixed joint
|
||||
if len(type)==1:
|
||||
pdict = getDictionary(type[0])
|
||||
joint['type'] = pdict['name']
|
||||
joint['axis'] = pdict['axis']
|
||||
if joint['type'] == 'weld':
|
||||
joint['type'] = 'fixed'
|
||||
else:
|
||||
joint['type'] = 'fixed'
|
||||
|
||||
# Ignore joints on the remove list
|
||||
if joint['parent'] in self.removeList:
|
||||
return
|
||||
|
||||
# Force joints to be fixed on the freezeList
|
||||
if joint['parent'] in self.freezeList or self.freezeAll:
|
||||
joint['type'] = 'fixed'
|
||||
|
||||
# Redefine specified joints on redefined list
|
||||
if joint['parent'] in self.redefinedjoints.keys():
|
||||
jdict = self.redefinedjoints[joint['parent']]
|
||||
if 'name' in jdict:
|
||||
uid = jdict['name']
|
||||
|
||||
# Default to continuous joints
|
||||
joint['type'] = jdict.get('type', 'continuous')
|
||||
|
||||
if 'axis' in jdict:
|
||||
joint['axis'] = jdict['axis']
|
||||
if 'limits' in jdict:
|
||||
joint['limits'] = jdict['limits']
|
||||
|
||||
self.joints[uid] = joint
|
||||
|
||||
def buildTree(self, root):
|
||||
"""Reduce the graph structure of links and joints to a tree
|
||||
by breadth first search. Then construct new coordinate frames
|
||||
from new tree structure"""
|
||||
|
||||
# Create a list of all neighboring links at each link
|
||||
for jid in self.joints:
|
||||
jointdict = self.joints[jid]
|
||||
if "Root" in jointdict['name']:
|
||||
continue
|
||||
pid = self.getLinkNameByFrame(jointdict['parent'])
|
||||
cid = self.getLinkNameByFrame(jointdict['child'])
|
||||
parent = self.links[pid]
|
||||
child = self.links[cid]
|
||||
|
||||
parent['neighbors'].append(cid)
|
||||
parent['jointmap'][cid] = jid
|
||||
child['neighbors'].append(pid)
|
||||
child['jointmap'][pid] = jid
|
||||
|
||||
# Add necessary information for any user-defined joints
|
||||
for (name, extrajoint) in self.extrajoints.items():
|
||||
pid = extrajoint['pid']
|
||||
cid = extrajoint['cid']
|
||||
jorigin = extrajoint['jorigin']
|
||||
newframe = name + "_frame"
|
||||
|
||||
self.links[pid]['neighbors'].append(cid)
|
||||
self.links[pid]['jointmap'][cid] = name
|
||||
self.links[cid]['neighbors'].append(pid)
|
||||
self.links[cid]['jointmap'][pid] = name
|
||||
self.joints[name] = {'name': name, 'parent': jorigin, 'child': newframe}
|
||||
for (k,v) in extrajoint['attributes'].items():
|
||||
self.joints[name][k] = v
|
||||
self.frames[jorigin] = {'parent': pid}
|
||||
self.frames[newframe] = {'parent': cid}
|
||||
|
||||
# Starting with designated root node, perform BFS to
|
||||
# create the tree
|
||||
queue = [ root ]
|
||||
self.links[root]['parent'] = "GROUND"
|
||||
while len(queue) > 0:
|
||||
id = queue.pop(0)
|
||||
link = self.links[id]
|
||||
for n in link['neighbors']:
|
||||
nbor = self.links[n]
|
||||
# if a neighbor has not been visited yet,
|
||||
# add it as a child node
|
||||
if not 'parent' in nbor:
|
||||
nbor['parent'] = id
|
||||
queue.append(n)
|
||||
link['children'].append(n)
|
||||
|
||||
# build new coordinate frames
|
||||
for id in self.links:
|
||||
link = self.links[id]
|
||||
if not 'parent' in link:
|
||||
continue
|
||||
parentid = link['parent']
|
||||
if parentid == "GROUND":
|
||||
ref = self.baseframe
|
||||
else:
|
||||
joint = self.joints[link['jointmap'][parentid]]
|
||||
ref = joint['parent']
|
||||
# The root of each link is the offset to the joint
|
||||
# and the rotation of the CS1 frame
|
||||
(off1, rot1) = self.tfman.get(WORLD, ref)
|
||||
(off2, rot2) = self.tfman.get(WORLD, id + "CS1")
|
||||
self.tfman.add(off1, rot2, WORLD, "X" + id)
|
||||
|
||||
|
||||
def output(self, rootid):
|
||||
"""Creates the URDF from the parsed document.
|
||||
Makes the document and starts the recursive build process"""
|
||||
doc = self.result;
|
||||
self.root = doc.createElement("robot")
|
||||
doc.appendChild(self.root)
|
||||
self.root.setAttribute("name", self.name)
|
||||
self.outputLink(rootid)
|
||||
self.processLink(rootid)
|
||||
|
||||
def processLink(self, id):
|
||||
""" Creates the output for the specified node's
|
||||
child links, the connecting joints, then
|
||||
recursively processes each child """
|
||||
link = self.links[id]
|
||||
for cid in link['children']:
|
||||
jid = link['jointmap'][cid]
|
||||
|
||||
self.outputLink(cid)
|
||||
self.outputJoint(jid, id)
|
||||
self.processLink(cid)
|
||||
|
||||
def outputLink(self, id):
|
||||
""" Creates the URDF output for a single link """
|
||||
doc = self.result
|
||||
linkdict = self.links[id]
|
||||
if linkdict['name'] == "RootPart":
|
||||
return
|
||||
|
||||
link = doc.createElement("link")
|
||||
link.setAttribute("name", id)
|
||||
|
||||
visual = doc.createElement("visual")
|
||||
inertial = doc.createElement("inertial")
|
||||
collision = doc.createElement("collision")
|
||||
link.appendChild(visual)
|
||||
link.appendChild(inertial)
|
||||
link.appendChild(collision)
|
||||
|
||||
# Define Geometry
|
||||
geometry = doc.createElement("geometry")
|
||||
mesh = doc.createElement("mesh")
|
||||
filename = linkdict['geometryFileName']
|
||||
if self.forcelowercase:
|
||||
filename = filename.lower()
|
||||
|
||||
mesh.setAttribute("filename", self.filenameformat % filename)
|
||||
if self.scale != None:
|
||||
mesh.setAttribute("scale", self.scale)
|
||||
geometry.appendChild(mesh)
|
||||
visual.appendChild(geometry)
|
||||
collision.appendChild(geometry.cloneNode(1))
|
||||
|
||||
# Define Inertial Frame
|
||||
mass = doc.createElement("mass")
|
||||
units = linkdict['massUnits']
|
||||
massval = convert(float(linkdict['mass']), units)
|
||||
mass.setAttribute("value", str(massval))
|
||||
inertial.appendChild(mass)
|
||||
|
||||
inertia = doc.createElement("inertia")
|
||||
matrix = getlist(linkdict["inertia"])
|
||||
units = linkdict['inertiaUnits']
|
||||
|
||||
for i in range(0,len(matrix)):
|
||||
matrix[i] = convert(matrix[i], units)
|
||||
|
||||
inertia.setAttribute("ixx", str(matrix[0]))
|
||||
inertia.setAttribute("ixy", str(matrix[1]))
|
||||
inertia.setAttribute("ixz", str(matrix[2]))
|
||||
inertia.setAttribute("iyy", str(matrix[4]))
|
||||
inertia.setAttribute("iyz", str(matrix[5]))
|
||||
inertia.setAttribute("izz", str(matrix[8]))
|
||||
inertial.appendChild(inertia)
|
||||
|
||||
# Inertial origin is the center of gravity
|
||||
iorigin = doc.createElement("origin")
|
||||
(off, rot) = self.tfman.get("X" + id, id+"CG")
|
||||
rpy = list(euler_from_quaternion(rot))
|
||||
iorigin.setAttribute("xyz", " ".join(map(str,zero(off))))
|
||||
iorigin.setAttribute("rpy", " ".join(map(str,zero(rpy))))
|
||||
inertial.appendChild(iorigin)
|
||||
|
||||
# Create link's origin
|
||||
origin = doc.createElement("origin")
|
||||
|
||||
# Visual offset is difference between origin and CS1
|
||||
(off, rot) = self.tfman.get("X" + id, id+"CS1")
|
||||
rpy = list(euler_from_quaternion(rot))
|
||||
origin.setAttribute("xyz", " ".join(map(str,zero(off))))
|
||||
origin.setAttribute("rpy", " ".join(map(str,zero(rpy))))
|
||||
visual.appendChild(origin)
|
||||
collision.appendChild(origin.cloneNode(1))
|
||||
|
||||
# Define Material
|
||||
material = doc.createElement("material")
|
||||
|
||||
# Use specified color, if exists. Otherwise, get random color
|
||||
if 'color' in linkdict:
|
||||
cname = "%s_color"%id
|
||||
rgba = linkdict['color']
|
||||
else:
|
||||
(cname, rgba) = self.getColor(linkdict['name'])
|
||||
material.setAttribute("name", cname)
|
||||
visual.appendChild(material)
|
||||
|
||||
# If color has already been output, only output name
|
||||
if not cname in self.usedcolors:
|
||||
color = doc.createElement("color")
|
||||
color.setAttribute("rgba", rgba)
|
||||
material.appendChild(color)
|
||||
self.usedcolors[cname] = True
|
||||
|
||||
self.root.appendChild(link)
|
||||
|
||||
def getColor(self, s):
|
||||
""" Gets a two element list containing a color name,
|
||||
and it's rgba. The color selected is based on the mesh name.
|
||||
If already seen, returns the saved color
|
||||
Otherwise, returns the next available color"""
|
||||
if s in self.colormap:
|
||||
return self.colormap[s]
|
||||
color = COLORS[self.colorindex]
|
||||
self.colormap[s] = color
|
||||
self.colorindex = (self.colorindex + 1) % len(COLORS)
|
||||
return color
|
||||
|
||||
def outputJoint(self, id, parentname):
|
||||
""" Outputs URDF for a single joint """
|
||||
doc = self.result
|
||||
jointdict = self.joints[id]
|
||||
|
||||
if "Root" in jointdict['name']:
|
||||
return
|
||||
|
||||
joint = doc.createElement("joint")
|
||||
joint.setAttribute('name', id)
|
||||
|
||||
# Define the parent and child
|
||||
pid = self.getLinkNameByFrame(jointdict['parent'])
|
||||
cid = self.getLinkNameByFrame(jointdict['child'])
|
||||
|
||||
# If the original joint was reversed while building the tree,
|
||||
# swap the two ids
|
||||
if parentname != pid:
|
||||
cid = pid
|
||||
pid = parentname
|
||||
|
||||
parent = doc.createElement("parent")
|
||||
child = doc.createElement("child")
|
||||
parent.setAttribute('link', pid)
|
||||
child.setAttribute('link', cid)
|
||||
joint.appendChild(parent)
|
||||
joint.appendChild(child)
|
||||
|
||||
# Define joint type
|
||||
jtype = jointdict['type']
|
||||
joint.setAttribute('type', jtype)
|
||||
if 'limits' in jointdict:
|
||||
limit = doc.createElement("limit")
|
||||
for (k,v) in jointdict['limits'].items():
|
||||
limit.setAttribute(str(k), str(v))
|
||||
joint.appendChild(limit)
|
||||
if 'axis' in jointdict and jtype != 'fixed':
|
||||
axis = doc.createElement("axis")
|
||||
xyz = jointdict['axis'].replace(',', ' ')
|
||||
axis.setAttribute('xyz', xyz)
|
||||
joint.appendChild(axis)
|
||||
|
||||
# Define the origin
|
||||
origin = doc.createElement("origin")
|
||||
(off, rot) = self.tfman.get("X" + pid, "X" + cid)
|
||||
rpy = list(euler_from_quaternion(rot))
|
||||
off = zero(off)
|
||||
rpy = zero(rpy)
|
||||
origin.setAttribute("xyz", " ".join(map(str,off)))
|
||||
origin.setAttribute("rpy", " ".join(map(str,rpy)))
|
||||
joint.appendChild(origin)
|
||||
|
||||
self.root.appendChild(joint)
|
||||
|
||||
def getName(self, basename):
|
||||
"""Return a unique name of the format
|
||||
basenameD where D is the lowest number
|
||||
to make the name unique"""
|
||||
index = 1
|
||||
name = basename + str(index)
|
||||
while name in self.names:
|
||||
index = index + 1
|
||||
name = basename + str(index)
|
||||
self.names[name] = 1
|
||||
return name
|
||||
|
||||
def getLinkNameByFrame(self, key):
|
||||
"""Gets the link name from the frame object"""
|
||||
return self.frames[key]['parent']
|
||||
|
||||
def graph(self):
|
||||
"""For debugging purposes, output a graphviz
|
||||
representation of the tree structure, noting
|
||||
which joints have been reversed and which have
|
||||
been removed"""
|
||||
graph = "digraph proe {\n"
|
||||
for jkey in self.joints:
|
||||
joint = self.joints[jkey]
|
||||
pref = joint['parent']
|
||||
cref = joint['child']
|
||||
label = pref + ":" + cref
|
||||
pkey = self.getLinkNameByFrame(pref)
|
||||
ckey = self.getLinkNameByFrame(cref)
|
||||
case = 'std'
|
||||
if pkey != "GROUND":
|
||||
parent = self.links[pkey]
|
||||
if not ckey in parent['children']:
|
||||
child = self.links[ckey]
|
||||
if pkey in child['children']:
|
||||
case = 'rev'
|
||||
else:
|
||||
case = 'not'
|
||||
pkey = pkey.replace("-", "_")
|
||||
ckey = ckey.replace("-", "_")
|
||||
|
||||
if (case == 'std' or case == 'rev') and (joint['type'] != "fixed"):
|
||||
style = " penwidth=\"5\""
|
||||
else:
|
||||
style = "";
|
||||
|
||||
if case == 'std':
|
||||
s = pkey + " -> " + ckey + " [ label = \""+label+"\"";
|
||||
elif case == 'not':
|
||||
s = pkey + " -> " + ckey + " [ label = \""+label+"\" color=\"yellow\""
|
||||
elif case == 'rev':
|
||||
s = ckey + " -> " + pkey + " [ label = \""+label+"\" color=\"blue\""
|
||||
s = s + style + "];"
|
||||
|
||||
if not "Root" in s and "-> SCR_" not in s:
|
||||
graph = graph + s + "\n"
|
||||
return graph + "}\n"
|
||||
|
||||
def groups(self, root):
|
||||
""" For planning purposes, print out lists of
|
||||
all the links between the different joints"""
|
||||
self.groups = {}
|
||||
self.makeGroup(root, "BASE")
|
||||
s = ""
|
||||
for key in self.groups.keys():
|
||||
s = s + key + ":\n\t"
|
||||
ids = self.groups[key]
|
||||
for id in ids:
|
||||
s = s+id + " "
|
||||
s = s + "\n\n"
|
||||
return s
|
||||
|
||||
def makeGroup(self, id, gid):
|
||||
""" Helper function for recursively gathering
|
||||
groups of joints. """
|
||||
if gid in self.groups:
|
||||
idlist = self.groups[gid]
|
||||
idlist.append(id)
|
||||
else:
|
||||
idlist = [id]
|
||||
self.groups[gid] = idlist
|
||||
link = self.links[id]
|
||||
for child in link['children']:
|
||||
jid = link['jointmap'][child]
|
||||
joint = self.joints[jid]
|
||||
if joint['type'] == 'weld':
|
||||
ngid = gid
|
||||
else:
|
||||
ngid = jid
|
||||
|
||||
self.makeGroup(child, ngid)
|
||||
|
||||
def getDictionary(tag):
|
||||
"""Builds a dictionary from the specified xml tag
|
||||
where each child of the tag is entered into the dictionary
|
||||
with the name of the child tag as the key, and the contents
|
||||
as the value. Also removes quotes from quoted values"""
|
||||
x = {}
|
||||
for child in tag.childNodes:
|
||||
if child.nodeType is child.TEXT_NODE:
|
||||
continue
|
||||
key = str(child.localName)
|
||||
if len(child.childNodes) == 1:
|
||||
data = str(child.childNodes[0].data)
|
||||
if data[0] == '"' and data[-1] == '"':
|
||||
if len(data) != 2:
|
||||
x[key] = data[1:-1]
|
||||
else:
|
||||
x[key] = data
|
||||
else:
|
||||
data = child.childNodes
|
||||
x[key] = data
|
||||
return x
|
||||
|
||||
def getlist(string):
|
||||
"""Splits a string of comma delimited floats to
|
||||
a list of floats"""
|
||||
slist = string.split(",")
|
||||
flist = []
|
||||
for s in slist:
|
||||
flist.append(float(s))
|
||||
return flist
|
||||
|
||||
|
||||
def convert(value, units):
|
||||
"""Convert value from the specified units to mks units"""
|
||||
if units == 'kg' or units == 'm' or units == 'kg*m^2':
|
||||
return value
|
||||
elif units == 'slug*in^2':
|
||||
return value * SLUGININ2KGMM
|
||||
elif units == 'slug':
|
||||
return value * SLUG2KG
|
||||
elif units == 'in':
|
||||
return value * INCH2METER
|
||||
elif units == 'mm':
|
||||
return value * MM2M
|
||||
else:
|
||||
raise Exception("unsupported mass unit: %s" % units)
|
||||
|
||||
def matrixToQuaternion(matrix):
|
||||
"""Concert 3x3 rotation matrix into a quaternion"""
|
||||
(R11, R12, R13, R21, R22, R23, R31, R32, R33) = matrix
|
||||
# Build 4x4 matrix
|
||||
M = [[R11, R21, R31, 0],
|
||||
[R12, R22, R32, 0],
|
||||
[R13, R23, R33, 0],
|
||||
[0, 0, 0, 1]]
|
||||
A = numpy.array(M)
|
||||
[w,x,y,z] = quaternion_from_matrix(A)
|
||||
return [w,x,y,z]
|
||||
|
||||
def quaternion_to_rpy(quat):
|
||||
"""Convert quaternion into roll pitch yaw list (in degrees)"""
|
||||
rpy = list(euler_from_quaternion(quat))
|
||||
for i in range(0, len(rpy)):
|
||||
rpy[i] = rpy[i]*180/math.pi
|
||||
return rpy
|
||||
|
||||
def zero(arr):
|
||||
"""Converts any numbers less than 1e-7 to 0 in the array"""
|
||||
for i in range(0,len(arr)):
|
||||
if math.fabs(arr[i]) < 1e-7:
|
||||
arr[i] = 0
|
||||
return arr
|
||||
|
||||
|
||||
class TransformManager(threading.Thread):
|
||||
"""Bridge to the ROS tf package. Constantly broadcasts
|
||||
all tfs, and retrieves them on demand."""
|
||||
|
||||
def __init__(self):
|
||||
threading.Thread.__init__(self)
|
||||
self.running = True
|
||||
self.tfs = []
|
||||
self.listener = tf.TransformListener()
|
||||
|
||||
def run(self):
|
||||
"""Broadcasts tfs until shutdown"""
|
||||
rate = rospy.Rate(10.0)
|
||||
br = tf.TransformBroadcaster()
|
||||
while self.running:
|
||||
for transform in self.tfs:
|
||||
br.sendTransform(transform['trans'],
|
||||
transform['rot'],
|
||||
rospy.Time.now(),
|
||||
transform['child'],
|
||||
transform['parent'])
|
||||
rate.sleep()
|
||||
|
||||
def add(self, trans, rot, parent, child):
|
||||
"""Adds a new transform to the set"""
|
||||
tf = {'trans':trans, 'rot':rot, 'child':child, 'parent':parent}
|
||||
self.tfs.append(tf)
|
||||
|
||||
def get(self, parent, child):
|
||||
"""Attempts to retrieve a transform"""
|
||||
attempts = 0
|
||||
rate = rospy.Rate(10.0)
|
||||
while attempts < 50:
|
||||
try:
|
||||
(off,rpy) = self.listener.lookupTransform(parent, child, rospy.Time(0))
|
||||
return [list(off), list(rpy)]
|
||||
except (tf.LookupException, tf.ConnectivityException):
|
||||
attempts+=1
|
||||
rate.sleep()
|
||||
continue
|
||||
|
||||
raise Exception("Attempt to transform %s exceeded attempt limit" % (parent + "/" + child))
|
||||
|
||||
def printTransform(self, parent, child):
|
||||
"""Attempts to print the specified transform"""
|
||||
(off, rot) = self.get(parent, child)
|
||||
rpy = quaternion_to_rpy(rot)
|
||||
|
||||
s = parent + "-->" + child
|
||||
l = (30 - len(s))*" "
|
||||
print "%s%s[%+.5f %+.5f %+.5f] \t [%+3.3f %+.3f %+.3f] \t [%+.6f %+.6f %+.6f %+.6f] " \
|
||||
% (s, l, off[0], off[1], off[2], rpy[0], rpy[1], rpy[2], rot[0], rot[1], rot[2], rot[3])
|
||||
|
||||
def kill(self):
|
||||
"""Stops thread from running"""
|
||||
self.running = False
|
||||
|
||||
if __name__ == '__main__':
|
||||
argc = len(sys.argv)
|
||||
if argc == 3:
|
||||
filename = sys.argv[1]
|
||||
config = None
|
||||
mode = sys.argv[2]
|
||||
elif argc == 4:
|
||||
filename = sys.argv[1]
|
||||
config = sys.argv[2]
|
||||
mode = sys.argv[3]
|
||||
else:
|
||||
print "Usage: " + sys.argv[0] + " {XML filename} [configfile] {tf|xml|graph|groups|none}"
|
||||
sys.exit(-1)
|
||||
try:
|
||||
rospy.init_node('convert')
|
||||
con = Converter()
|
||||
try:
|
||||
con.convert(filename, config, mode)
|
||||
while mode == "tf" and not rospy.is_shutdown():
|
||||
None
|
||||
except Exception:
|
||||
raise
|
||||
finally:
|
||||
con.tfman.kill()
|
||||
|
||||
except rospy.ROSInterruptException: pass
|
||||
|
||||
|
|
@ -1,18 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
import os
|
||||
import sys
|
||||
import subprocess
|
||||
|
||||
if __name__ == '__main__':
|
||||
if len(sys.argv) != 2:
|
||||
print "Usage: " + sys.argv[0] + " [directory]"
|
||||
sys.exit(-1)
|
||||
path= sys.argv[1]
|
||||
dirList=os.listdir(path)
|
||||
for fname in dirList:
|
||||
path1 = path + fname
|
||||
path2 = path + fname + "b"
|
||||
cmd = "rosrun ivcon ivcon " + path1 + " " + path2
|
||||
proc = subprocess.Popen([cmd], stdout=subprocess.PIPE, shell=True)
|
||||
(out, err) = proc.communicate()
|
||||
print err
|
||||
|
|
@ -1,26 +0,0 @@
|
|||
/**
|
||||
\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.
|
||||
-->
|
||||
|
||||
|
||||
*/
|
||||
|
|
@ -1,15 +0,0 @@
|
|||
<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>
|
||||
|
||||
|
||||
|
|
@ -1,10 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot name="myfirst">
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
</robot>
|
||||
|
|
@ -1,25 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot name="multipleshapes">
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
|
@ -1,27 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot name="origins">
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
|
@ -1,49 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot name="materials">
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0 0 .8 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="-0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
|
@ -1,247 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot name="visual">
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0 0 .8 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_base_joint" type="fixed">
|
||||
<parent link="right_leg"/>
|
||||
<child link="right_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_front_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_front_wheel_joint" type="fixed">
|
||||
<parent link="right_base"/>
|
||||
<child link="right_front_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_back_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_back_wheel_joint" type="fixed">
|
||||
<parent link="right_base"/>
|
||||
<child link="right_back_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="-0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_base_joint" type="fixed">
|
||||
<parent link="left_leg"/>
|
||||
<child link="left_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_front_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_front_wheel_joint" type="fixed">
|
||||
<parent link="left_base"/>
|
||||
<child link="left_front_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_back_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_back_wheel_joint" type="fixed">
|
||||
<parent link="left_base"/>
|
||||
<child link="left_back_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<joint name="gripper_extension" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
<material name="Gray">
|
||||
<color rgba=".7 .7 .7 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_gripper_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="left_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_gripper">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_tip_joint" type="fixed">
|
||||
<parent link="left_gripper"/>
|
||||
<child link="left_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_tip">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_gripper_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_gripper">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_tip_joint" type="fixed">
|
||||
<parent link="right_gripper"/>
|
||||
<child link="right_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_tip">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="head_swivel" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<origin xyz="0 0 0.3"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0 0.1414 0.1414"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
|
@ -1,264 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot name="flexible">
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0 0 .8 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_base_joint" type="fixed">
|
||||
<parent link="right_leg"/>
|
||||
<child link="right_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_front_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_front_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_front_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_back_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_back_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_back_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="-0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_base_joint" type="fixed">
|
||||
<parent link="left_leg"/>
|
||||
<child link="left_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_front_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_front_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_front_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_back_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_back_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_back_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<joint name="gripper_extension" type="prismatic">
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
|
||||
<origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
<material name="Gray">
|
||||
<color rgba=".7 .7 .7 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="left_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_gripper">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_tip_joint" type="fixed">
|
||||
<parent link="left_gripper"/>
|
||||
<child link="left_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_tip">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_gripper">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_tip_joint" type="fixed">
|
||||
<parent link="right_gripper"/>
|
||||
<child link="right_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_tip">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="head_swivel" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin xyz="0 0 0.3"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0 0.1414 0.1414"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
|
@ -1,415 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot name="physics">
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0 0 .8 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_base_joint" type="fixed">
|
||||
<parent link="right_leg"/>
|
||||
<child link="right_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_front_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_front_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_front_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_back_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_back_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_back_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="-0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_base_joint" type="fixed">
|
||||
<parent link="left_leg"/>
|
||||
<child link="left_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_front_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_front_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_front_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_back_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_back_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_back_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<joint name="gripper_extension" type="prismatic">
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
|
||||
<origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
<material name="Gray">
|
||||
<color rgba=".7 .7 .7 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="left_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_gripper">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_tip_joint" type="fixed">
|
||||
<parent link="left_gripper"/>
|
||||
<child link="left_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_tip">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_gripper">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_tip_joint" type="fixed">
|
||||
<parent link="right_gripper"/>
|
||||
<child link="right_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_tip">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="head_swivel" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin xyz="0 0 0.3"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0 0.1414 0.1414"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
|
@ -1,236 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
|
||||
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
|
||||
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
|
||||
xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" name="macroed">
|
||||
|
||||
<xacro:property name="width" value=".2" />
|
||||
<xacro:property name="leglen" value=".6" />
|
||||
<xacro:property name="polelen" value=".2" />
|
||||
<xacro:property name="bodylen" value=".6" />
|
||||
<xacro:property name="baselen" value=".4" />
|
||||
<xacro:property name="wheeldiam" value=".07" />
|
||||
<xacro:property name="pi" value="3.1415" />
|
||||
|
||||
<xacro:macro name="default_inertial" params="mass">
|
||||
<inertial>
|
||||
<mass value="${mass}" />
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0"
|
||||
iyy="1.0" iyz="0.0"
|
||||
izz="1.0" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder radius="${width}" length="${bodylen}"/>
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0 0 .8 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder radius="${width}" length="${bodylen}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:default_inertial mass="10"/>
|
||||
</link>
|
||||
|
||||
<xacro:macro name="wheel" params="prefix suffix reflect">
|
||||
<link name="${prefix}_${suffix}_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder radius="${wheeldiam/2}" length=".1"/>
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder radius="${wheeldiam/2}" length=".1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:default_inertial mass="1"/>
|
||||
</link>
|
||||
<joint name="${prefix}_${suffix}_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="${prefix}_base"/>
|
||||
<child link="${prefix}_${suffix}_wheel"/>
|
||||
<origin xyz="0 ${baselen*reflect/3} -${wheeldiam/2+.05}" rpy="0 ${pi/2} 0"/>
|
||||
<limit effort="100" velocity="100" />
|
||||
<joint_properties damping="0.0" friction="0.0" />
|
||||
|
||||
</joint>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
|
||||
|
||||
<xacro:macro name="leg" params="prefix reflect">
|
||||
<link name="${prefix}_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="${leglen} .2 .1"/>
|
||||
</geometry>
|
||||
<origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="${leglen} .2 .1"/>
|
||||
</geometry>
|
||||
<origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
|
||||
</collision>
|
||||
<xacro:default_inertial mass="10"/>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_${prefix}_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="${prefix}_leg"/>
|
||||
<origin xyz="${reflect*(width+.02)} 0 .25" />
|
||||
</joint>
|
||||
|
||||
<link name="${prefix}_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".1 ${baselen} .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".1 ${baselen} .1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:default_inertial mass="10"/>
|
||||
</link>
|
||||
|
||||
<joint name="${prefix}_base_joint" type="fixed">
|
||||
<parent link="${prefix}_leg"/>
|
||||
<child link="${prefix}_base"/>
|
||||
<origin xyz="0 0 ${-leglen}" />
|
||||
</joint>
|
||||
<xacro:wheel prefix="${prefix}" suffix="front" reflect="1"/>
|
||||
<xacro:wheel prefix="${prefix}" suffix="back" reflect="-1"/>
|
||||
</xacro:macro>
|
||||
<xacro:leg prefix="right" reflect="1" />
|
||||
<xacro:leg prefix="left" reflect="-1" />
|
||||
|
||||
<joint name="gripper_extension" type="prismatic">
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<limit effort="1000.0" lower="-${width*2-.02}" upper="0" velocity="0.5"/>
|
||||
<origin rpy="0 0 ${pi/2}" xyz="0 ${width-.01} .2"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="${polelen}" radius=".01"/>
|
||||
</geometry>
|
||||
<origin xyz="${polelen/2} 0 0" rpy="0 ${pi/2} 0 "/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${polelen}" radius=".01"/>
|
||||
</geometry>
|
||||
<origin xyz="${polelen/2} 0 0" rpy="0 ${pi/2} 0 "/>
|
||||
</collision>
|
||||
<xacro:default_inertial mass=".05"/>
|
||||
</link>
|
||||
|
||||
<xacro:macro name="gripper" params="prefix reflect">
|
||||
<joint name="${prefix}_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 ${reflect}"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="${polelen} ${reflect*0.01} 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="${prefix}_gripper"/>
|
||||
</joint>
|
||||
<link name="${prefix}_gripper">
|
||||
<visual>
|
||||
<origin rpy="${(reflect-1)/2*pi} 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="${(reflect-1)/2*pi} 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<xacro:default_inertial mass=".05"/>
|
||||
</link>
|
||||
<joint name="${prefix}_tip_joint" type="fixed">
|
||||
<parent link="${prefix}_gripper"/>
|
||||
<child link="${prefix}_tip"/>
|
||||
</joint>
|
||||
<link name="${prefix}_tip">
|
||||
<visual>
|
||||
<origin rpy="${(reflect-1)/2*pi} 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="${(reflect-1)/2*pi} 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<xacro:default_inertial mass=".05"/>
|
||||
</link>
|
||||
</xacro:macro>
|
||||
<xacro:gripper prefix="left" reflect="1" />
|
||||
<xacro:gripper prefix="right" reflect="-1" />
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="${width}"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="${width}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:default_inertial mass="10"/>
|
||||
</link>
|
||||
|
||||
<joint name="head_swivel" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin xyz="0 0 ${bodylen/2}"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:default_inertial mass="1"/>
|
||||
</link>
|
||||
|
||||
<joint name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0 ${.707*width} ${.707*width}"/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
||||
|
|
@ -1,30 +0,0 @@
|
|||
cmake_minimum_required(VERSION 2.4.6)
|
||||
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
|
||||
|
||||
# Set the build type. Options are:
|
||||
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
|
||||
# Debug : w/ debug symbols, w/o optimization
|
||||
# Release : w/o debug symbols, w/ optimization
|
||||
# RelWithDebInfo : w/ debug symbols, w/ optimization
|
||||
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
|
||||
#set(ROS_BUILD_TYPE RelWithDebInfo)
|
||||
|
||||
rosbuild_init()
|
||||
|
||||
#set the default path for built executables to the "bin" directory
|
||||
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
|
||||
#set the default path for built libraries to the "lib" directory
|
||||
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
|
||||
|
||||
#uncomment if you have defined messages
|
||||
#rosbuild_genmsg()
|
||||
#uncomment if you have defined services
|
||||
#rosbuild_gensrv()
|
||||
|
||||
#common commands for building c++ executables and libraries
|
||||
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
|
||||
#target_link_libraries(${PROJECT_NAME} another_library)
|
||||
#rosbuild_add_boost_directories()
|
||||
#rosbuild_link_boost(${PROJECT_NAME} thread)
|
||||
#rosbuild_add_executable(example examples/example.cpp)
|
||||
#target_link_libraries(example ${PROJECT_NAME})
|
||||
|
|
@ -1 +0,0 @@
|
|||
include $(shell rospack find mk)/cmake.mk
|
||||
|
|
@ -1,9 +0,0 @@
|
|||
<launch>
|
||||
<arg name="model" />
|
||||
<arg name="gui" default="False" />
|
||||
<param name="robot_description" textfile="$(arg model)" />
|
||||
<param name="use_gui" value="$(arg gui)"/>
|
||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.vcg" />
|
||||
</launch>
|
||||
|
|
@ -1,24 +0,0 @@
|
|||
<launch>
|
||||
<!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/pr2/erratic_defs/robots for full erratic -->
|
||||
<!-- start up wg world -->
|
||||
<include file="$(find gazebo_worlds)/launch/wg_world.launch"/>
|
||||
|
||||
<!-- Create a transform sender for linking these frames. -->
|
||||
<node name="tf_footprint_base" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 base_link base_footprint 40" />
|
||||
|
||||
<arg name="model" />
|
||||
<param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
|
||||
|
||||
<!-- push robot_description to factory and spawn robot in gazebo -->
|
||||
<node name="spawn_erratic" pkg="gazebo_tools" type="gazebo_model" args="-z 1.051 -p robot_description spawn robot_description" respawn="false" output="screen" />
|
||||
|
||||
<!-- Controller Manager -->
|
||||
<include file="$(find pr2_controller_manager)/controller_manager.launch" />
|
||||
|
||||
<!-- Fake Calibration -->
|
||||
<node pkg="rostopic" type="rostopic" name="fake_joint_calibration" args="pub /calibrated std_msgs/Bool true" />
|
||||
|
||||
<!-- load controllers -->
|
||||
<node name="diffdrive" pkg="gazebo_plugins" type="gazebo_ros_diffdrive" respawn="true" output="screen"/>
|
||||
</launch>
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 125 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 90 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 89 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 89 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 89 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 94 KiB |
|
|
@ -1,26 +0,0 @@
|
|||
/**
|
||||
\mainpage
|
||||
\htmlinclude manifest.html
|
||||
|
||||
\b urdf_tutorial 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.
|
||||
-->
|
||||
|
||||
|
||||
*/
|
||||
|
|
@ -1,12 +0,0 @@
|
|||
<package>
|
||||
<description brief="Code for some URDF Tutorials">
|
||||
Support code for the step by step URDF tutorials on ROS.org
|
||||
</description>
|
||||
<author>David Lu!!</author>
|
||||
<license>BSD</license>
|
||||
<review status="unreviewed" notes=""/>
|
||||
<url>http://ros.org/wiki/urdf_tutorial</url>
|
||||
|
||||
</package>
|
||||
|
||||
|
||||
|
|
@ -1,277 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
|
||||
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
|
||||
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
|
||||
xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
|
||||
name="r2d2">
|
||||
|
||||
<xacro:property name="width" value=".2" />
|
||||
<xacro:property name="leglen" value=".6" />
|
||||
<xacro:property name="polelen" value=".2" />
|
||||
<xacro:property name="bodylen" value=".6" />
|
||||
<xacro:property name="baselen" value=".4" />
|
||||
<xacro:property name="wheeldiam" value=".07" />
|
||||
<xacro:property name="pi" value="3.1415" />
|
||||
|
||||
<xacro:macro name="default_inertial" params="mass">
|
||||
<inertial>
|
||||
<mass value="${mass}" />
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0"
|
||||
iyy="1.0" iyz="0.0"
|
||||
izz="1.0" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder radius="${width}" length="${bodylen}"/>
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0 0 .8 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder radius="${width}" length="${bodylen}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:default_inertial mass="10"/>
|
||||
</link>
|
||||
|
||||
<xacro:macro name="wheel" params="prefix suffix reflect">
|
||||
<link name="${prefix}_${suffix}_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder radius="${wheeldiam/2}" length=".1"/>
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder radius="${wheeldiam/2}" length=".1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:default_inertial mass="1"/>
|
||||
</link>
|
||||
<joint name="${prefix}_${suffix}_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="${prefix}_base"/>
|
||||
<child link="${prefix}_${suffix}_wheel"/>
|
||||
<origin xyz="0 ${baselen*reflect/3} -${wheeldiam/2+.05}" rpy="0 ${pi/2} 0"/>
|
||||
<limit effort="100" velocity="100" />
|
||||
<joint_properties damping="0.0" friction="0.0" />
|
||||
|
||||
</joint>
|
||||
<gazebo reference="${prefix}_${suffix}_wheel" >
|
||||
<mu1>50.0</mu1>
|
||||
<mu2>50.0</mu2>
|
||||
<kp>100000000.0</kp>
|
||||
<kd>1.0</kd>
|
||||
<material>Gazebo/Fish</material>
|
||||
</gazebo>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
|
||||
|
||||
<xacro:macro name="leg" params="prefix reflect">
|
||||
<link name="${prefix}_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="${leglen} .2 .1"/>
|
||||
</geometry>
|
||||
<origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="${leglen} .2 .1"/>
|
||||
</geometry>
|
||||
<origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
|
||||
</collision>
|
||||
<xacro:default_inertial mass="10"/>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_${prefix}_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="${prefix}_leg"/>
|
||||
<origin xyz="${reflect*(width+.02)} 0 .25" />
|
||||
</joint>
|
||||
|
||||
<link name="${prefix}_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".1 ${baselen} .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".1 ${baselen} .1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:default_inertial mass="10"/>
|
||||
</link>
|
||||
|
||||
<joint name="${prefix}_base_joint" type="fixed">
|
||||
<parent link="${prefix}_leg"/>
|
||||
<child link="${prefix}_base"/>
|
||||
<origin xyz="0 0 ${-leglen}" />
|
||||
</joint>
|
||||
<xacro:wheel prefix="${prefix}" suffix="front" reflect="1"/>
|
||||
<xacro:wheel prefix="${prefix}" suffix="back" reflect="-1"/>
|
||||
</xacro:macro>
|
||||
<xacro:leg prefix="right" reflect="1" />
|
||||
<xacro:leg prefix="left" reflect="-1" />
|
||||
|
||||
<joint name="gripper_extension" type="prismatic">
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<limit effort="1000.0" lower="-${width*2-.02}" upper="0" velocity="0.5"/>
|
||||
<origin rpy="0 0 ${pi/2}" xyz="0 ${width-.01} .2"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="${polelen}" radius=".01"/>
|
||||
</geometry>
|
||||
<origin xyz="${polelen/2} 0 0" rpy="0 ${pi/2} 0 "/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${polelen}" radius=".01"/>
|
||||
</geometry>
|
||||
<origin xyz="${polelen/2} 0 0" rpy="0 ${pi/2} 0 "/>
|
||||
</collision>
|
||||
<xacro:default_inertial mass=".05"/>
|
||||
</link>
|
||||
|
||||
<xacro:macro name="gripper" params="prefix reflect">
|
||||
<joint name="${prefix}_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 ${reflect}"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="${polelen} ${reflect*0.01} 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="${prefix}_gripper"/>
|
||||
</joint>
|
||||
<link name="${prefix}_gripper">
|
||||
<visual>
|
||||
<origin rpy="${(reflect-1)/2*pi} 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="${(reflect-1)/2*pi} 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<xacro:default_inertial mass=".05"/>
|
||||
</link>
|
||||
<joint name="${prefix}_tip_joint" type="fixed">
|
||||
<parent link="${prefix}_gripper"/>
|
||||
<child link="${prefix}_tip"/>
|
||||
</joint>
|
||||
<link name="${prefix}_tip">
|
||||
<visual>
|
||||
<origin rpy="${(reflect-1)/2*pi} 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="${(reflect-1)/2*pi} 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<xacro:default_inertial mass=".05"/>
|
||||
</link>
|
||||
</xacro:macro>
|
||||
<xacro:gripper prefix="left" reflect="1" />
|
||||
<xacro:gripper prefix="right" reflect="-1" />
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="${width}"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="${width}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:default_inertial mass="10"/>
|
||||
</link>
|
||||
|
||||
<joint name="head_swivel" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin xyz="0 0 ${bodylen/2}"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:default_inertial mass="1"/>
|
||||
</link>
|
||||
|
||||
<joint name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0 ${.707*width} ${.707*width}"/>
|
||||
</joint>
|
||||
|
||||
<gazebo>
|
||||
<controller:differential_position2d name="controller1">
|
||||
<update>100</update>
|
||||
<leftJoint>left_back_wheel_joint</leftJoint>
|
||||
<rightJoint>right_back_wheel_joint</rightJoint>
|
||||
<wheelSeparation>${width}</wheelSeparation>
|
||||
<wheelDiameter>${wheeldiam}</wheelDiameter>
|
||||
<torque>5</torque>
|
||||
<interface:position name="position_iface_0"/>
|
||||
</controller:differential_position2d>
|
||||
|
||||
<controller:gazebo_ros_p3d name="p3d_base_controller" plugin="libgazebo_ros_p3d.so">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>100.0</updateRate>
|
||||
<bodyName>base_link</bodyName>
|
||||
<topicName>base_pose_ground_truth</topicName>
|
||||
<gaussianNoise>0.01</gaussianNoise>
|
||||
<frameName>map</frameName>
|
||||
<xyzOffsets>25.7 25.7 0</xyzOffsets> <!-- initialize odometry for fake localization-->
|
||||
<rpyOffsets>0 0 0</rpyOffsets>
|
||||
<interface:position name="p3d_base_position"/>
|
||||
</controller:gazebo_ros_p3d>
|
||||
|
||||
<!-- this publishes empty joint_states due to no transmission, but
|
||||
triggering robot_state_puublisher to publish tf between fixed joints in erratic,
|
||||
(e.g. base_laser_link for the base_scan frame) -->
|
||||
<controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>1000.0</updateRate>
|
||||
<interface:audio name="gazebo_ros_controller_manager_dummy_iface" />
|
||||
</controller:gazebo_ros_controller_manager>
|
||||
</gazebo>
|
||||
</robot>
|
||||
|
||||
|
|
@ -1,44 +0,0 @@
|
|||
Background\ ColorR=0.0941176
|
||||
Background\ ColorG=0
|
||||
Background\ ColorB=0.466667
|
||||
Fixed\ Frame=/base_link
|
||||
Target\ Frame=<Fixed Frame>
|
||||
Grid.Alpha=0.5
|
||||
Grid.Cell\ Size=0.5
|
||||
Grid.ColorR=0.898039
|
||||
Grid.ColorG=0.898039
|
||||
Grid.ColorB=0.898039
|
||||
Grid.Enabled=1
|
||||
Grid.Line\ Style=0
|
||||
Grid.Line\ Width=0.03
|
||||
Grid.Normal\ Cell\ Count=0
|
||||
Grid.OffsetX=0
|
||||
Grid.OffsetY=0
|
||||
Grid.OffsetZ=0
|
||||
Grid.Plane=0
|
||||
Grid.Plane\ Cell\ Count=10
|
||||
Grid.Reference\ Frame=<Fixed Frame>
|
||||
Robot\ Model.Alpha=1
|
||||
Robot\ Model.Collision\ Enabled=0
|
||||
Robot\ Model.Enabled=1
|
||||
Robot\ Model.Robot\ Description=robot_description
|
||||
Robot\ Model.TF\ Prefix=
|
||||
Robot\ Model.Update\ Interval=0
|
||||
Robot\ Model.Visual\ Enabled=1
|
||||
Robot\:\ Robot\ Model\ Link\ base_linkShow\ Axes=0
|
||||
Robot\:\ Robot\ Model\ Link\ base_linkShow\ Trail=0
|
||||
Robot\:\ Robot\ Model\ Link\ legShow\ Axes=0
|
||||
Robot\:\ Robot\ Model\ Link\ legShow\ Trail=0
|
||||
Tool\ 2D\ Nav\ GoalTopic=goal
|
||||
Tool\ 2D\ Pose\ EstimateTopic=initialpose
|
||||
Camera\ Type=rviz::OrbitViewController
|
||||
Camera\ Config=1.15779 3.76081 2.16475 0 0 0
|
||||
Property\ Grid\ State=selection=.Global StatusTopStatus;expanded=.Global Options,Grid.Enabled,Robot Model.Enabled;scrollpos=0,0;splitterpos=150,285;ispageselected=1
|
||||
[Display0]
|
||||
Name=Grid
|
||||
Package=rviz
|
||||
ClassName=rviz::GridDisplay
|
||||
[Display1]
|
||||
Name=Robot Model
|
||||
Package=rviz
|
||||
ClassName=rviz::RobotModelDisplay
|
||||
|
|
@ -1,9 +0,0 @@
|
|||
<launch>
|
||||
<arg name="model" />
|
||||
<arg name="gui" default="False" />
|
||||
<param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
|
||||
<param name="use_gui" value="$(arg gui)"/>
|
||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.vcg" />
|
||||
</launch>
|
||||
Loading…
Reference in New Issue