bool framesFromKDLTree(const KDL::Tree& tree,
std::vector<std::string>& framesNames,
std::vector<std::string>& parentLinkNames)
{
framesNames.clear();
parentLinkNames.clear();
KDL::SegmentMap::iterator seg;
KDL::SegmentMap segs;
KDL::SegmentMap::const_iterator root_seg;
root_seg = tree.getRootSegment();
segs = tree.getSegments();
for( seg = segs.begin(); seg != segs.end(); seg++ )
{
if( GetTreeElementChildren(seg->second).size() == 0 &&
GetTreeElementSegment(seg->second).getJoint().getType() == KDL::Joint::None &&
GetTreeElementSegment(seg->second).getInertia().getMass() == 0.0 )
{
std::string frameName = GetTreeElementSegment(seg->second).getName();
std::string parentLinkName = GetTreeElementSegment(GetTreeElementParent(seg->second)->second).getName();
framesNames.push_back(frameName);
parentLinkNames.push_back(parentLinkName);
//also check parent
KDL::Segment parent = GetTreeElementSegment(GetTreeElementParent(seg->second)->second);
if (parent.getJoint().getType() == KDL::Joint::None &&
parent.getInertia().getMass() == 0.0)
{
framesNames.push_back(parentLinkName);
}
}
}
return true;
}
bool isBaseLinkFake(const KDL::Tree & tree)
{
KDL::SegmentMap::const_iterator root = tree.getRootSegment();
bool return_value;
if (GetTreeElementChildren(root->second).size() == 1 && GetTreeElementSegment(GetTreeElementChildren(root->second)[0]->second).getJoint().getType() == Joint::None) {
return_value = true;
} else {
return_value = false;
}
return return_value;
}
bool treeToUrdfModel(const KDL::Tree& tree, const std::string & robot_name, urdf::ModelInterface& robot_model)
{
robot_model.clear();
robot_model.name_ = robot_name;
//Add all links
KDL::SegmentMap::iterator seg;
KDL::SegmentMap segs;
KDL::SegmentMap::const_iterator root_seg;
root_seg = tree.getRootSegment();
segs = tree.getSegments();
for( seg = segs.begin(); seg != segs.end(); seg++ ) {
if (robot_model.getLink(seg->first))
{
std::cerr << "[ERR] link " << seg->first << " is not unique." << std::endl;
robot_model.clear();
return false;
}
else
{
urdf::LinkPtr link;
resetPtr(link, new urdf::Link);
//add name
link->name = seg->first;
//insert link
robot_model.links_.insert(make_pair(seg->first,link));
std::cerr << "[DEBUG] successfully added a new link " << link->name << std::endl;
}
//inserting joint
//The fake root segment has no joint to add
if( seg->first != root_seg->first ) {
KDL::Joint jnt;
jnt = GetTreeElementSegment(seg->second).getJoint();
if (robot_model.getJoint(jnt.getName()))
{
std::cerr << "[ERR] joint " << jnt.getName() << " is not unique." << std::endl;
robot_model.clear();
return false;
}
else
{
urdf::JointPtr joint;
urdf::LinkPtr link = robot_model.links_[seg->first];
//This variable will be set by toUrdf
KDL::Frame H_new_old_successor;
KDL::Frame H_new_old_predecessor = getH_new_old(GetTreeElementSegment(GetTreeElementParent(seg->second)->second));
urdf::resetPtr(joint, new urdf::Joint());
//convert joint
*joint = toUrdf(jnt, GetTreeElementSegment(seg->second).getFrameToTip(),H_new_old_predecessor,H_new_old_successor);
//insert parent
joint->parent_link_name = GetTreeElementParent(seg->second)->first;
//insert child
joint->child_link_name = seg->first;
//insert joint
robot_model.joints_.insert(make_pair(seg->first,joint));
std::cerr << "[DEBUG] successfully added a new joint" << jnt.getName() << std::endl;
//add inertial, taking in account an eventual change in the link frame
resetPtr(link->inertial, new urdf::Inertial());
*(link->inertial) = toUrdf(H_new_old_successor * GetTreeElementSegment(seg->second).getInertia());
}
}
}
// every link has children links and joints, but no parents, so we create a
// local convenience data structure for keeping child->parent relations
std::map<std::string, std::string> parent_link_tree;
parent_link_tree.clear();
// building tree: name mapping
//try
//{
robot_model.initTree(parent_link_tree);
//}
/*
catch(ParseError &e)
{
logError("Failed to build tree: %s", e.what());
robot_model.clear();
return false;
}*/
// find the root link
//try
//{
robot_model.initRoot(parent_link_tree);
//}
/*
catch(ParseError &e)
{
logError("Failed to find root link: %s", e.what());
robot_model.reset();
return false;
}
*/
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "cartesian");
ros::NodeHandle n;
ros::NodeHandle n_private("~");
string robot_desc;
n.getParam("robot_description", robot_desc);
KDL::Tree tree;
if (!kdl_parser::treeFromString(robot_desc, tree))
{
ROS_ERROR("failed to extract kdl tree from xml robot description");
return 1;
}
if (!tree.getNrOfSegments())
{
ROS_ERROR("empty tree. sad.");
return 1;
}
KDL::Chain chain;
if (!tree.getChain("torso_link", "tool_link", chain))
{
ROS_ERROR("couldn't pull arm chain from robot model");
return 1;
}
ROS_INFO("parsed tree successfully");
///////////////////////////////////////////////////////////////////////
//ros::Subscriber target_sub = n.subscribe("ik_request", 1, ik_request_cb);
ros::Subscriber joint_sub = n.subscribe("joint_states", 1, joint_cb);
ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("target_joints", 1);
ros::ServiceServer ik_param_srv = n.advertiseService("arm_ik_params",
ik_params_cb);
g_joint_pub = &joint_pub; // ugly ugly
tf::TransformBroadcaster tf_broadcaster;
tf::TransformListener tf_listener;
g_tf_broadcaster = &tf_broadcaster;
g_tf_listener = &tf_listener;
ros::Rate loop_rate(30);
geometry_msgs::TransformStamped world_trans;
world_trans.header.frame_id = "world";
world_trans.child_frame_id = "torso_link";
world_trans.transform.translation.x = 0;
world_trans.transform.translation.y = 0;
world_trans.transform.translation.z = 0;
world_trans.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(0,0,0);
g_js.name.resize(7);
g_js.position.resize(7);
g_js.name[0] = "shoulder1";
g_js.name[1] = "shoulder2";
g_js.name[2] = "shoulder3";
g_js.name[3] = "elbow";
g_js.name[4] = "wrist1";
g_js.name[5] = "wrist2";
g_js.name[6] = "wrist3";
for (int i = 0; i < 7; i++)
g_js.position[i] = 0;
KDL::TreeFkSolverPosFull_recursive fk_solver(tree);
g_fk_solver = &fk_solver;
KDL::SegmentMap::const_iterator root_seg = tree.getRootSegment();
string tree_root_name = root_seg->first;
printf("root: %s\n", tree_root_name.c_str());
KDL::ChainFkSolverPos_recursive fk_solver_chain(chain);
KDL::ChainIkSolverVel_pinv ik_solver_vel(chain);
KDL::JntArray q_min(7), q_max(7);
for (int i = 0; i < 7; i++)
{
q_min.data[i] = g_joint_min[i];
q_max.data[i] = g_joint_max[i];
}
KDL::ChainIkSolverPos_NR_JL ik_solver_pos(chain, q_min, q_max,
fk_solver_chain, ik_solver_vel,
100, 1e-6);
//KDL::ChainIkSolverPos_NR ik_solver_pos(chain, fk_solver_chain, ik_solver_vel, 100, 1e-6);
g_ik_solver = &ik_solver_pos;
KDL::ChainJntToJacSolver jac_solver(chain);
g_jac_solver = &jac_solver;
//boost::scoped_ptr<KDL::TreeFkSolverPosFull_recursive> fk_solver;
g_pose.resize(7);
g_pose[0] = .3;
g_pose[1] = -.3;
g_pose[2] = -.6;
g_pose[3] = 0;
g_pose[4] = 0;
g_pose[5] = 0;
g_pose[6] = 0;
//tf::Transform t_tool = fk_tool(g_pose);
//double d_pose = 0, d_pose_inc = 0.01;
// set origin to be something we can comfortably reach
//g_target_origin = fk_tool(g_pose);
//btQuaternion target_quat;
//target_quat.setEuler(1.57, -1.57, 0);
//g_target_origin.setRotation(target_quat);
//g_target_origin = btTransform(btQuaternion::getIdentity(), btVector3(0, 0.1, 0));
//tf::Transform(btQuaternion::getIdentity(), btVector3(0, 0, 0));
std::vector<double> j_ik;
j_ik.resize(7);
while (ros::ok())
{
loop_rate.sleep();
ros::spinOnce();
world_trans.header.stamp = ros::Time::now();
tf_broadcaster.sendTransform(world_trans);
if (g_actual_js.position.size() >= 7)
{
for (int i = 0; i < 7; i++)
j_ik[i] = g_actual_js.position[i];
}
tf::StampedTransform t;
try
{
g_tf_listener->lookupTransform("torso_link", "ik_target",
ros::Time(0), t);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
continue;
}
if (ik_tool(t, j_ik, g_posture, g_posture_gain))
{
g_js.header.stamp = ros::Time::now();
g_js.position = j_ik;
g_joint_pub->publish(g_js);
}
ros::spinOnce();
/*
double x = t.getOrigin().x(), y = t.getOrigin().y(), z = t.getOrigin().z();
double roll, pitch, yaw;
btMatrix3x3(t.getRotation()).getRPY(roll, pitch, yaw);
printf("%.3f %.3f %.3f %.3f %.3f %.3f\n", x, y, z, roll, pitch, yaw);
*/
//std::vector<double> j_ik = pose;
//js.position[2] += 1;
//j_ik = pose;js.position;
//printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
// j_ik[0], j_ik[1], j_ik[2], j_ik[3], j_ik[4], j_ik[5], j_ik[6]);
//j_ik[2] += posture;
//j_ik[3] += posture;
//j_ik[4] += posture;
#if 0
tf::Transform t_bump(btQuaternion::getIdentity(),
btVector3(d_pose, 0, 0));
//tf::Transform t_target = t_tool * t_bump;
ik_tool(t_tool * t_bump, j_ik);
#endif
//double x = t.getOrigin().x(), y = t.getOrigin().y(), z = t.getOrigin().z();
/*
printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
posture,
j_ik[0], j_ik[1], j_ik[2], j_ik[3], j_ik[4], j_ik[5], j_ik[6]);
printf("\n");
*/
/*
js.header.stamp = ros::Time::now();
js.position = j_ik;
joint_pub.publish(js);
d_pose += d_pose_inc;
if (d_pose > 0.28)
d_pose_inc = -0.005;
else if (d_pose < -0.30)
d_pose_inc = 0.005;
printf("%f %f\n", d_pose, d_pose_inc);
*/
}
return 0;
}
RobotStatePublisher::RobotStatePublisher(const KDL::Tree& tree)
{
// walk the tree and add segments to segments_
addChildren(tree.getRootSegment());
}
bool TreeKinematics::readJoints(urdf::Model &robot_model,
KDL::Tree &kdl_tree,
std::string &tree_root_name,
unsigned int &nr_of_jnts,
KDL::JntArray &joint_min,
KDL::JntArray &joint_max,
KDL::JntArray &joint_vel_max)
{
KDL::SegmentMap segmentMap;
segmentMap = kdl_tree.getSegments();
tree_root_name = kdl_tree.getRootSegment()->second.segment.getName();
nr_of_jnts = kdl_tree.getNrOfJoints();
ROS_DEBUG( "the tree's number of joints: [%d]", nr_of_jnts );
joint_min.resize(nr_of_jnts);
joint_max.resize(nr_of_jnts);
joint_vel_max.resize(nr_of_jnts);
info_.joint_names.resize(nr_of_jnts);
info_.limits.resize(nr_of_jnts);
// The following walks through all tree segments, extracts their joints except joints of KDL::None and extracts
// the information about min/max position and max velocity of the joints not of type urdf::Joint::UNKNOWN or
// urdf::Joint::FIXED
ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None.");
boost::shared_ptr<const urdf::Joint> joint;
for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); ++seg_it)
{
if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None)
{
// check, if joint can be found in the URDF model of the robot
joint = robot_model.getJoint(seg_it->second.segment.getJoint().getName().c_str());
if (!joint)
{
ROS_FATAL("Joint '%s' has not been found in the URDF robot model!", joint->name.c_str());
return false;
}
// extract joint information
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
{
ROS_DEBUG( "getting information about joint: [%s]", joint->name.c_str() );
double lower = 0.0, upper = 0.0, vel_limit = 0.0;
unsigned int has_pos_limits = 0, has_vel_limits = 0;
if ( joint->type != urdf::Joint::CONTINUOUS )
{
ROS_DEBUG("joint is not continuous.");
lower = joint->limits->lower;
upper = joint->limits->upper;
has_pos_limits = 1;
if (joint->limits->velocity)
{
has_vel_limits = 1;
vel_limit = joint->limits->velocity;
ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
}
else
{
has_vel_limits = 0;
vel_limit = 0.0;
ROS_DEBUG("joint has no velocity limit.");
}
}
else
{
ROS_DEBUG("joint is continuous.");
lower = -M_PI;
upper = M_PI;
has_pos_limits = 0;
if(joint->limits && joint->limits->velocity)
{
has_vel_limits = 1;
vel_limit = joint->limits->velocity;
ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
}
else
{
has_vel_limits = 0;
vel_limit = 0.0;
ROS_DEBUG("joint has no velocity limit.");
}
}
joint_min(seg_it->second.q_nr) = lower;
joint_max(seg_it->second.q_nr) = upper;
joint_vel_max(seg_it->second.q_nr) = vel_limit;
ROS_DEBUG("pos_min = %f, pos_max = %f, vel_max = %f", lower, upper, vel_limit);
info_.joint_names[seg_it->second.q_nr] = joint->name;
info_.limits[seg_it->second.q_nr].joint_name = joint->name;
info_.limits[seg_it->second.q_nr].has_position_limits = has_pos_limits;
info_.limits[seg_it->second.q_nr].min_position = lower;
info_.limits[seg_it->second.q_nr].max_position = upper;
info_.limits[seg_it->second.q_nr].has_velocity_limits = has_vel_limits;
info_.limits[seg_it->second.q_nr].max_velocity = vel_limit;
}
}
}
return true;
}