constraint_app
/home/jbrooksh/drc/software/perception/constraint_app/src/matlab_mex/Affordance.cpp
Go to the documentation of this file.
00001 #include "Affordance.h"
00002 #include <otdf_lcm_utils/otdf_lcm_utils.h>
00003 #include "ConstraintApp.h"
00004 #include <kdl/treefksolverpos_recursive.hpp>
00005 #include <kdl/treejnttojacsolver.hpp>
00006 #include "MyTreeJntToJacSolver.hpp"
00007 
00008 Affordance::Affordance(const std::string& filename, std::ostream& log /*= std::cout*/) : m_log(log) 
00009 {
00010   // load the OTDF file
00011   std::string xml_string;
00012   if ( !otdf::get_xml_string_from_file(filename, xml_string) ) {
00013     m_log << "unable to get_xml_string_from_file " << filename << std::endl;
00014     return;
00015   }
00016 
00017   //convert OTDF to URDF
00018   boost::shared_ptr<otdf::ModelInterface> otdf_instance(otdf::parseOTDF(xml_string));
00019   otdf_instance->update();
00020 
00021   std::string urdf_xml_string(otdf::convertObjectInstanceToCompliantURDFstring(otdf_instance));
00022 
00023   // Parse KDL tree
00024   KDL::Tree deviceTree;
00025   if ( !kdl_parser::treeFromString(urdf_xml_string, deviceTree) ) {
00026     m_log << "ERROR: Failed to extract kdl tree from " << filename 
00027           << " otdf object instance "<< std::endl; 
00028     return;
00029   }
00030 
00031   /*
00032   m_log << "OTDF deviceTree = " << std::endl;
00033   PrintKdlTree(deviceTree, deviceTree.getRootSegment()->second, 0);
00034 
00035   KDL::Tree tempTree;
00036   noisyTreeCopy(tempTree, deviceTree.getRootSegment(), "root");
00037   m_log << "tempTree = " << std::endl;
00038   PrintKdlTree(tempTree, tempTree.getRootSegment()->second, 0);
00039   m_log << "---------------------------------------" << std::endl;
00040   */
00041 
00042   //add a base joint
00043   m_tree.addSegment(KDL::Segment("segment_x", 
00044                                  KDL::Joint("x", KDL::Joint::TransX), 
00045                                  KDL::Frame(KDL::Vector(0.0,0.0,0.0))),  
00046                     "root");
00047   m_tree.addSegment(KDL::Segment("segment_y", 
00048                                  KDL::Joint("y", KDL::Joint::TransY), 
00049                                  KDL::Frame(KDL::Vector(0.0,0.0,0.0))),
00050                     "segment_x");
00051   m_tree.addSegment(KDL::Segment("segment_z", 
00052                                  KDL::Joint("z", KDL::Joint::TransZ), 
00053                                  KDL::Frame(KDL::Vector(0.0,0.0,0.0))),
00054                     "segment_y");
00055   m_tree.addSegment(KDL::Segment("segment_yaw",   
00056                                  KDL::Joint("yaw", KDL::Joint::RotZ), 
00057                                  KDL::Frame(KDL::Vector(0.0,0.0,0.0))), 
00058                     "segment_z");
00059   m_tree.addSegment(KDL::Segment("segment_pitch",   
00060                                  KDL::Joint("pitch", KDL::Joint::RotY), 
00061                                  KDL::Frame(KDL::Vector(0.0,0.0,0.0))), 
00062                     "segment_yaw");
00063   m_tree.addSegment(KDL::Segment("segment_roll", 
00064                                  KDL::Joint("roll", KDL::Joint::RotX), 
00065                                  KDL::Frame(KDL::Vector(0.0,0.0,0.0))),
00066                     "segment_pitch");
00067   m_tree.addTree(deviceTree, "segment_roll");
00068   
00069   UpdateJointNames();
00070 
00071   //print some debug info on the new affordance
00072   m_log << "Joint names and numbers: " << std::endl;
00073   PrintJointNames();
00074   
00075   m_log << "created KDL object with " << m_tree.getNrOfJoints() << " joints and " 
00076         << m_tree.getNrOfSegments() << " segments" << std::endl;
00077   PrintKdlTree(m_tree, m_tree.getRootSegment()->second, 0);
00078 
00079   m_originalTree = m_tree; //backup copy, handy if adding noise
00080 }
00081 
00082 /*
00083 Affordance::Affordance(std::ostream& log, const drc::affordance_t& msg) : m_log(log)
00084 {
00085   m_log << "ERROR: you might need to alter this function to add the world-to-base transform, unless the OTDF's already contain such a joint" << std::endl;
00086   assert(false);
00087   #if 0
00088   std::string urdf_xml_string;
00089   if ( !otdf::AffordanceLcmMsgToUrdfString(msg, urdf_xml_string) ) {
00090     m_log << "ERROR: failed to process new affordance" << std::endl;
00091     return;
00092   }
00093   #endif
00094 
00095   const std::string filename(msg.otdf_type);
00096   
00097   // load the OTDF file
00098   std::string xml_string;
00099   if ( !otdf::get_xml_string_from_file(filename, xml_string) ) {
00100     m_log << "unable to get_xml_string_from_file " << filename << std::endl;
00101     return;
00102   }
00103 
00104   //convert OTDF to URDF
00105   boost::shared_ptr<otdf::ModelInterface> otdf_instance(otdf::parseOTDF(xml_string));
00106   
00107   for(int i=0;i<msg.nparams;i++){
00108     otdf_instance->setParam(msg.param_names[i],msg.params[i]); 
00109     m_log << msg.param_names[i] << "<=" << msg.params[i] << std::endl;
00110   }
00111   for(int i=0;i<msg.nstates;i++){
00112     double pos, vel;
00113     pos = msg.states[i];
00114     vel =0;
00115     otdf_instance->setJointState(msg.state_names[i],pos,vel); 
00116     m_log << msg.state_names[i] << "<=" << pos << std::endl;
00117   }
00118   otdf_instance->update();
00119 
00120   std::string urdf_xml_string(otdf::convertObjectInstanceToCompliantURDFstring(otdf_instance));
00121 
00122   // Parse KDL tree
00123   if ( !kdl_parser::treeFromString(urdf_xml_string, m_tree) ) {
00124     m_log << "ERROR: Failed to extract kdl tree from " << msg.otdf_type << ", uid=" << msg.uid
00125           << " otdf object instance "<< std::endl; 
00126     return;
00127   }
00128 
00129   UpdateJointNames();
00130 
00131   //print some debug info on the new affordance
00132   m_log << "xml string: " << std::endl 
00133         << urdf_xml_string << std::endl;
00134 
00135   m_log << "Joint names and numbers: " << std::endl;
00136   PrintJointNames();
00137   
00138   m_log << "created KDL object with " << m_tree.getNrOfJoints() << " joints and " 
00139         << m_tree.getNrOfSegments() << " segments" << std::endl;
00140   PrintKdlTree(m_tree, m_tree.getRootSegment()->second, 0);
00141 
00142   m_originalTree = m_tree; //backup copy, handy if adding noise
00143 }
00144 */
00145 
00146 bool Affordance::noisyTreeCopy(const SegmentNoiseMap& noiseMap,
00147                                KDL::Tree& newTree, 
00148                                KDL::SegmentMap::const_iterator root, 
00149                                const std::string& hook_name)
00150 {
00151   KDL::SegmentMap::const_iterator child;
00152   for (unsigned int i = 0; i < root->second.children.size(); i++) {
00153     child = root->second.children[i];
00154     //Try to add the child
00155     m_log << "adding " << child->first << " to " << hook_name << std::endl;
00156 
00157     SegmentNoiseMap::const_iterator noiseIter = noiseMap.find(child->first);
00158 
00159     KDL::Segment originalSegment = child->second.segment;
00160     KDL::Segment newSegment;
00161     if ( noiseIter == noiseMap.end() ) {
00162       newSegment = originalSegment;
00163     } else {
00164       newSegment = KDL::Segment(originalSegment.getName(),
00165                                 originalSegment.getJoint(),
00166                                 KDL::addDelta(originalSegment.getFrameToTip(), 
00167                                               noiseIter->second),
00168                                 originalSegment.getInertia());
00169       m_log << "adding noise to segment " << originalSegment.getName() << std::endl
00170             << "original tip frame: " << originalSegment.getFrameToTip() << std::endl
00171             << "new tip frame: " << newSegment.getFrameToTip() << std::endl
00172             << "noise twist: " << noiseIter->second << std::endl;
00173     }
00174 
00175     if (newTree.addSegment(newSegment, hook_name)) {
00176       //if child is added, add all the child's children
00177       if (!(noisyTreeCopy(noiseMap, newTree, child, child->first)))
00178         //if it didn't work, return false
00179         return false;
00180     } else
00181       //If the child could not be added, return false
00182       return false;
00183   }
00184   return true;
00185 }
00186 
00187 void Affordance::PrintKdlTree(const KDL::Tree& tree, 
00188                               const KDL::TreeElement& segment,
00189                               unsigned int depth) 
00190 {
00191   for ( unsigned int i = 0; i < depth; i++ ) {
00192     m_log << " ";
00193   }
00194   KDL::Frame frameToTip(segment.segment.getFrameToTip());
00195   
00196   m_log << segment.segment.getName() 
00197         << ", joint " << segment.segment.getJoint().getName() 
00198         << " of type " << segment.segment.getJoint().getTypeName()
00199         << ", joint #" << segment.q_nr
00200         << ", tip = " << frameToTip.p
00201         << std::endl;
00202   for ( unsigned int i = 0; i < segment.children.size(); i++ ) {
00203     PrintKdlTree(tree, segment.children[i]->second, depth+1);
00204   }
00205 }
00206 
00207 void Affordance::PrintJointNames()
00208 {
00209   for ( JointMultiIndex::nth_index<0>::type::iterator iter = m_joints.begin();
00210         iter != m_joints.end(); ++iter ) {
00211     m_log << iter->name << " #" << iter->num << std::endl;
00212   }
00213 }
00214 
00215 void Affordance::UpdateJointNames(const KDL::TreeElement* element /*=NULL*/)
00216 {
00217   if ( !element ) {
00218     m_joints.clear();
00219     UpdateJointNames(&m_tree.getRootSegment()->second);
00220     
00221     //perform sanity checks; loop through the joints and verify that the q_nr's are sequential and start at zero
00222     int q_nr = 0;
00223     for ( JointMultiIndex::nth_index<0>::type::iterator iter = m_joints.begin();
00224         iter != m_joints.end(); ++iter ) {
00225       if ( q_nr != iter->num ) {
00226         m_log << "ERROR: while parsing tree, expected joint #" << q_nr << ", found joint #" << iter->num << std::endl;
00227         //m_joints.clear();
00228         //m_tree = KDL::Tree();
00229         return;
00230       }
00231       q_nr++;
00232     }
00233     return;
00234   }
00235 
00236   if ( element->segment.getJoint().getType() != KDL::Joint::None ) {
00237     m_joints.insert(Joint(element->segment.getJoint().getName(), element->q_nr));
00238   }
00239 
00240   for ( unsigned int i = 0; i < element->children.size(); i++ ) {
00241     UpdateJointNames(&element->children[i]->second);
00242   }
00243 
00244 }
00245 
00246 /*
00247 void Affordance::GetState(std::vector<double>& state) const
00248 {
00249   state = ConstraintApp::FrameToVector(m_basepose);
00250 
00251   for ( JointMultiIndex::nth_index<0>::type::const_iterator iter = m_joints.begin();
00252         iter != m_joints.end(); ++iter ) {
00253     state.push_back(iter->value);
00254   }
00255 }
00256 */
00257 
00258 bool Affordance::GetStateFromMsg(const drc::affordance_t& msg, StateVector& state) 
00259 {
00260   // move all joints into the map
00261   typedef std::map<std::string, double> ValueMap;
00262   ValueMap values;
00263   for ( int i=0; i<msg.nstates; i++ ) {
00264     values.insert(std::pair<std::string, double>(msg.state_names[i], msg.states[i]));
00265   }
00266   values.insert(std::pair<std::string, double>("x", msg.origin_xyz[0]));
00267   values.insert(std::pair<std::string, double>("y", msg.origin_xyz[1]));
00268   values.insert(std::pair<std::string, double>("z", msg.origin_xyz[2]));
00269   values.insert(std::pair<std::string, double>("roll",  msg.origin_rpy[0]));
00270   values.insert(std::pair<std::string, double>("pitch", msg.origin_rpy[1]));
00271   values.insert(std::pair<std::string, double>("yaw",   msg.origin_rpy[2]));
00272 
00273   m_log << "Affordance::GetStateFromMsg" << std::endl;
00274   state.clear();
00275   state.reserve(m_joints.size());
00276 
00277   //iterate through the joints according to their q_nr number
00278   for ( JointMultiIndex::nth_index<0>::type::iterator iter = m_joints.begin();
00279         iter != m_joints.end(); ++iter ) {
00280     ValueMap::iterator match = values.find(iter->name);
00281     if ( match == values.end() ) {
00282       m_log << "ERROR: unable to find joint " << iter->name 
00283             << " in the message state" << std::endl;
00284       return false;
00285     }
00286     m_log << "  found joint " << iter->num << ": " << iter->name << std::endl;
00287     state.push_back(match->second);
00288   }
00289 
00290   //perform sanity checks
00291   if ( state.size() != m_joints.size() ) {
00292     m_log << "ERROR: state has improper size" << std::endl;
00293     return false;
00294   }
00295 
00296   double temp = state[3];
00297   state[3] = state[5];
00298   state[5] = temp;
00299 
00300   return true;
00301 }
00302 
00303 void Affordance::PrintState(const StateVector& state)
00304 {
00305   for ( int i = 0; i < state.size(); i++ ) {
00306     m_log << state[i] << ", ";
00307   }
00308   m_log << std::endl;
00309 }
00310 
00311 bool Affordance::DecodeState(const StateVector& state, KDL::Frame& root_expressedIn_world, KDL::JntArray& joints)
00312 {
00313   const int basePoseSize = 6;
00314   if ( state.size() < basePoseSize ) {
00315     m_log << "ERROR: Affordance::DecodeState must be of at least size 6 (not " << state.size() << ")" << std::endl;
00316     m_log << "   state=";
00317     for ( int i = 0; i < state.size(); i++ ) m_log << state[i] << ", ";
00318     m_log << std::endl;
00319     return false;
00320   }
00321 
00322   root_expressedIn_world = GetFrameFromVector(state);
00323 
00324   int numJoints = state.size() - basePoseSize;
00325   if ( numJoints < 0 ) return false;
00326   joints.resize(numJoints);
00327   for ( int i = 0; i < numJoints; i++ ) {
00328     joints(i) = state[i+basePoseSize];
00329   }
00330 
00331   return true;
00332 }
00333 
00334 void Affordance::DecodeState(const StateVector& state, KDL::JntArray& joints)
00335 {
00336   joints.resize(state.size());
00337   for ( int i = 0; i < state.size(); i++ ) {
00338     joints(i) = state[i];
00339   }
00340   //state has order rpw, but joints has order wpr
00341   joints(3) = state[5];
00342   joints(4) = state[4];
00343   joints(5) = state[3];
00344 }
00345 
00346 bool Affordance::GetSegmentExpressedInWorld(const std::string& segmentName, 
00347                                             const StateVector& state, 
00348                                             KDL::Frame& segment_expressedIn_world)
00349 {
00350   KDL::JntArray joints;
00351   DecodeState(state, joints);
00352 
00353   KDL::TreeFkSolverPos_recursive fk(m_tree);
00354   int fkret = fk.JntToCart(joints, segment_expressedIn_world, segmentName);
00355   if ( fkret < 0 ) {
00356     m_log << "ERROR: Affordance::GetSegmentExpressedInWorld: fk returned " << fkret << std::endl
00357           << "   segmentName=" << segmentName << std::endl
00358           << "   joints=" << joints.data << std::endl;
00359     m_log << "   state=";
00360     for ( int i = 0; i < state.size(); i++ ) m_log << state[i] << ", ";
00361     m_log << std::endl;
00362     return false;
00363   }
00364 
00365   return true;
00366 }
00367 
00368 bool Affordance::GetSegmentJacobianExpressedInWorld(const std::string& segmentName, 
00369                                                     const StateVector& state, 
00370                                                     KDL::Jacobian& jacobian)
00371 {
00372   KDL::JntArray joints;
00373   DecodeState(state, joints);
00374 
00375   jacobian.resize(state.size());
00376   KDL::TreeJntToJacSolver jntjac(m_tree);
00377   int result = jntjac.JntToJac(joints, jacobian, segmentName);
00378   if ( result < 0 ) {
00379     m_log << "ERROR: Affordance::GetSegmentJacobianExpressedInWorld: JntToJac returned " 
00380           << result << std::endl
00381           << "   segmentName=" << segmentName << std::endl
00382           << "   joints=" << joints.data << std::endl;
00383     m_log << "   state=";
00384     for ( int i = 0; i < state.size(); i++ ) m_log << state[i] << ", ";
00385     m_log << std::endl;
00386     return false;
00387   }
00388 
00389   //swap the wpr back to rpw
00390   Eigen::MatrixXd temp(jacobian.data.col(3));
00391   jacobian.data.col(3) = jacobian.data.col(5);
00392   jacobian.data.col(5) = temp;
00393 
00394   return true;
00395 }
00396 
00397 bool Affordance::GetSegmentJacobianExpressedInWorld2(const std::string& segmentName, 
00398                                                      const StateVector& state, 
00399                                                      KDL::Jacobian& jacobian)
00400 {
00401   KDL::JntArray joints;
00402   DecodeState(state, joints);
00403 
00404   jacobian.resize(state.size());
00405   MyTreeJntToJacSolver jntjac(m_tree);
00406   int result = jntjac.JntToJac(joints, jacobian, segmentName);
00407   if ( result < 0 ) {
00408     m_log << "ERROR: Affordance::GetSegmentJacobianExpressedInWorld: JntToJac returned " 
00409           << result << std::endl
00410           << "   segmentName=" << segmentName << std::endl
00411           << "   joints=" << joints.data << std::endl;
00412     m_log << "   state=";
00413     for ( int i = 0; i < state.size(); i++ ) m_log << state[i] << ", ";
00414     m_log << std::endl;
00415     return false;
00416   }
00417 
00418   //swap the wpr back to rpw
00419   Eigen::MatrixXd temp(jacobian.data.col(3));
00420   jacobian.data.col(3) = jacobian.data.col(5);
00421   jacobian.data.col(5) = temp;
00422 
00423   return true;
00424 }
00425 
00426 KDL::Frame Affordance::GetFrameFromVector(const StateVector& state) 
00427 {
00428   KDL::Frame ret;
00429   ret.p[0] = state[0];
00430   ret.p[1] = state[1];
00431   ret.p[2] = state[2];
00432   ret.M = KDL::Rotation::RPY(state[3], state[4], state[5]);
00433   return ret;
00434 }
00435 
00436 void Affordance::GetJointNames(std::vector<std::string>& names)
00437 {
00438   names.clear();
00439   for ( JointMultiIndex::nth_index<0>::type::iterator iter = m_joints.begin();
00440         iter != m_joints.end(); ++iter ) {
00441     names.push_back(iter->name);
00442   }
00443 }
All Classes Files Functions Variables Typedefs