debut projection

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -123,6 +123,13 @@ module hpp
 		/// \param com target com
 		double projectStateToCOM(in unsigned short stateId,  in floatSeq com) raises (Error);
 
+		/// Create a state and push it to the state array
+		///
+		/// \param q configuration
+		/// \param names list of effectors in contact
+		/// \return stateId
+		short createState(in floatSeq configuration, in Names_t contactLimbs) raises (Error);
+
     /// Get Sample configuration by its id
     /// \param sampleName name of the limb from which to retrieve a sample
     /// \param sampleId id of the desired samples

src/rbprmbuilder.impl.cc

@@ -618,6 +618,29 @@ namespace hpp {
         }
     }
 
+    CORBA::Short RbprmBuilder::createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error)
+    {
+        model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
+        fullBody_->device_->currentConfiguration(config);
+        fullBody_->device_->computeForwardKinematics();
+        State state;
+        state.configuration_ = config;
+        std::vector<std::string> names = stringConversion(contactLimbs);
+        for(std::vector<std::string>::const_iterator cit = names.begin(); cit != names.end(); ++cit)
+        {
+            rbprm::RbPrmLimbPtr_t limb = fullBody_->GetLimbs().at(*cit);
+            const std::string& limbName = *cit;
+            state.contacts_[limbName] = true;
+            const fcl::Vec3f position = limb->effector_->currentTransformation().getTranslation();
+            state.contactPositions_[limbName] = position;
+            state.contactNormals_[limbName] = limb->effector_->currentTransformation().getRotation() * limb->normal_;
+            state.contactRotation_[limbName] = limb->effector_->currentTransformation().getRotation();
+
+        }
+        lastStatesComputed_.push_back(state);
+        return lastStatesComputed_.size()-1;
+    }
+
     double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com) throw (hpp::Error)
     {        
         model::Configuration_t com_target = dofArrayToConfig (3, com);

src/rbprmbuilder.impl.hh

@@ -191,6 +191,7 @@ namespace hpp {
                                            unsigned short numOptimizations,
                                            const hpp::Names_t& trackedEffectors) throw (hpp::Error);
         virtual hpp::floatSeq* projectToCom(double state, const hpp::floatSeq& targetCom) throw (hpp::Error);
+        virtual CORBA::Short createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
         virtual hpp::floatSeq* getConfigAtState(unsigned short stateId) throw (hpp::Error);
         double projectStateToCOMEigen(unsigned short stateId, const model::Configuration_t& com_target)throw (hpp::Error);
         virtual double projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com) throw (hpp::Error);