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; }
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; }
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; }
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; }
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; }
/** * * @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; }
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; }
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; }
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; }
bool TransformationCalculator::is_valid_joint_type(const KDL::Joint& joint){ if(joint.getType() == KDL::Joint::None) return false; return true; }