bool kdl_urdf_tools::initialize_kinematics_from_urdf(
const std::string &robot_description,
const std::string &root_link,
const std::string &tip_link,
unsigned int &n_dof,
KDL::Chain &kdl_chain,
KDL::Tree &kdl_tree,
urdf::Model &urdf_model)
{
if(robot_description.length() == 0) {
ROS_ERROR("URDF string is empty.");
return false;
}
// Construct an URDF model from the xml string
urdf_model.initString(robot_description);
// Get a KDL tree from the robot URDF
if (!kdl_parser::treeFromUrdfModel(urdf_model, kdl_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
// Populate the KDL chain
if(!kdl_tree.getChain(root_link, tip_link, kdl_chain))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_link<<" --> "<<tip_link);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin();
it != segment_map.end();
it++ )
{
ROS_ERROR_STREAM( " "<<(*it).first);
}
return false;
}
// Store the number of degrees of freedom of the chain
n_dof = kdl_chain.getNrOfJoints();
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;
}
bool Kinematics::build_chain(FINGERS f, const KDL::Tree &kdl_tree_){
std::string root_name, tip_name;
se_finger finger_info;
std::string finger_name;
try{
finger_info = root_tip.at(f);
root_name = finger_info.root_name;
tip_name = finger_info.tip_name;
}catch(const std::out_of_range& oor){
ROS_ERROR("Kinematics::build_chain no such finger type");
return false;
}
// joint_limits& joint_limits_ = finger_joint_limits.at(f);
KDL::Chain& kdl_chain_ = kdl_chains[f];
finger_name = f_enum2str.at(f);
// Populate the KDL chain
if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_name<<" --> "<<tip_name);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree_.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin(); it != segment_map.end(); it++ )
ROS_ERROR_STREAM( " "<<(*it).first);
return false;
}
ROS_INFO("=== Loading %s KChain ===",finger_name.c_str());
ROS_INFO("root: %s\t tip: %s",root_name.c_str(),tip_name.c_str());
ROS_INFO("Number of segments: %d",kdl_chain_.getNrOfSegments());
ROS_INFO("Number of joints in chain: %d",kdl_chain_.getNrOfJoints());
// Parsing joint limits from urdf model
/* boost::shared_ptr<const urdf::Link> link_ = model.getLink(tip_name);
boost::shared_ptr<const urdf::Joint> joint_;
joint_limits_.min.resize(kdl_chain_.getNrOfJoints());
joint_limits_.max.resize(kdl_chain_.getNrOfJoints());
joint_limits_.center.resize(kdl_chain_.getNrOfJoints());*/
/*int index;
for (int i = 0; i < kdl_chain_.getNrOfJoints() && link_; i++)
{
joint_ = model.getJoint(link_->parent_joint->name);
index = kdl_chain_.getNrOfJoints() - i - 1;
joint_limits_.min(index) = joint_->limits->lower;
joint_limits_.max(index) = joint_->limits->upper;
joint_limits_.center(index) = (joint_limits_.min(index) + joint_limits_.max(index))/2;
link_ = model.getLink(link_->getParent()->name);
}
*/
/* std::cout<< "segment names" << std::endl;
int index;
for(std::size_t i = 0; i < kdl_chain_.getNrOfJoints();i++){
// std::cout<< kdl_chain_.segments[i].getName() << " " << kdl_chain_.segments[i].getJoint().getName() << " " << kdl_chain_.segments[i].getJoint().getTypeName() << std::endl;
joint_ = model.getJoint(kdl_chain_.segments[i].getJoint().getName());
index = i;
switch(joint_->type){
case urdf::Joint::REVOLUTE:
{
joint_limits_.min(i) = joint_->limits->lower;
joint_limits_.max(i) = joint_->limits->upper;
joint_limits_.center(i) = (joint_limits_.min(i) + joint_limits_.max(i))/2;
ROS_INFO("joint name: %s \t type: %d \t %f %f",joint_->name.c_str(),joint_->type,joint_->limits->lower,joint_->limits->upper);
break;
}
case urdf::Joint::FIXED:
{
ROS_INFO("joint name: %s \t type: %d",joint_->name.c_str(),joint_->type);
break;
}
default:
{
std::cerr<< "Kinematics::build_chain no such joint type: " << joint_->type << " implemented" << std::endl;
break;
}
}
}*/
}
void createMyTree(KDL::Tree& a_tree)
{
unsigned int numberofsegments = 66;
unsigned int numberofbranches = 5;
std::string linknamecontainer[numberofsegments];
Joint jointcontainer[numberofsegments];
Frame framecontainer[numberofsegments];
Segment segmentcontainer[numberofsegments];
RigidBodyInertia inertiacontainer[numberofsegments];
RotationalInertia rotInerSeg(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); //around symmetry axis of rotation
double pointMass = 0.25; //in kg
std::string jointname, linkname;
//create names and segments of the tree
for (unsigned int i = 0; i < numberofsegments - 2; i = i + 3)
{
//this name manipulation is required to ensure that
//topological order of segments in the tree and the order std::map data structure are equivalent
if (i < 10)
{
ostringstream converter, converter3;
converter << "joint00" << i;
jointname = converter.str();
converter3 << "link00" << i;
linkname = converter3.str();
linknamecontainer[i] = linkname;
// std::cout << jointname << linkname << std::endl;
}
else
{
ostringstream converter, converter3;
converter << "joint0" << i;
jointname = converter.str();
converter3 << "link0" << i;
linkname = converter3.str();
linknamecontainer[i] = linkname;
// std::cout << jointname << linkname << std::endl;
}
jointcontainer[i] = Joint(jointname, Joint::RotZ, 1, 0, 0.01);
framecontainer[i] = Frame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
segmentcontainer[i] = Segment(linkname, jointcontainer[i], framecontainer[i]);
inertiacontainer[i] = RigidBodyInertia(pointMass, Vector(0.0, -0.4, 0.0), rotInerSeg);
segmentcontainer[i].setInertia(inertiacontainer[i]);
if (i < 9)
{
ostringstream converter1, converter4;
converter1 << "joint00" << i + 1;
jointname = converter1.str();
converter4 << "link00" << i + 1;
linkname = converter4.str();
linknamecontainer[i + 1] = linkname;
// std::cout << jointname << linkname << std::endl;
}
else
{
ostringstream converter1, converter4;
converter1 << "joint0" << i + 1;
jointname = converter1.str();
converter4 << "link0" << i + 1;
linkname = converter4.str();
linknamecontainer[i + 1] = linkname;
// std::cout << jointname << linkname << std::endl;
}
jointcontainer[i + 1] = Joint(jointname, Joint::RotX, 1, 0, 0.01);
framecontainer[i + 1] = Frame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
segmentcontainer[i + 1] = Segment(linkname, jointcontainer[i + 1], framecontainer[i + 1]);
inertiacontainer[i + 1] = RigidBodyInertia(pointMass, Vector(0.0, -0.4, 0.0), rotInerSeg);
segmentcontainer[i + 1].setInertia(inertiacontainer[i + 1]);
if (i < 8)
{
ostringstream converter2, converter5;
converter2 << "joint00" << i + 2;
jointname = converter2.str();
converter5 << "link00" << i + 2;
linkname = converter5.str();
linknamecontainer[i + 2] = linkname;
// std::cout << jointname << linkname << std::endl;
}
else
{
ostringstream converter2, converter5;
converter2 << "joint0" << i + 2;
jointname = converter2.str();
converter5 << "link0" << i + 2;
linkname = converter5.str();
linknamecontainer[i + 2] = linkname;
// std::cout << jointname << linkname << std::endl;
}
jointcontainer[i + 2] = Joint(jointname, Joint::RotY, 1, 0, 0.01);
framecontainer[i + 2] = Frame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
segmentcontainer[i + 2] = Segment(linkname, jointcontainer[i + 2], framecontainer[i + 2]);
inertiacontainer[i + 2] = RigidBodyInertia(pointMass, Vector(0.0, -0.4, 0.0), rotInerSeg);
segmentcontainer[i + 2].setInertia(inertiacontainer[i + 2]);
}
//add created segments to the tree (1 initial base chain + 5 x branches)
//connect initial base chain to tree root
a_tree.addSegment(segmentcontainer[0], "L0");
std::cout << "Initial base chain" << std::endl;
for (unsigned int i = 0; i < numberofbranches - 1; i++) //chain including link0-link4 (5 segments)
{
a_tree.addSegment(segmentcontainer[i + 1], linknamecontainer[i]);
std::cout << linknamecontainer[i] << " and " << segmentcontainer[i + 1].getName() << std::endl;
}
int initialChainElementNumber = a_tree.getNrOfSegments(); //number of segments in initial base chain section of the tree
int elementsInBranch = (numberofsegments - initialChainElementNumber) / numberofbranches; //number of elements in each branch
//connect 1st branch to the last link of the initial chain
a_tree.addSegment(segmentcontainer[numberofbranches], linknamecontainer[numberofbranches - 1]);
std::cout << "Branch " << numberofbranches-4 << std::endl;
//segments of the 1st tree branch
for (unsigned int j = numberofbranches; j < (elementsInBranch + initialChainElementNumber) - 1; j++)
{
a_tree.addSegment(segmentcontainer[j + 1], linknamecontainer[j]);
std::cout << linknamecontainer[j] << " and " << segmentcontainer[j + 1].getName() << std::endl;
}
//connect 2nd branch to the last link of the initial chain
a_tree.addSegment(segmentcontainer[(elementsInBranch + initialChainElementNumber)], linknamecontainer[numberofbranches - 2]);
std::cout << "Branch " << numberofbranches-3 << std::endl;
//segments of the 2nd tree branch
for (unsigned int j = (elementsInBranch + initialChainElementNumber); j < (2 * elementsInBranch + initialChainElementNumber) - 1; j++)
{
a_tree.addSegment(segmentcontainer[j + 1], linknamecontainer[j]);
std::cout << linknamecontainer[j] << " and " << segmentcontainer[j + 1].getName() << std::endl;
}
//connect 3rd branch to the last link of the initial chain
a_tree.addSegment(segmentcontainer[(2 * elementsInBranch + initialChainElementNumber)], linknamecontainer[numberofbranches - 3]);
std::cout << "Branch " << numberofbranches-2 << std::endl;
//segments of the 3rd tree branch
for (unsigned int j = (2 * elementsInBranch + initialChainElementNumber); j < (3 * elementsInBranch + initialChainElementNumber) - 1; j++)
{
a_tree.addSegment(segmentcontainer[j + 1], linknamecontainer[j]);
std::cout << linknamecontainer[j] << " and " << segmentcontainer[j + 1].getName() << std::endl;
}
//connect 4th branch to the last link of the initial chain
a_tree.addSegment(segmentcontainer[(3 * elementsInBranch + initialChainElementNumber)], linknamecontainer[numberofbranches - 4]);
std::cout << "Branch " << numberofbranches-1 << std::endl;
//segments of the 4ht tree branch
for (unsigned int j = (3 * elementsInBranch + initialChainElementNumber); j < (4 * elementsInBranch + initialChainElementNumber) - 1; j++)
{
a_tree.addSegment(segmentcontainer[j + 1], linknamecontainer[j]);
std::cout << linknamecontainer[j] << " and " << segmentcontainer[j + 1].getName() << std::endl;
}
//connect 5th branch to the last link of the initial chain
a_tree.addSegment(segmentcontainer[(4 * elementsInBranch + initialChainElementNumber)], linknamecontainer[numberofbranches - 1]);
//segments of the 5th tree branch
std::cout << "Branch " << numberofbranches << std::endl;
for (unsigned int j = (4 * elementsInBranch + initialChainElementNumber); j < (5 * elementsInBranch + initialChainElementNumber) - 1; j++)
{
a_tree.addSegment(segmentcontainer[j + 1], linknamecontainer[j]);
std::cout << linknamecontainer[j] << " and " << segmentcontainer[j + 1].getName() << std::endl;
}
}
void computeTemplatedDynamicsForTree(KDL::Tree& twoBranchTree, KDL::Vector& grav, std::vector<kdle::JointState>& jointState,
std::vector<kdle::SegmentState>& linkState, std::vector<kdle::SegmentState>& linkState2)
{
printf("Templated dynamics values for Tree \n");
kdle::transform<kdle::kdl_tree_iterator, kdle::pose> _comp1;
kdle::transform<kdle::kdl_tree_iterator, kdle::twist> _comp2;
kdle::transform<kdle::kdl_tree_iterator, kdle::accTwist> _comp3;
kdle::balance<kdle::kdl_tree_iterator, kdle::force> _comp4;
kdle::project<kdle::kdl_tree_iterator, kdle::wrench> _comp5;
#ifdef VERBOSE_CHECK_MAIN
std::cout << "Transform initial state" << std::endl << linkState[0].X << std::endl;
std::cout << "Twist initial state" << std::endl << linkState[0].Xdot << std::endl;
std::cout << "Acc Twist initial state" << std::endl << linkState[0].Xdotdot << std::endl;
std::cout << "Wrench initial state" << std::endl << linkState[0].F << std::endl << std::endl;
#endif
#ifdef VERBOSE_CHECK_MAIN
std::cout << "Transform L1" << linkState[1].X << std::endl;
std::cout << "Twist L1" << linkState[1].Xdot << std::endl;
std::cout << "Acc Twist L1" << linkState[1].Xdotdot << std::endl;
std::cout << "Wrench L1" << linkState[1].F << std::endl << std::endl;
#endif
#ifdef VERBOSE_CHECK_MAIN
std::cout << "Transform L2" << linkState[2].X << std::endl;
std::cout << "Twist L2" << linkState[2].Xdot << std::endl;
std::cout << "Acc Twist L2" << linkState[2].Xdotdot << std::endl;
std::cout << "Wrench L2" << linkState[2].F << std::endl << std::endl;
#endif
//typedef Composite<kdle::func_ptr(myTestComputation), kdle::func_ptr(myTestComputation) > compositeType0;
typedef kdle::Composite< kdle::transform<kdle::kdl_tree_iterator, kdle::twist>, kdle::transform<kdle::kdl_tree_iterator, kdle::pose> > compositeType1;
typedef kdle::Composite< kdle::balance<kdle::kdl_tree_iterator, kdle::force>, kdle::transform<kdle::kdl_tree_iterator, kdle::accTwist> > compositeType2;
typedef kdle::Composite<compositeType2, compositeType1> compositeType3;
// compositeType1 composite1 = kdle::compose(_comp2, _comp1);
compositeType3 composite2 = kdle::compose(kdle::compose(_comp4, _comp3), kdle::compose(_comp2, _comp1));
//kdle::DFSPolicy<KDL::Tree> mypolicy;
kdle::DFSPolicy<KDL::Tree, kdle::inward> mypolicy1;
kdle::DFSPolicy<KDL::Tree, kdle::outward> mypolicy2;
std::cout << std::endl << std::endl << "FORWARD TRAVERSAL" << std::endl << std::endl;
// traverseGraph(twoBranchTree, composite2, mypolicy2)(jointState, jointState, linkState, linkState2);
traverseGraph(twoBranchTree, composite2, mypolicy2)(jointState, linkState, linkState2);
#ifdef VERBOSE_CHECK_MAIN
std::cout << std::endl << std::endl << "LSTATE" << std::endl << std::endl;
for (unsigned int i = 0; i < twoBranchTree.getNrOfSegments(); i++)
{
std::cout << linkState[i].segmentName << std::endl;
std::cout << std::endl << linkState[i].X << std::endl;
std::cout << linkState[i].Xdot << std::endl;
std::cout << linkState[i].Xdotdot << std::endl;
std::cout << linkState[i].F << std::endl;
}
std::cout << std::endl << std::endl << "LSTATE2" << std::endl << std::endl;
for (unsigned int i = 0; i < twoBranchTree.getNrOfSegments(); i++)
{
std::cout << linkState2[i].segmentName << std::endl;
std::cout << std::endl << linkState2[i].X << std::endl;
std::cout << linkState2[i].Xdot << std::endl;
std::cout << linkState2[i].Xdotdot << std::endl;
std::cout << linkState2[i].F << std::endl;
}
#endif
std::vector<kdle::SegmentState> linkState3;
linkState3.resize(twoBranchTree.getNrOfSegments()+1);
std::cout << std::endl << std::endl << "REVERSE TRAVERSAL" << std::endl << std::endl;
std::vector<kdle::JointState> jstate1;
jstate1.resize(twoBranchTree.getNrOfSegments() + 1);
traverseGraph(twoBranchTree, _comp5, mypolicy1)(jointState, jstate1, linkState2, linkState3);
//version 1 traversal
//traverseGraph(twoBranchTree, kdl_extensions::func_ptr(myTestComputation), mypolicy)(1, 2, 3);
#ifdef VERBOSE_CHECK_MAIN
std::cout << std::endl << std::endl << "LSTATE3" << std::endl << std::endl;
for (KDL::SegmentMap::const_reverse_iterator iter = twoBranchTree.getSegments().rbegin(); iter != twoBranchTree.getSegments().rend(); ++iter)
{
std::cout << std::endl << iter->first << std::endl;
std::cout << linkState3[iter->second.q_nr].X << std::endl;
std::cout << linkState3[iter->second.q_nr].Xdot << std::endl;
std::cout << linkState3[iter->second.q_nr].Xdotdot << std::endl;
std::cout << linkState3[iter->second.q_nr].F << std::endl;
std::cout << "Joint index and torque " << iter->second.q_nr << " " << jstate1[iter->second.q_nr].torque << std::endl;
}
#endif
return;
}
void drawMyTree(KDL::Tree& twoBranchTree)
{
//graphviz stuff
/****************************************/
Agraph_t *g;
GVC_t *gvc;
/* set up a graphviz context */
gvc = gvContext();
/* Create a simple digraph */
g = agopen("robot-structure", AGDIGRAPH);
//create vector to hold nodes
std::vector<Agnode_t*> nodeVector;
nodeVector.resize(twoBranchTree.getSegments().size());
printf("size of segments in tree map %d\n", twoBranchTree.getSegments().size());
printf("size of segments in tree %d\n", twoBranchTree.getNrOfSegments());
//create vector to hold edges
std::vector<Agedge_t*> edgeVector;
edgeVector.resize(twoBranchTree.getNrOfJoints() + 1);
int jointIndex = twoBranchTree.getNrOfJoints() + 1;
printf("size of joint array %d %d\n", jointIndex, twoBranchTree.getNrOfJoints());
int segmentIndex = 0;
// fill in the node vector by iterating over tree segments
for (KDL::SegmentMap::const_iterator iter = twoBranchTree.getSegments().begin(); iter != twoBranchTree.getSegments().end(); ++iter)
{
//it would have been very useful if one could access list of joints of a tree
//list of segments is already possible
int stringLength = iter->second.segment.getName().size();
char name[stringLength + 1];
strcpy(name, iter->second.segment.getName().c_str());
//q_nr returned is the same value for the root and the its child. this is a bug
nodeVector[iter->second.q_nr] = agnode(g, name);
agsafeset(nodeVector[iter->second.q_nr], "color", "red", "");
agsafeset(nodeVector[iter->second.q_nr], "shape", "box", "");
std::cout << "index parent " << iter->second.q_nr << std::endl;
std::cout << "name parent " << iter->second.segment.getName() << std::endl;
std::cout << "joint name parent " << iter->second.segment.getJoint().getName() << std::endl;
std::cout << "joint type parent " << iter->second.segment.getJoint().getType() << std::endl;
// if (iter->second.segment.getJoint().getType() == Joint::None) //equals to joint type None
// {
// int stringLength = iter->second.segment.getJoint().getName().size();
// char name[stringLength + 1];
// strcpy(name, iter->second.segment.getJoint().getName().c_str());
// edgeVector[iter->second.q_nr] = agedge(g, nodeVector[iter->second.q_nr], nodeVector[iter->second.q_nr]);
// agsafeset(edgeVector[iter->second.q_nr], "label", name, "");
// }
if (segmentIndex < twoBranchTree.getSegments().size())
segmentIndex++;
}
//fill in edge vector by iterating over joints in the tree
for (KDL::SegmentMap::const_iterator iter = twoBranchTree.getSegments().begin(); iter != twoBranchTree.getSegments().end(); ++iter)
{
//TODO: Fix node-edge connection relation
int stringLength = iter->second.segment.getJoint().getName().size();
std::cout << "Joint name " << iter->second.segment.getJoint().getName() << std::endl;
char name[stringLength + 1];
strcpy(name, iter->second.segment.getJoint().getName().c_str());
for (std::vector<KDL::SegmentMap::const_iterator>::const_iterator childIter = iter->second.children.begin(); childIter != iter->second.children.end(); childIter++)
{
edgeVector[iter->second.q_nr] = agedge(g, nodeVector[iter->second.q_nr], nodeVector[(*childIter)->second.q_nr]);
agsafeset(edgeVector[iter->second.q_nr], "label", name, "");
}
// if (jointIndex != 0)
// {
// edgeVector[jointIndex] = agedge(g, nodeVector[segmentIndex], nodeVector[jointIndex]);
// agsafeset(edgeVector[jointIndex], "label", name, "");
// }
}
/* Compute a layout using layout engine from command line args */
// gvLayoutJobs(gvc, g);
gvLayout(gvc, g, "dot");
/* Write the graph according to -T and -o options */
//gvRenderJobs(gvc, g);
gvRenderFilename(gvc, g, "ps", "test-fext.ps");
/* Free layout data */
gvFreeLayout(gvc, g);
/* Free graph structures */
agclose(g);
gvFreeContext(gvc);
/* close output file, free context, and return number of errors */
return;
}