C++ Tree::getNrOfSegments方法代码示例

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));
//.........这里部分代码省略.........

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;
        }
        }

    }*/

}

//.........这里部分代码省略.........
        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;
}