00001 /* 00002 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 00018 #include <ros/ros.h> 00019 #include "cob_obstacle_distance/chainfk_solvers/advanced_chainfksolver_recursive.hpp" 00020 00021 AdvancedChainFkSolverPos_recursive::AdvancedChainFkSolverPos_recursive(const KDL::Chain& _chain): 00022 chain_(_chain) 00023 { 00024 } 00025 00031 int AdvancedChainFkSolverPos_recursive::JntToCart(const KDL::JntArray& q_in, KDL::Frame& p_out, int seg_nr) 00032 { 00033 unsigned int segmentNr; 00034 if (seg_nr < 0) 00035 { 00036 segmentNr = this->chain_.getNrOfSegments(); 00037 } 00038 else 00039 { 00040 segmentNr = seg_nr; 00041 } 00042 00043 p_out = KDL::Frame::Identity(); 00044 00045 if (q_in.rows() != this->chain_.getNrOfJoints()) 00046 { 00047 return -1; 00048 } 00049 else if (segmentNr > this->chain_.getNrOfSegments()) 00050 { 00051 return -1; 00052 } 00053 else 00054 { 00055 this->segment_pos_.clear(); 00056 int j = 0; 00057 for (unsigned int i = 0; i < segmentNr; i++) 00058 { 00059 if (this->chain_.getSegment(i).getJoint().getType() != KDL::Joint::None) 00060 { 00061 p_out = p_out * this->chain_.getSegment(i).pose(q_in(j)); 00062 j++; 00063 } 00064 else 00065 { 00066 p_out = p_out * this->chain_.getSegment(i).pose(0.0); 00067 } 00068 00069 this->segment_pos_.push_back(KDL::Frame(p_out)); // store copies not references 00070 } 00071 return 0; 00072 } 00073 } 00074 00078 KDL::Frame AdvancedChainFkSolverPos_recursive::getFrameAtSegment(uint16_t seg_idx) const 00079 { 00080 KDL::Frame p_out = KDL::Frame::Identity(); 00081 00082 if (seg_idx < this->chain_.getNrOfSegments()) 00083 { 00084 p_out = this->segment_pos_.at(seg_idx); 00085 } 00086 00087 return p_out; 00088 } 00089 00093 void AdvancedChainFkSolverPos_recursive::dumpAllSegmentPostures() const 00094 { 00095 uint16_t id = 0; 00096 ROS_INFO_STREAM("=== Dump all Jnt Postures ==="); 00097 for (FrameVector_t::const_iterator it = this->segment_pos_.begin(); it != this->segment_pos_.end(); ++it) 00098 { 00099 ROS_INFO_STREAM("Segment " << id++ << ". Position: " << std::endl << 00100 it->p.x() << std::endl << 00101 it->p.y() << std::endl << 00102 it->p.z()); 00103 ROS_INFO_STREAM("Rotation: " << std::endl << 00104 it->M.GetRot().x() << std::endl << 00105 it->M.GetRot().y() << std::endl << 00106 it->M.GetRot().z() << std::endl << 00107 "================================="); 00108 } 00109 } 00110 00111 00112 AdvancedChainFkSolverPos_recursive::~AdvancedChainFkSolverPos_recursive() 00113 { 00114 } 00115 00116 00117 AdvancedChainFkSolverVel_recursive::AdvancedChainFkSolverVel_recursive(const KDL::Chain& _chain): 00118 chain_(_chain) 00119 { 00120 } 00121 00127 int AdvancedChainFkSolverVel_recursive::JntToCart(const KDL::JntArrayVel& q_in, KDL::FrameVel& out, int seg_nr) 00128 { 00129 unsigned int segmentNr; 00130 if (seg_nr < 0) 00131 { 00132 segmentNr = this->chain_.getNrOfSegments(); 00133 } 00134 else 00135 { 00136 segmentNr = seg_nr; 00137 } 00138 00139 out = KDL::FrameVel::Identity(); 00140 00141 if (!(q_in.q.rows() == this->chain_.getNrOfJoints() && q_in.qdot.rows() == this->chain_.getNrOfJoints())) 00142 { 00143 ROS_ERROR("Rows do not match!"); 00144 return -1; 00145 } 00146 else if (segmentNr > this->chain_.getNrOfSegments()) 00147 { 00148 return -2; 00149 } 00150 else 00151 { 00152 this->segment_vel_.clear(); 00153 int j = 0; 00154 for (unsigned int i = 0; i < segmentNr; ++i) 00155 { 00156 // Calculate new Frame_base_ee 00157 if (this->chain_.getSegment(i).getJoint().getType() != KDL::Joint::None) 00158 { 00159 out = out * KDL::FrameVel(this->chain_.getSegment(i).pose(q_in.q(j)), 00160 this->chain_.getSegment(i).twist(q_in.q(j), q_in.qdot(j))); 00161 j++; // Only increase jointnr if the segment has a joint 00162 } 00163 else 00164 { 00165 out = out * KDL::FrameVel(this->chain_.getSegment(i).pose(0.0), 00166 this->chain_.getSegment(i).twist(0.0, 0.0)); 00167 } 00168 00169 this->segment_vel_.push_back(KDL::FrameVel(out)); 00170 } 00171 00172 return 0; 00173 } 00174 } 00175 00176 00177 KDL::FrameVel AdvancedChainFkSolverVel_recursive::getFrameVelAtSegment(uint16_t seg_idx) const 00178 { 00179 KDL::FrameVel vel_out = KDL::FrameVel::Identity(); 00180 00181 if (seg_idx < this->chain_.getNrOfSegments()) 00182 { 00183 vel_out = this->segment_vel_.at(seg_idx); 00184 } 00185 00186 return vel_out; 00187 } 00188 00189 00190 AdvancedChainFkSolverVel_recursive::~AdvancedChainFkSolverVel_recursive() 00191 { 00192 }