Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #include "kinfamproperties.hpp"
00041 #include "motionproperties.hpp"
00042
00043 #include <rtt/Property.hpp>
00044 #include <rtt/PropertyBag.hpp>
00045
00046 #include <boost/lexical_cast.hpp>
00047 #include <string>
00048
00049 namespace RTT
00050 {
00051 void decomposeProperty(const Joint& joint, PropertyBag& targetbag)
00052 {
00053 targetbag.setType("KDL.Joint");
00054 targetbag.add( new Property<std::string>("Type", "Type of Joint",joint.getTypeName()));
00055 }
00056
00057 bool composeProperty(const PropertyBag& bag, Joint& joint)
00058 {
00059 if ( bag.getType() == "KDL.Joint" ){
00060 Property<std::string> type_name = bag.getPropertyType<std::string>("Type");
00061 if ( !type_name.ready() )
00062 return false;
00063 if (type_name.value()=="RotX"){
00064 joint=Joint(Joint::RotX);
00065 return true;
00066 }else if(type_name.value()=="RotY"){
00067 joint=Joint(Joint::RotY);
00068 return true;
00069 }else if(type_name.value()=="RotZ"){
00070 joint=Joint(Joint::RotZ);
00071 return true;
00072 }else if(type_name.value()=="TransX"){
00073 joint=Joint(Joint::TransX);
00074 return true;
00075 }else if(type_name.value()=="TransY"){
00076 joint=Joint(Joint::TransY);
00077 return true;
00078 }else if(type_name.value()=="TransZ"){
00079 joint=Joint(Joint::TransZ);
00080 return true;
00081 }else if(type_name.value()=="None"){
00082 joint=Joint(Joint::None);
00083 return true;
00084 }
00085 else
00086 return false;
00087 }
00088 return false;
00089 }
00090
00091 void decomposeProperty(const Segment& segment, PropertyBag& targetbag)
00092 {
00093 PropertyBag frame_bag;
00094 PropertyBag joint_bag;
00095 targetbag.setType("KDL.Segment");
00096 decomposeProperty(segment.getJoint(), joint_bag);
00097 targetbag.add(new Property<PropertyBag>("joint","The joint at the base of this segment",joint_bag));
00098 decomposeProperty(segment.getFrameToTip(), frame_bag);
00099 targetbag.add(new Property<PropertyBag>("frame","The offset frame from the joint at the base to the end of the link",frame_bag));
00100 }
00101
00102 bool composeProperty(const PropertyBag& bag, Segment& segment)
00103 {
00104 if ( bag.getType() == "KDL.Segment" ){
00105 Property<PropertyBag> joint_bag = bag.getPropertyType<PropertyBag>("joint");
00106 Property<PropertyBag> frame_bag = bag.getPropertyType<PropertyBag>("frame");
00107 if(!(frame_bag.ready()&&joint_bag.ready()))
00108 return false;
00109 Property<Joint> joint_prop(joint_bag.getName(),joint_bag.getDescription());
00110 joint_prop.getTypeInfo()->composeType(joint_bag.getDataSource(),
00111 joint_prop.getDataSource());
00112 Property<Frame> frame_prop(frame_bag.getName(),frame_bag.getDescription());
00113 frame_prop.getTypeInfo()->composeType(frame_bag.getDataSource(),
00114 frame_prop.getDataSource());
00115 segment=Segment(joint_prop.value(),frame_prop.value());
00116 return true;
00117 }
00118 else
00119 return false;
00120 }
00121
00122 void decomposeProperty(const Chain& chain, PropertyBag& targetbag)
00123 {
00124 targetbag.setType("KDL.Chain");
00125 PropertyBag segment_bag;
00126 for(unsigned int i=0;i<chain.getNrOfSegments();i++){
00127 decomposeProperty(chain.getSegment(i),segment_bag);
00128 targetbag.add(new Property<PropertyBag>("Segment","Segment of the chain",segment_bag));
00129 }
00130 }
00131
00132 bool composeProperty(const PropertyBag& bag, Chain& chain)
00133 {
00134 Chain chain_new;
00135 if( bag.getType() =="KDL.Chain"){
00136 for(unsigned int i=0;i<bag.size();i++){
00137 RTT::base::PropertyBase* segment_bag = bag.getItem(i);
00138 if(!segment_bag->ready())
00139 return false;
00140 Property<Segment> segment_prop(segment_bag->getName(),
00141 segment_bag->getDescription());
00142 segment_prop.getTypeInfo()->composeType(segment_bag->getDataSource(),
00143 segment_prop.getDataSource());
00144 chain_new.addSegment(segment_prop.value());
00145 }
00146 chain = chain_new;
00147 return true;
00148 }else
00149 return false;
00150 }
00151
00152 void decomposeProperty(const JntArray& jntarray, PropertyBag& targetbag)
00153 {
00154 targetbag.setType("KDL.JntArray");
00155 for(int i = 0; i < jntarray.data.rows(); i++)
00156 {
00157 std::string rindx = boost::lexical_cast<std::string>( i );
00158 targetbag.add(new Property<double>("Element" + rindx,"JntArray element",jntarray.data(i)));
00159 }
00160 }
00161
00162 bool composeProperty(const PropertyBag& bag, JntArray& jntarray)
00163 {
00164 JntArray jntarray_new(bag.size());
00165 if( bag.getType() =="KDL.JntArray"){
00166 for(unsigned int i = 0; i < bag.size(); i++){
00167 RTT::base::PropertyBase* jntarray_bag = bag.getItem(i);
00168 if(!jntarray_bag->ready())
00169 return false;
00170 Property<double> jntarray_prop(jntarray_bag->getName(), jntarray_bag->getDescription());
00171 jntarray_prop.getTypeInfo()->composeType(jntarray_bag->getDataSource(), jntarray_prop.getDataSource());
00172 jntarray_new.data(i) = jntarray_prop.value();
00173 }
00174 jntarray = jntarray_new;
00175 return true;
00176 } else
00177 return false;
00178 }
00179
00180 void decomposeProperty(const Jacobian& jacobian, PropertyBag& targetbag)
00181 {
00182 targetbag.setType("KDL.Jacobian");
00183 for(int i = 0; i < jacobian.data.rows(); i++){
00184 for(int j = 0; j < jacobian.data.cols(); j++){
00185 std::string rindx = boost::lexical_cast<std::string>( i );
00186 std::string cindx = boost::lexical_cast<std::string>( j );
00187 targetbag.add(new Property<double>("Element (" + rindx + "," + cindx + ")","Jacobian element",jacobian.data(i,j)));
00188 }
00189 }
00190 }
00191
00192 bool composeProperty(const PropertyBag& bag, Jacobian& jacobian)
00193 {
00194 Jacobian jacobian_new((bag.size() / 6));
00195
00196 if( bag.getType() =="KDL.Jacobian"){
00197 for(unsigned int i = 0; i < 6; i++){
00198 for(unsigned int j = 0; j < bag.size() / 6; j++){
00199 RTT::base::PropertyBase* data_bag = bag.getItem((bag.size() / 6) * i + j);
00200 if(!data_bag->ready())
00201 return false;
00202 Property<double> data_prop(data_bag->getName(), data_bag->getDescription());
00203 data_prop.getTypeInfo()->composeType(data_bag->getDataSource(), data_prop.getDataSource());
00204 jacobian_new.data(i,j) = data_prop.value();
00205 }
00206 }
00207 jacobian = jacobian_new;
00208 return true;
00209 }
00210 else
00211 return false;
00212 }
00213 }