Esempio n. 1
0
    bool perform_ik(trac_ik_baxter::GetConstrainedPositionIK::Request &request,
                    trac_ik_baxter::GetConstrainedPositionIK::Response &response) {

          if(!this->_ready)
              return false;

          double initial_tolerance = 1e-5;
          if(request.num_steps == 0)
              request.num_steps = 1;
          if(request.end_tolerance < initial_tolerance) {
              request.num_steps = 1;
              ROS_WARN("Invalid IK end tolerance, using the default");
          }
          double step = (request.end_tolerance - initial_tolerance)/request.num_steps;

          int rc;
          KDL::JntArray result;
          sensor_msgs::JointState joint_state;
          for(uint segment=0; segment<this->_chain.getNrOfSegments(); ++segment) {
              KDL::Joint joint = this->_chain.getSegment(segment).getJoint();
              if(joint.getType()!=KDL::Joint::None)
                  joint_state.name.push_back(joint.getName());
          }
          bool seeds_provided = request.seed_angles.size() == request.pose_stamp.size();

          for(uint point=0; point<request.pose_stamp.size(); ++point) {
              KDL::Frame end_effector_pose(KDL::Rotation::Quaternion(request.pose_stamp[point].pose.orientation.x,
                                                                     request.pose_stamp[point].pose.orientation.y,
                                                                     request.pose_stamp[point].pose.orientation.z,
                                                                     request.pose_stamp[point].pose.orientation.w),
                                           KDL::Vector(request.pose_stamp[point].pose.position.x,
                                                       request.pose_stamp[point].pose.position.y,
                                                       request.pose_stamp[point].pose.position.z));

              KDL::JntArray seed(this->_chain.getNrOfJoints());
              if(seeds_provided)
                  seed = JointState2JntArray(request.seed_angles[point]);

              for(uint num_attempts=0; num_attempts<request.num_steps; ++num_attempts) {
                  std::stringstream ss;
                  double tolerance = initial_tolerance + num_attempts*step;
                  ss << "Attempt num " << num_attempts +1 << " with tolerence " << tolerance << std::endl;
                  ROS_INFO("%s", ss.str().c_str());
                  this->_tracik_solver->setEpsilon(tolerance);
                  rc = this->_tracik_solver->CartToJnt(seeds_provided? seed: *(this->_nominal),
                                                       end_effector_pose, result);
                  if(rc>=0) break;
              }

              for(uint joint=0; joint<this->_chain.getNrOfJoints(); ++joint) {
                  joint_state.position.push_back(result(joint));
              }

              response.joints.push_back(joint_state);
              response.isValid.push_back(rc>=0);
          }
          return true;
        }
Esempio n. 2
0
sensor_msgs::JointState init_message(const KDL::Chain &chain) {
	sensor_msgs::JointState msg;
	for (unsigned int i=0; i < chain.getNrOfSegments(); ++i) {
		KDL::Segment segment = chain.getSegment(i);
		KDL::Joint joint = segment.getJoint();
		if (joint.getType() == KDL::Joint::JointType::None) continue;
		msg.name.push_back(joint.getName());
		msg.position.push_back(0);
	}
	return msg;
}
Esempio n. 3
0
KDL::Frame getH_new_old(KDL::Joint jnt, KDL::Frame frameToTip)
{
    KDL::Frame H_new_old;
    if( (jnt.JointOrigin()-frameToTip.p).Norm() < 1e-12 || jnt.getType() == KDL::Joint::None ) {
        //No need of changing link frame
        H_new_old = KDL::Frame::Identity();
    } else {
        std::cerr << "[WARN] the reference frame of link connected to joint " << jnt.getName()  << "  has to be shifted to comply to URDF constraints" << std::endl;
        //H_new_old = (KDL::Frame(jnt.JointOrigin())*KDL::Frame(frameToTip.M)).Inverse()*frameToTip;
        H_new_old = KDL::Frame(frameToTip.M.Inverse()*(frameToTip.p-jnt.JointOrigin()));
    }
    return H_new_old;
}
Esempio n. 4
0
    int JointInvertPolarity(const KDL::Joint & old_joint, const KDL::Frame & old_f_tip, KDL::Joint & new_joint, KDL::Frame & new_f_tip)
    {
 
        switch(old_joint.getType()) 
        {
            case Joint::RotAxis:
            case Joint::RotX:
            case Joint::RotY:
            case Joint::RotZ:
                //\todo document this
                new_f_tip = KDL::Frame(old_f_tip.M.Inverse(),-(old_f_tip.M.Inverse()*old_joint.JointOrigin()));
                new_joint = Joint(old_joint.getName(),-(old_f_tip.M.Inverse()*old_f_tip.p), -(old_f_tip.M.Inverse()*old_joint.JointAxis()), Joint::RotAxis);
            break;
            case Joint::TransAxis:
            case Joint::TransX:
            case Joint::TransY:
            case Joint::TransZ:
                new_f_tip = KDL::Frame(old_f_tip.M.Inverse(),-(old_f_tip.M.Inverse()*old_f_tip.p));
                new_joint = Joint(old_joint.getName(),-(old_f_tip.M.Inverse()*old_joint.JointOrigin()), -(old_f_tip.M.Inverse()*old_joint.JointAxis()), Joint::TransAxis);
            break;
            case Joint::None:
            default:
                new_f_tip = KDL::Frame(old_f_tip.M.Inverse(),-(old_f_tip.M.Inverse()*old_f_tip.p));
                new_joint = Joint(old_joint.getName(), Joint::None);
            break;
        }
        #ifndef NDEBUG
        std::cerr << "Exiting joint polarity inversion, checking all is working" << std::endl;
        double q = 0.83;
        std::cerr << old_joint.pose(q)*old_f_tip*new_joint.pose(q)*new_f_tip << std::endl;
        #endif 
        return 0;
    }
Esempio n. 5
0
int main()
{
    KDLCollada kdlCollada;
    vector <KDL::Chain> kinematicsModels;
    const string filename =  "puma.dae";  // loading collada kinematics model and converting it to kdl serial chain
    if (!kdlCollada.load(COLLADA_MODELS_PATH + filename, kinematicsModels))
    {
        cout << "Failed to import " << filename;
        return 0;
    }

    cout << "Imported " << kinematicsModels.size() << " kinematics chains" << endl;

    for (unsigned int i = 0; i < kinematicsModels.size(); i++)  // parsing output kdl serail chain
    {
        KDL::Chain chain = kinematicsModels[i];
        cout << "Chain " << i << " has " << chain.getNrOfSegments() << " segments" << endl;

        for (unsigned int u = 0; u < chain.getNrOfSegments(); u++)
        {
            KDL::Segment segment = chain.segments[u];
            string segmentName = segment.getName();
            cout << "Segment " << segmentName << " :" <<endl;

            KDL::Frame f_tip = segment.getFrameToTip();
            KDL::Vector rotAxis = f_tip.M.GetRot();
            double rotAngle = f_tip.M.GetRotAngle(rotAxis);
            KDL::Vector trans = f_tip.p;
            cout << "   frame: rotation " << rotAxis.x() << " " << rotAxis.y() << " " << rotAxis.z() << " " << rotAngle * 180 / M_PI << endl;
            cout << "   frame: translation " << trans.x() << " " << trans.y() << " " << trans.z() << endl;

            KDL::RigidBodyInertia inertia = segment.getInertia();
            KDL::Joint joint = segment.getJoint();

            string jointName = joint.getName();
            string jointType = joint.getTypeName();
            KDL::Vector jointAxis = joint.JointAxis();
            KDL::Vector jointOrigin = joint.JointOrigin();

            cout << "   joint name: " << jointName << endl;
            cout << "         type: " << jointType << endl;
            cout << "         axis: " << jointAxis.x() << " " <<jointAxis.y() << " " << jointAxis.z() << endl;
            cout << "         origin: " << jointOrigin.x() << " " << jointOrigin.y() << " " << jointOrigin.z() << endl;
        }
    }

    return 0;
}
bool TransformationCalculator::is_valid_joint(const KDL::Joint& joint){
    if(!is_valid_joint_type(joint))
        return false;
    if(!is_valid_joint_name(joint.getName()))
        return false;

    return true;
}
Esempio n. 7
0
/**
 *
 * @param jnt the KDL::Joint to convert (axis and origin expressed in the
 *          predecessor frame of reference, as by KDL convention)
 * @param frameToTip the predecessor/successor frame transformation
 * @param H_new_old_predecessor in the case the predecessor frame is being
 *          modified to comply to URDF constraints (frame origin on the joint axis)
 *          this matrix the transformation from the old frame to the new frame (H_new_old)
 * @param H_new_old_successor  in the case the successor frame is being
 *          modified to comply to URDF constraints (frame origin on the joint axis)
 *          this matrix the transformation from the old frame to the new frame (H_new_old)
 */
urdf::Joint toUrdf(const KDL::Joint & jnt,
                   const KDL::Frame & frameToTip,
                   const KDL::Frame & H_new_old_predecessor,
                   KDL::Frame & H_new_old_successor)
{
    //URDF constaints the successor link frame origin to lay on the axis
    //of the joint ( see : http://www.ros.org/wiki/urdf/XML/joint )
    //Then if the JointOrigin of the KDL joint is not zero, it is necessary
    //to move the link frame (then it is necessary to change also the spatial inertia)
    //and the definition of the childrens of the successor frame
    urdf::Joint ret;
    ret.name = jnt.getName();

    H_new_old_successor = getH_new_old(jnt,frameToTip);

    ret.parent_to_joint_origin_transform = toUrdf(H_new_old_predecessor*frameToTip*(H_new_old_successor.Inverse()));

    switch(jnt.getType())
    {
        case KDL::Joint::RotAxis:
        case KDL::Joint::RotX:
        case KDL::Joint::RotY:
        case KDL::Joint::RotZ:
            //using continuos if no joint limits are specified
            ret.type = urdf::Joint::CONTINUOUS;
            //in urdf, the joint axis is expressed in the joint/successor frame
            //in kdl, the joint axis is expressed in the predecessor rame
            ret.axis = toUrdf(((frameToTip).M.Inverse((jnt.JointAxis()))));
        break;
        case KDL::Joint::TransAxis:
        case KDL::Joint::TransX:
        case KDL::Joint::TransY:
        case KDL::Joint::TransZ:
            ret.type = urdf::Joint::PRISMATIC;
            //in urdf, the joint axis is expressed in the joint/successor frame
            //in kdl, the joint axis is expressed in the predecessor rame
            ret.axis = toUrdf((frameToTip.M.Inverse(jnt.JointAxis())));
        break;
        default:
            std::cerr << "[WARN] Converting unknown joint type of joint " << jnt.getTypeName() << " into a fixed joint" << std::endl;
        case KDL::Joint::None:
            ret.type = urdf::Joint::FIXED;
    }
    return ret;
}
Esempio n. 8
0
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;
}
Esempio n. 9
0
    static inline YAML::Node extract(const std::string& start_link, const std::string& end_link, const KDL::Chain& chain)
    {
        std::string var_suffix = "_var";
        std::string frame_suffix = "_frame";
        std::string expression_name = "fk";

        std::vector<YAML::Node> input_vars;
        std::vector<YAML::Node> joint_frames;
        YAML::Node frame_mul;
        YAML::Node axis_angle_null;
        axis_angle_null["axis-angle"].push_back(get_vector3(1, 0, 0));
        axis_angle_null["axis-angle"].push_back(0);

        int input_var_index = 0;
        for (std::vector<KDL::Segment>::const_iterator it = chain.segments.begin(); it != chain.segments.end(); ++it)
        {
            KDL::Joint joint = it->getJoint();

            std::string var_name = joint.getName() + var_suffix;
            std::string frame_name = joint.getName() + frame_suffix;

            YAML::Node joint_frame;
            if (joint.getType() != KDL::Joint::None)
            {
                // Set input variable definition
                YAML::Node input_var;
                input_var[joint.getName() + var_suffix]["input-var"] = input_var_index;
                input_var_index++;
                input_vars.push_back(input_var);

                // Set joint transform
                YAML::Node translation;
                YAML::Node rotation;

                // Set joint axis
                YAML::Node joint_axis = get_vector3(joint.JointAxis());
                if (joint.getType() == KDL::Joint::TransAxis)
                {
                    YAML::Node scale_vec;
                    scale_vec["scale-vector"].push_back(var_name);
                    scale_vec["scale-vector"].push_back(joint_axis);
                    translation["vector-add"].push_back(scale_vec);
                    translation["vector-add"].push_back(get_vector3(joint.JointOrigin()));
                    rotation = axis_angle_null;
                }
                else if (joint.getType() == KDL::Joint::RotAxis)
                {
                    translation = get_vector3(joint.JointOrigin());
                    rotation["axis-angle"].push_back(joint_axis);
                    rotation["axis-angle"].push_back(var_name);
                }

                // Create frame and add to list
                YAML::Node joint_transform;
                joint_transform["frame"].push_back(rotation);
                joint_transform["frame"].push_back(translation);
                joint_frame[frame_name]["frame-mul"].push_back(joint_transform);
            }

            std::vector<YAML::Node> f_tip_nodes = get_frame_tip_nodes(*it);
            for (std::vector<YAML::Node>::iterator f_tip_it = f_tip_nodes.begin(); f_tip_it != f_tip_nodes.end(); ++f_tip_it)
            {
                joint_frame[frame_name]["frame-mul"].push_back(*f_tip_it);
            }

            if (joint_frame.size() > 0)
            {
                joint_frames.push_back(joint_frame);
                frame_mul.push_back(frame_name);
            }
        }

        // Merge nodes
        YAML::Node node;
        for (std::vector<YAML::Node>::iterator it = input_vars.begin(); it != input_vars.end(); ++it)
        {
            node.push_back(*it);
        }
        for (std::vector<YAML::Node>::iterator it = joint_frames.begin(); it != joint_frames.end(); ++it)
        {
            node.push_back(*it);
        }
        YAML::Node fk_def;
        fk_def[expression_name]["frame-mul"] = frame_mul;
        node.push_back(fk_def);

        return node;
    }
Esempio n. 10
0
void TransformationCalculator::load_robot_model(std::string filepath, bool init_invalid){
    clear_all();

    bool success=false;
    success = kdl_parser::treeFromFile(filepath, kdl_tree_);
    if(!success){
        is_initialized_ = false;
        throw("Could not load Model file");
    }

    //Extract names of all joints defined in urdf file.
    //These are the names that are expected to be referred to in update function.
    KDL::SegmentMap map = kdl_tree_.getSegments();
    std::string root_name = kdl_tree_.getRootSegment()->first;

    KDL::Joint joint;
    KDL::Segment segment;
    for(KDL::SegmentMap::const_iterator it = map.begin(); it!=map.end(); ++it){
#ifdef KDL_USE_NEW_TREE_INTERFACE
        segment = it->second->segment;
#else
        segment = it->second.segment;
#endif
        joint = segment.getJoint();
        std::string j_name = joint.getName();

        if(j_name == "NoName" || j_name == ""){
            if(segment.getName() != root_name){
                LOG_ERROR("Segment %s has an unnamed joint. This is not okay, but will skip it for now.", segment.getName().c_str());
            }
            LOG_DEBUG("Skipping NonName joint of root segment");
            continue;
        }

        if(is_fixed(joint)){
            if(std::find(static_joint_names_.begin(), static_joint_names_.end(), j_name) == static_joint_names_.end()){
                static_segment_names_.push_back(segment.getName());
                static_joint_names_.push_back(j_name);
                joint_name2seg_name_[j_name] = segment.getName();
                all_joint_names_.push_back(j_name);
            }
        }
        if(is_valid_joint(joint)){
            if(std::find(moving_joint_names_.begin(), moving_joint_names_.end(), j_name) == moving_joint_names_.end()){
                moving_joint_names_.push_back(j_name);
                joint_name2seg_name_[j_name] = segment.getName();
                all_joint_names_.push_back(j_name);
            }
        }

    }
    //link_names_ = extract_keys(map);

    //Populate transforms map with identity transforms
    for(std::vector<std::string>::iterator it = moving_joint_names_.begin();
        it != moving_joint_names_.end(); ++it)
    {
        std::string j_name = *it;
        std::string seg_name = joint_name2seg_name_[j_name];
        moving_joints_transforms_[j_name] = base::samples::RigidBodyState(true);
        if(!init_invalid){
            //initUnknow sets trasform to identity. If we donÄt want invalidating, set to identity.
            moving_joints_transforms_[j_name].initUnknown();
        }
        moving_joints_transforms_[j_name].sourceFrame = seg_name;
#ifdef KDL_USE_NEW_TREE_INTERFACE
        moving_joints_transforms_[j_name].targetFrame = get_tree_element(seg_name).parent->second->segment.getName();
#else
        moving_joints_transforms_[j_name].targetFrame = get_tree_element(seg_name).parent->second.segment.getName();
#endif
    }

    //Populate static transforms vector
    for(uint i=0; i<static_joint_names_.size(); i++){
        std::string seg_name = static_segment_names_[i];
        KDL::TreeElement tree_elem = get_tree_element(seg_name);
        KDL::Frame kdl_transform = tree_elem.segment.pose(0);
        std::string j_name = static_joint_names_[i];
        base::samples::RigidBodyState rbs;
        convert(kdl_transform, rbs);
        //Skip root
        if(tree_elem.segment.getName() == kdl_tree_.getRootSegment()->first){
            continue;
        }
#ifdef KDL_USE_NEW_TREE_INTERFACE
        rbs.sourceFrame = tree_elem.parent->second->segment.getName();
#else
        rbs.sourceFrame = tree_elem.parent->second.segment.getName();
#endif
        rbs.targetFrame = tree_elem.segment.getName();
        static_joints_transforms_[j_name] = rbs;
    }


    is_initialized_ = true;
}
Esempio n. 11
0
bool TransformationCalculator::is_valid_joint_type(const KDL::Joint& joint){
    if(joint.getType() == KDL::Joint::None)
        return false;
    return true;
}