constraint_app
/home/jbrooksh/drc/software/perception/constraint_app/src/matlab_mex/ConstraintApp_MB.cpp
Go to the documentation of this file.
00001 #include "ConstraintApp_MB.h"
00002 #include <boost/format.hpp>
00003 #include "mytreefksolverpos_recursive.hpp"
00004 
00005 ConstraintApp_MB::ConstraintApp_MB() : ConstraintApp(), m_wasReset(true)
00006 {
00007   m_log << "ConstraintApp_MB::ConstraintApp_MB: built at " << __DATE__ << " " << __TIME__ << std::endl;
00008 }
00009 
00010 ConstraintApp_MB::~ConstraintApp_MB() 
00011 {
00012   m_log << "ConstraintApp_MB::~ConstraintApp_MB" << std::endl;
00013 }
00014 
00015 void ConstraintApp_MB::AffordanceTrackCollectionHandler(const drc::affordance_track_collection_t *msg)
00016 {
00017   boost::mutex::scoped_lock lock(m_dataMutex);
00018 
00019   /*
00020   if ( msg->uid != m_affordanceUID ) {
00021     m_log << "WARNING: received track collection for uid=" << msg->uid 
00022           << ", but expecting for uid=" << m_affordanceUID 
00023           << ".  ignoring tracks." << std::endl;
00024     return;
00025   }
00026   */
00027 
00028   m_log << "got a new track collection for affordance with uid=" << msg->uid << std::endl;
00029 
00030   KDL::Tree& tree(m_affordance->GetTree());
00031   const KDL::SegmentMap& segmentMap(tree.getSegments());
00032 
00033   for ( int i = 0; i < msg->ntracks; i++ ) {
00034     const drc::affordance_track_t* track = &msg->tracks[i];
00035     m_log << "  track: segment=" << track->segment << ", id=" << track->id << std::endl;
00036 
00037     KDL::Vector track_expressedIn_world(track->position.x, track->position.y, 
00038                                         track->position.z);
00039 
00040     std::string segmentName(track->segment); 
00041     KDL::SegmentMap::const_iterator segmentIter(segmentMap.find(segmentName));
00042     if ( segmentIter == segmentMap.end() ) {
00043       m_log << "ERROR: " << segmentName << " was not found in tree" << std::endl;
00044       continue;
00045     }
00046     const KDL::Segment& parentSegment(segmentIter->second.segment);
00047     
00048     //this is an existing observation
00049     ObservationMap::iterator existingObs(m_currentObservations.find(segmentName));
00050     Observation newobs(track_expressedIn_world);
00051     if ( existingObs == m_currentObservations.end() ) {
00052       //we have not yet observed this link, so add it
00053       m_currentObservations.insert(std::pair<std::string, Observation>(segmentName, newobs));
00054       m_log << "  new observation for " << segmentName << std::endl
00055             << "    world pose: " << track_expressedIn_world << std::endl;
00056     } else {
00057       //we've observed, and have not yet processed. just update the observation
00058       existingObs->second = newobs;
00059       m_log << "  repeat observation for " << segmentName << std::endl
00060             << "    world pose: " << track_expressedIn_world << std::endl;
00061     }
00062   }
00063 
00064   m_dataCondition.notify_all();
00065 }
00066 
00067 void ConstraintApp_MB::AffordanceFitHandler(const drc::affordance_plus_t *msg)
00068 {
00069   boost::mutex::scoped_lock lock(m_dataMutex);
00070 
00071   Affordance::Ptr msgAffordance(new Affordance(msg->aff.otdf_type, m_log));
00072   Affordance::StateVector msgState;
00073   if ( msgAffordance->GetStateFromMsg(msg->aff, msgState) ) {
00074     m_affordance = msgAffordance;
00075     m_fastFKSolver = boost::shared_ptr<FastFKSolver>(new FastFKSolver(m_affordance->m_tree));
00076     m_affordanceUID = msg->aff.uid;
00077     m_wasReset = true;
00078     m_currentState = msgState;
00079     m_prevFit = *msg; //this is a deep copy in c++ version of lcm
00080     m_log << "ConstraintApp_MB::AffordanceFitHandler: updated new affordance and state" << std::endl;
00081     m_affordance->PrintState(msgState);
00082   } else {
00083     m_log << "ConstraintApp_MB::AffordanceFitHandler: unable to create new afforance due to invalid state" << std::endl;
00084   }
00085 }
00086 
00087 bool ConstraintApp_MB::GetResetAndClear()
00088 {
00089   boost::mutex::scoped_lock lock(m_dataMutex);
00090   bool ret = m_wasReset;
00091   m_wasReset = false;
00092   return ret;
00093 }
00094 
00095 void ConstraintApp_MB::GetCurrentStateEstimate(StateVector& state) 
00096 { 
00097   boost::mutex::scoped_lock lock(m_dataMutex);
00098   state = m_currentState;
00099 }
00100 
00101 void ConstraintApp_MB::SetCurrentStateEstimate(const StateVector& state)
00102 {
00103   boost::mutex::scoped_lock lock(m_dataMutex);
00104   m_currentState = state;
00105 
00106   PublishFitMessage();
00107 }
00108 
00109 bool ConstraintApp_MB::WaitForObservations(unsigned int timeout_ms)
00110 {
00111   m_log << "ConstraintApp_MB::WaitForObservations, timeout = " << timeout_ms << std::endl;
00112 
00113   boost::mutex::scoped_lock lock(m_dataMutex);
00114 
00115   if ( !m_currentObservations.empty() ) return true;
00116 
00117   boost::system_time timeout = boost::get_system_time() + boost::posix_time::milliseconds(timeout_ms);
00118   m_dataCondition.timed_wait(lock, timeout);
00119 
00120   return !m_currentObservations.empty();
00121 }
00122 
00123 bool ConstraintApp_MB::GetObservations(ObservationVector& actualObservations,
00124                                        IdVector& observationIds)
00125 {
00126   boost::mutex::scoped_lock lock(m_dataMutex);
00127 
00128   //m_log << "ConstraintApp_MB::GetObservations" << std::endl;
00129 
00130   actualObservations.clear();
00131   observationIds.clear();
00132 
00133   actualObservations.reserve(m_currentObservations.size()*3);
00134   observationIds.reserve(m_currentObservations.size());
00135 
00136   for ( ObservationMap::const_iterator iter = m_currentObservations.begin(); 
00137         iter != m_currentObservations.end(); ++iter ) {
00138     actualObservations.push_back(iter->second.obs_expressedIn_world[0]);
00139     actualObservations.push_back(iter->second.obs_expressedIn_world[1]);
00140     actualObservations.push_back(iter->second.obs_expressedIn_world[2]);
00141 
00142     observationIds.push_back(iter->first);
00143 
00144     //m_log << "   added observation for id " << iter->first << std::endl
00145     //    << "      obs: " << iter->second.obs_expressedIn_world << std::endl;
00146   }
00147 
00148   m_currentObservations.clear();
00149 
00150   return true;
00151 }
00152 
00153 /*
00154 bool ConstraintApp_MB::GetExpectedObservations(const StateVector& state,
00155                                                const IdVector& observationIds,
00156                                                ObservationVector& observations)
00157 {
00158   boost::mutex::scoped_lock lock(m_dataMutex);
00159 
00160   m_log << "ConstraintApp_MB::GetExpectedObservations" << std::endl;
00161 
00162   observations.clear();
00163   observations.reserve(observationIds.size()*3);
00164 
00165   KDL::JntArray joints(state.size());
00166   m_affordance->DecodeState(state, joints);
00167 
00168   MyTreeFkSolverPos_recursive fk(m_affordance->m_tree);
00169   MyTreeFkSolverPos_recursive::SegmentToPoseMap map;
00170   int fkret = fk.JntToCart(joints, map);
00171   if ( fkret < 0 ) {
00172     std::cout << "ConstraintApp_HCA::GetExpectedObservations: error while getting forward kinematics." << std::endl;
00173     return false;
00174   }
00175 
00176   for ( int i = 0; i < observationIds.size(); i++ ) {
00177     MyTreeFkSolverPos_recursive::SegmentToPoseMap::const_iterator iter = map.find(observationIds[i]);
00178     if ( iter == map.end() ) {
00179       std::cout << "ConstraintApp_HCA::GetExpectedObservations: unable to find pose for segment " 
00180                 << observationIds[i] << std::endl;
00181       return false;
00182     } 
00183     const KDL::Frame& frame(iter->second);
00184 
00185     observations.push_back(frame.p[0]);
00186     observations.push_back(frame.p[1]);
00187     observations.push_back(frame.p[2]);
00188   }
00189 
00190   return true;
00191 }
00192 */
00193 
00194 bool ConstraintApp_MB::GetExpectedObservations(const StateVector& state,
00195                                                const IdVector& observationIds,
00196                                                ObservationVector& observations)
00197 {
00198   boost::mutex::scoped_lock lock(m_dataMutex);
00199 
00200   //m_log << "ConstraintApp_MB::GetExpectedObservations" << std::endl;
00201 
00202   observations.clear();
00203   observations.reserve(observationIds.size()*3);
00204 
00205   KDL::JntArray joints(state.size());
00206   m_affordance->DecodeState(state, joints);
00207   m_fastFKSolver->setJointPositions(joints);
00208 
00209   //KDL::Tree& tree(m_affordance->GetTree());
00210   //const KDL::SegmentMap& segmentMap(tree.getSegments());
00211 
00212   for ( IdVector::const_iterator iter = observationIds.begin(); 
00213         iter != observationIds.end(); ++iter ) {
00214 
00215     std::string trackSegmentName(*iter);
00216     /*
00217     KDL::SegmentMap::const_iterator obsIter(segmentMap.find(trackSegmentName));
00218     if ( obsIter == segmentMap.end() ) {
00219       m_log << "ERROR: unable to find corresponding segment: " << *iter << std::endl;
00220       return false;
00221     }
00222 
00223     KDL::Frame track_expressedIn_world;
00224     if ( !m_affordance->GetSegmentExpressedInWorld(trackSegmentName, state, 
00225                                                    track_expressedIn_world) ) {
00226       m_log << "ERROR: unable to decode current state. not processing any further expected observations." << std::endl;
00227       return false;
00228     }
00229     */
00230     KDL::Frame track_expressedIn_world;
00231     if ( !m_fastFKSolver->getFrame(*iter, track_expressedIn_world) ) {
00232       m_log << "ERROR: unable to decode current state. not processing any further expected observations." << std::endl;
00233       return false;
00234     }
00235 
00236     /*
00237     m_log << "old_output: " << track_expressedIn_world << std::endl
00238           << "new_output: " << track_expressedIn_world2 << std::endl;
00239     */
00240 
00241     observations.push_back(track_expressedIn_world.p[0]);
00242     observations.push_back(track_expressedIn_world.p[1]);
00243     observations.push_back(track_expressedIn_world.p[2]);
00244 
00245     /*
00246     m_log << "   added expected observation for " << trackSegmentName << std::endl
00247           << "      exp: " << track_expressedIn_world.p << std::endl;
00248     */
00249   }
00250   return true;
00251 }
00252 
00253 int ConstraintApp_MB::GetStateSize() 
00254 {
00255   return m_currentState.size();
00256 }
00257 
00258 void ConstraintApp_MB::PublishFitMessage()
00259 {
00260   drc::affordance_plus_t newMsg = m_prevFit;
00261   drc::affordance_t& affordance(newMsg.aff);
00262 
00263   affordance.aff_store_control = drc::affordance_t::UPDATE;
00264   affordance.origin_xyz[0] = m_currentState[0];
00265   affordance.origin_xyz[1] = m_currentState[1];
00266   affordance.origin_xyz[2] = m_currentState[2];
00267   affordance.origin_rpy[0] = m_currentState[3];
00268   affordance.origin_rpy[1] = m_currentState[4];
00269   affordance.origin_rpy[2] = m_currentState[5];
00270 
00271   std::vector<std::string> jointNames;
00272   m_affordance->GetJointNames(jointNames);
00273 
00274   assert(affordance.nstates == jointNames.size()-6);
00275   
00276   for(int i = 6; i < jointNames.size(); i++ ) {
00277     affordance.states[i-6] = m_currentState[i];
00278     affordance.state_names[i-6] = jointNames[i];
00279   }
00280 
00281   m_lcm->publish("AFFORDANCE_FIT", &newMsg);
00282 }
00283 
00284 bool ConstraintApp_MB::GetJacobian(const StateVector& state, 
00285                                    const IdVector& observationIds,
00286                                    ConstraintApp_MB::Jacobian& jacobian, 
00287                                    int method /*= 1*/)
00288 {
00289   const StateVector& eulerState(state);
00290 
00291   // the KDL jacobian is (observation twists) x (delta euler state & delta joint angles)
00292   // our Jacobiain is (observation positions) x (delta euler state & delta joint angles)
00293   jacobian.resize(observationIds.size()*3, eulerState.size()); 
00294 
00295   for ( int i = 0; i < observationIds.size(); i++ ) {
00296     KDL::Jacobian thisObservationJacobian;
00297     if ( method == 1 ) {
00298       if ( !m_affordance->GetSegmentJacobianExpressedInWorld(observationIds[i],
00299                                                             eulerState,
00300                                                             thisObservationJacobian) ) {
00301         std::cout << "ERROR: GetJacobian: unable to find " 
00302                   << observationIds[i] << " segment" << std::endl;
00303         return false;
00304       }
00305     } else if ( method == 2 ) {
00306       if ( !m_affordance->GetSegmentJacobianExpressedInWorld2(observationIds[i],
00307                                                              eulerState,
00308                                                              thisObservationJacobian) ) {
00309         std::cout << "ERROR: GetJacobian: unable to find " 
00310                   << observationIds[i] << " segment" << std::endl;
00311         return false;
00312       }
00313     } else {
00314       std::cout << "ERROR: GetJacobian: invalid method=" << method << std::endl;
00315       return false;
00316     }
00317 
00318     //the first 3 rows contain the position jacobian
00319     jacobian.middleRows<3>(i*3) = thisObservationJacobian.data.topRows(3);
00320   }
00321 
00322   return true;
00323 }
All Classes Files Functions Variables Typedefs