Commit 269fe450 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

debut projection

parent 4680d270
......@@ -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
......
......@@ -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);
......
......@@ -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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment