$search
00001 /****************************************************************************** 00002 * This file is part of the KDL project * 00003 * * 00004 * (C) 2010 Ruben Smits * 00005 * 2010 Steven Bellens * 00006 * ruben.smits@mech.kuleuven.be * 00007 * steven.bellens@mech.kuleuven.be * 00008 * Department of Mechanical Engineering, * 00009 * Katholieke Universiteit Leuven, Belgium. * 00010 * * 00011 * You may redistribute this software and/or modify it under either the * 00012 * terms of the GNU Lesser General Public License version 2.1 (LGPLv2.1 * 00013 * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>) or (at your * 00014 * discretion) of the Modified BSD License: * 00015 * Redistribution and use in source and binary forms, with or without * 00016 * modification, are permitted provided that the following conditions * 00017 * are met: * 00018 * 1. Redistributions of source code must retain the above copyright * 00019 * notice, this list of conditions and the following disclaimer. * 00020 * 2. Redistributions in binary form must reproduce the above copyright * 00021 * notice, this list of conditions and the following disclaimer in the * 00022 * documentation and/or other materials provided with the distribution. * 00023 * 3. The name of the author may not be used to endorse or promote * 00024 * products derived from this software without specific prior written * 00025 * permission. * 00026 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR * 00027 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * 00028 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 00029 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,* 00030 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 00031 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * 00032 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * 00033 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, * 00034 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING * 00035 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * 00036 * POSSIBILITY OF SUCH DAMAGE. * 00037 * * 00038 *******************************************************************************/ 00039 00040 #include "kinfamproperties.hpp" 00041 #include "motionproperties.hpp" 00042 #include <boost/lexical_cast.hpp> 00043 #include <string> 00044 00045 namespace RTT 00046 { 00047 void decomposeProperty(const Joint& joint, PropertyBag& targetbag) 00048 { 00049 targetbag.setType("KDL.Joint"); 00050 targetbag.add( new Property<std::string>("Type", "Type of Joint",joint.getTypeName())); 00051 } 00052 00053 bool composeProperty(const PropertyBag& bag, Joint& joint) 00054 { 00055 if ( bag.getType() == "KDL.Joint" ){ // check the type 00056 Property<std::string> type_name = bag.getPropertyType<std::string>("Type"); 00057 if ( !type_name.ready() ) 00058 return false; 00059 if (type_name.value()=="RotX"){ 00060 joint=Joint(Joint::RotX); 00061 return true; 00062 }else if(type_name.value()=="RotY"){ 00063 joint=Joint(Joint::RotY); 00064 return true; 00065 }else if(type_name.value()=="RotZ"){ 00066 joint=Joint(Joint::RotZ); 00067 return true; 00068 }else if(type_name.value()=="TransX"){ 00069 joint=Joint(Joint::TransX); 00070 return true; 00071 }else if(type_name.value()=="TransY"){ 00072 joint=Joint(Joint::TransY); 00073 return true; 00074 }else if(type_name.value()=="TransZ"){ 00075 joint=Joint(Joint::TransZ); 00076 return true; 00077 }else if(type_name.value()=="None"){ 00078 joint=Joint(Joint::None); 00079 return true; 00080 } 00081 else 00082 return false; 00083 } 00084 return false; 00085 } 00086 00087 void decomposeProperty(const Segment& segment, PropertyBag& targetbag) 00088 { 00089 PropertyBag frame_bag; 00090 PropertyBag joint_bag; 00091 targetbag.setType("KDL.Segment"); 00092 decomposeProperty(segment.getJoint(), joint_bag); 00093 targetbag.add(new Property<PropertyBag>("joint","The joint at the base of this segment",joint_bag)); 00094 decomposeProperty(segment.getFrameToTip(), frame_bag); 00095 targetbag.add(new Property<PropertyBag>("frame","The offset frame from the joint at the base to the end of the link",frame_bag)); 00096 } 00097 00098 bool composeProperty(const PropertyBag& bag, Segment& segment) 00099 { 00100 if ( bag.getType() == "KDL.Segment" ){ // check the type 00101 Property<PropertyBag> joint_bag = bag.getPropertyType<PropertyBag>("joint"); 00102 Property<PropertyBag> frame_bag = bag.getPropertyType<PropertyBag>("frame"); 00103 if(!(frame_bag.ready()&&joint_bag.ready())) 00104 return false; 00105 Property<Joint> joint_prop(joint_bag.getName(),joint_bag.getDescription()); 00106 joint_prop.getTypeInfo()->composeType(joint_bag.getDataSource(), 00107 joint_prop.getDataSource()); 00108 Property<Frame> frame_prop(frame_bag.getName(),frame_bag.getDescription()); 00109 frame_prop.getTypeInfo()->composeType(frame_bag.getDataSource(), 00110 frame_prop.getDataSource()); 00111 segment=Segment(joint_prop.value(),frame_prop.value()); 00112 return true; 00113 } 00114 else 00115 return false; 00116 } 00117 00118 void decomposeProperty(const Chain& chain, PropertyBag& targetbag) 00119 { 00120 targetbag.setType("KDL.Chain"); 00121 PropertyBag segment_bag; 00122 for(unsigned int i=0;i<chain.getNrOfSegments();i++){ 00123 decomposeProperty(chain.getSegment(i),segment_bag); 00124 targetbag.add(new Property<PropertyBag>("Segment","Segment of the chain",segment_bag)); 00125 } 00126 } 00127 00128 bool composeProperty(const PropertyBag& bag, Chain& chain) 00129 { 00130 Chain chain_new; 00131 if( bag.getType() =="KDL.Chain"){ 00132 for(unsigned int i=0;i<bag.size();i++){ 00133 RTT::base::PropertyBase* segment_bag = bag.getItem(i); 00134 if(!segment_bag->ready()) 00135 return false; 00136 Property<Segment> segment_prop(segment_bag->getName(), 00137 segment_bag->getDescription()); 00138 segment_prop.getTypeInfo()->composeType(segment_bag->getDataSource(), 00139 segment_prop.getDataSource()); 00140 chain_new.addSegment(segment_prop.value()); 00141 } 00142 chain = chain_new; 00143 return true; 00144 }else 00145 return false; 00146 } 00147 00148 void decomposeProperty(const JntArray& jntarray, PropertyBag& targetbag) 00149 { 00150 targetbag.setType("KDL.JntArray"); 00151 for(int i = 0; i < jntarray.data.rows(); i++) 00152 { 00153 std::string rindx = boost::lexical_cast<std::string>( i ); 00154 targetbag.add(new Property<double>("Element" + rindx,"JntArray element",jntarray.data(i))); 00155 } 00156 } 00157 00158 bool composeProperty(const PropertyBag& bag, JntArray& jntarray) 00159 { 00160 JntArray jntarray_new(bag.size()); 00161 if( bag.getType() =="KDL.JntArray"){ 00162 for(unsigned int i = 0; i < bag.size(); i++){ 00163 RTT::base::PropertyBase* jntarray_bag = bag.getItem(i); 00164 if(!jntarray_bag->ready()) 00165 return false; 00166 Property<double> jntarray_prop(jntarray_bag->getName(), jntarray_bag->getDescription()); 00167 jntarray_prop.getTypeInfo()->composeType(jntarray_bag->getDataSource(), jntarray_prop.getDataSource()); 00168 jntarray_new.data(i) = jntarray_prop.value(); 00169 } 00170 jntarray = jntarray_new; 00171 return true; 00172 } else 00173 return false; 00174 } 00175 00176 void decomposeProperty(const Jacobian& jacobian, PropertyBag& targetbag) 00177 { 00178 targetbag.setType("KDL.Jacobian"); 00179 for(int i = 0; i < jacobian.data.rows(); i++){ 00180 for(int j = 0; j < jacobian.data.cols(); j++){ 00181 std::string rindx = boost::lexical_cast<std::string>( i ); 00182 std::string cindx = boost::lexical_cast<std::string>( j ); 00183 targetbag.add(new Property<double>("Element (" + rindx + "," + cindx + ")","Jacobian element",jacobian.data(i,j))); 00184 } 00185 } 00186 } 00187 00188 bool composeProperty(const PropertyBag& bag, Jacobian& jacobian) 00189 { 00190 Jacobian jacobian_new((bag.size() / 6)); 00191 00192 if( bag.getType() =="KDL.Jacobian"){ 00193 for(unsigned int i = 0; i < 6; i++){ 00194 for(unsigned int j = 0; j < bag.size() / 6; j++){ 00195 RTT::base::PropertyBase* data_bag = bag.getItem((bag.size() / 6) * i + j); 00196 if(!data_bag->ready()) 00197 return false; 00198 Property<double> data_prop(data_bag->getName(), data_bag->getDescription()); 00199 data_prop.getTypeInfo()->composeType(data_bag->getDataSource(), data_prop.getDataSource()); 00200 jacobian_new.data(i,j) = data_prop.value(); 00201 } 00202 } 00203 jacobian = jacobian_new; 00204 return true; 00205 } 00206 else 00207 return false; 00208 } 00209 }