Skip to content

GitLab

Unverified Commit f62cc0d0 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by GitHub
Browse files

Merge pull request #84 from florent-lamiraux/devel 

Fix memory leak and update code
parents 16344234 a10f1b13
Pipeline #14588 passed with stage
in 4 minutes and 16 seconds
Hide whitespace changes
Inline Side-by-side
src/rbprmbuilder.impl.cc
View file @ f62cc0d0
......@@ -38,6 +38,7 @@
#include <hpp/core/configuration-shooter/uniform.hh>
#include <hpp/core/collision-validation.hh>
#include <hpp/core/problem-solver.hh>
#include <hpp/corbaserver/conversions.hh>
#include <fstream>
#include <hpp/rbprm/planner/dynamic-planner.hh>
#include <hpp/rbprm/planner/rbprm-steering-kinodynamic.hh>
......@@ -61,6 +62,9 @@ namespace rbprm {
namespace impl {
using corbaServer::vectorToFloatSeq;
using corbaServer::toNames_t;
const pinocchio::Computation_t flag =
static_cast<pinocchio::Computation_t>(pinocchio::JOINT_POSITION | pinocchio::JACOBIAN | pinocchio::COM);
......@@ -73,19 +77,9 @@ RbprmBuilder::RbprmBuilder()
// NOTHING
}
hpp::floatSeq vectorToFloatseq(const hpp::core::vector_t& input) {
CORBA::ULong size = (CORBA::ULong)input.size();
double* dofArray = hpp::floatSeq::allocbuf(size);
hpp::floatSeq floats(size, size, dofArray, true);
for (std::size_t i = 0; i < size; ++i) {
dofArray[i] = input[i];
}
return floats;
}
void RbprmBuilder::loadRobotRomModel(const char* robotName,
const char* rootJointType,
const char* urdfName) throw(hpp::Error) {
const char* urdfName) {
try {
hpp::pinocchio::DevicePtr_t romDevice = pinocchio::Device::create(robotName);
romDevices_.insert(std::make_pair(robotName, romDevice));
......@@ -103,7 +97,7 @@ void RbprmBuilder::loadRobotRomModel(const char* robotName,
void RbprmBuilder::loadRobotCompleteModel(const char* robotName,
const char* rootJointType,
const char* urdfName,
const char* srdfName) throw(hpp::Error) {
const char* srdfName) {
if (!romLoaded_) {
std::string err("Rom must be loaded before loading complete model");
hppDout(error, err);
......@@ -128,7 +122,7 @@ void RbprmBuilder::loadFullBodyRobot(const char* robotName,
const char* rootJointType,
const char* urdfName,
const char* srdfName,
const char* selectedProblem) throw(hpp::Error) {
const char* selectedProblem) {
try {
hpp::pinocchio::DevicePtr_t device = pinocchio::Device::create(robotName);
hpp::pinocchio::urdf::loadModel(device, 0, "",
......@@ -163,7 +157,7 @@ void RbprmBuilder::loadFullBodyRobot(const char* robotName,
analysisFactory_ = new sampling::AnalysisFactory(fullBody());
}
void RbprmBuilder::loadFullBodyRobotFromExistingRobot() throw(hpp::Error) {
void RbprmBuilder::loadFullBodyRobotFromExistingRobot() {
try {
fullBody() = rbprm::RbPrmFullBody::create(problemSolver()->problem()->robot());
problemSolver()->pathValidationType("Discretized", 0.05); // reset to avoid conflict with rbprm path
......@@ -178,7 +172,7 @@ void RbprmBuilder::loadFullBodyRobotFromExistingRobot() throw(hpp::Error) {
analysisFactory_ = new sampling::AnalysisFactory(fullBody());
}
hpp::floatSeq* RbprmBuilder::getSampleConfig(const char* limb, unsigned int sampleId) throw(hpp::Error) {
hpp::floatSeq* RbprmBuilder::getSampleConfig(const char* limb, unsigned int sampleId) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
const T_Limb& limbs = fullBody()->GetLimbs();
T_Limb::const_iterator lit = limbs.find(std::string(limb));
......@@ -187,7 +181,6 @@ hpp::floatSeq* RbprmBuilder::getSampleConfig(const char* limb, unsigned int samp
throw Error(err.c_str());
}
const RbPrmLimbPtr_t& limbPtr = lit->second;
hpp::floatSeq* dofArray;
Eigen::VectorXd config = fullBody()->device_->currentConfiguration();
if (sampleId > limbPtr->sampleContainer_.samples_.size()) {
std::string err("Limb " + std::string(limb) + "does not have samples.");
......@@ -195,13 +188,10 @@ hpp::floatSeq* RbprmBuilder::getSampleConfig(const char* limb, unsigned int samp
}
const sampling::Sample& sample = limbPtr->sampleContainer_.samples_[sampleId];
config.segment(sample.startRank_, sample.length_) = sample.configuration_;
dofArray = new hpp::floatSeq();
dofArray->length(_CORBA_ULong(config.rows()));
for (std::size_t i = 0; i < _CORBA_ULong(config.rows()); i++) (*dofArray)[(_CORBA_ULong)i] = config[i];
return dofArray;
return vectorToFloatSeq(config);
}
hpp::floatSeq* RbprmBuilder::getSamplePosition(const char* limb, unsigned int sampleId) throw(hpp::Error) {
hpp::floatSeq* RbprmBuilder::getSamplePosition(const char* limb, unsigned int sampleId) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
const T_Limb& limbs = fullBody()->GetLimbs();
T_Limb::const_iterator lit = limbs.find(std::string(limb));
......@@ -210,17 +200,12 @@ hpp::floatSeq* RbprmBuilder::getSamplePosition(const char* limb, unsigned int sa
throw Error(err.c_str());
}
const RbPrmLimbPtr_t& limbPtr = lit->second;
hpp::floatSeq* dofArray;
if (sampleId > limbPtr->sampleContainer_.samples_.size()) {
std::string err("Limb " + std::string(limb) + "does not have samples.");
throw Error(err.c_str());
}
const sampling::Sample& sample = limbPtr->sampleContainer_.samples_[sampleId];
const fcl::Vec3f& position = sample.effectorPosition_;
dofArray = new hpp::floatSeq();
dofArray->length(_CORBA_ULong(3));
for (std::size_t i = 0; i < 3; i++) (*dofArray)[(_CORBA_ULong)i] = position[i];
return dofArray;
return vectorToFloatSeq(sample.effectorPosition_);
}
typedef Eigen::Matrix<value_type, 4, 3, Eigen::RowMajor> Matrix43;
......@@ -395,7 +380,7 @@ T_Configuration doubleDofArrayToConfig(const pinocchio::DevicePtr_t& robot, cons
}
hpp::floatSeqSeq* RbprmBuilder::getEffectorPosition(const char* lb,
const hpp::floatSeq& configuration) throw(hpp::Error) {
const hpp::floatSeq& configuration) {
try {
const std::string limbName(lb);
const RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(limbName);
......@@ -430,7 +415,7 @@ hpp::floatSeqSeq* RbprmBuilder::getEffectorPosition(const char* lb,
}
}
CORBA::UShort RbprmBuilder::getNumSamples(const char* limb) throw(hpp::Error) {
CORBA::UShort RbprmBuilder::getNumSamples(const char* limb) {
const T_Limb& limbs = fullBody()->GetLimbs();
T_Limb::const_iterator lit = limbs.find(std::string(limb));
if (lit == limbs.end()) {
......@@ -440,7 +425,7 @@ CORBA::UShort RbprmBuilder::getNumSamples(const char* limb) throw(hpp::Error) {
return (CORBA::UShort)(lit->second->sampleContainer_.samples_.size());
}
floatSeq* RbprmBuilder::getOctreeNodeIds(const char* limb) throw(hpp::Error) {
floatSeq* RbprmBuilder::getOctreeNodeIds(const char* limb) {
const T_Limb& limbs = fullBody()->GetLimbs();
T_Limb::const_iterator lit = limbs.find(std::string(limb));
if (lit == limbs.end()) {
......@@ -457,7 +442,7 @@ floatSeq* RbprmBuilder::getOctreeNodeIds(const char* limb) throw(hpp::Error) {
return dofArray;
}
double RbprmBuilder::getSampleValue(const char* limb, const char* valueName, unsigned int sampleId) throw(hpp::Error) {
double RbprmBuilder::getSampleValue(const char* limb, const char* valueName, unsigned int sampleId) {
const T_Limb& limbs = fullBody()->GetLimbs();
T_Limb::const_iterator lit = limbs.find(std::string(limb));
if (lit == limbs.end()) {
......@@ -477,7 +462,7 @@ double RbprmBuilder::getSampleValue(const char* limb, const char* valueName, uns
return cit->second[sampleId];
}
double RbprmBuilder::getEffectorDistance(unsigned short state1, unsigned short state2) throw(hpp::Error) {
double RbprmBuilder::getEffectorDistance(unsigned short state1, unsigned short state2) {
try {
std::size_t s1((std::size_t)state1), s2((std::size_t)state2);
if (lastStatesComputed_.size() < s1 || lastStatesComputed_.size() < s2) {
......@@ -508,25 +493,25 @@ std::vector<double> doubleConversion(const hpp::floatSeq& dofArray) {
return res;
}
void RbprmBuilder::setStaticStability(const bool staticStability) throw(hpp::Error) {
void RbprmBuilder::setStaticStability(const bool staticStability) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
fullBody()->staticStability(staticStability);
}
void RbprmBuilder::setReferenceConfig(const hpp::floatSeq& referenceConfig) throw(hpp::Error) {
void RbprmBuilder::setReferenceConfig(const hpp::floatSeq& referenceConfig) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
Configuration_t config(dofArrayToConfig(fullBody()->device_, referenceConfig));
refPose_ = config;
fullBody()->referenceConfig(config);
}
void RbprmBuilder::setPostureWeights(const hpp::floatSeq& postureWeights) throw(hpp::Error) {
void RbprmBuilder::setPostureWeights(const hpp::floatSeq& postureWeights) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
Configuration_t config(dofArrayToConfig(fullBody()->device_->numberDof(), postureWeights));
fullBody()->postureWeights(config);
}
void RbprmBuilder::setReferenceEndEffector(const char* romName, const hpp::floatSeq& ref) throw(hpp::Error) {
void RbprmBuilder::setReferenceEndEffector(const char* romName, const hpp::floatSeq& ref) {
std::string name(romName);
hpp::pinocchio::RbPrmDevicePtr_t device =
std::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot());
......@@ -536,23 +521,23 @@ void RbprmBuilder::setReferenceEndEffector(const char* romName, const hpp::float
device->setEffectorReference(name, config);
}
void RbprmBuilder::usePosturalTaskContactCreation(const bool usePosturalTaskContactCreation) throw(hpp::Error) {
void RbprmBuilder::usePosturalTaskContactCreation(const bool usePosturalTaskContactCreation) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
fullBody()->usePosturalTaskContactCreation(usePosturalTaskContactCreation);
}
void RbprmBuilder::setFilter(const hpp::Names_t& roms) throw(hpp::Error) {
void RbprmBuilder::setFilter(const hpp::Names_t& roms) {
bindShooter_.romFilter_ = stringConversion(roms);
}
void RbprmBuilder::setAffordanceFilter(const char* romName, const hpp::Names_t& affordances) throw(hpp::Error) {
void RbprmBuilder::setAffordanceFilter(const char* romName, const hpp::Names_t& affordances) {
std::string name(romName);
std::vector<std::string> affNames = stringConversion(affordances);
bindShooter_.affFilter_.erase(name);
bindShooter_.affFilter_.insert(std::make_pair(name, affNames));
}
void RbprmBuilder::boundSO3(const hpp::floatSeq& limitszyx) throw(hpp::Error) {
void RbprmBuilder::boundSO3(const hpp::floatSeq& limitszyx) {
std::vector<double> limits = doubleConversion(limitszyx);
if (limits.size() != 6) {
throw Error("Can not bound SO3, array of 6 double required");
......@@ -587,7 +572,7 @@ rbprm::T_Limb GetFreeLimbs(const RbPrmFullBodyPtr_t fullBody, const hpp::rbprm::
}
double RbprmBuilder::projectStateToCOMEigen(unsigned short stateId, const pinocchio::Configuration_t& com_target,
unsigned short maxNumeSamples) throw(hpp::Error) {
unsigned short maxNumeSamples) {
if (lastStatesComputed_.size() <= stateId) {
throw std::runtime_error("Unexisting state " + std::string("" + (stateId)));
}
......@@ -599,7 +584,7 @@ double RbprmBuilder::projectStateToCOMEigen(unsigned short stateId, const pinocc
}
double RbprmBuilder::projectStateToCOMEigen(State& s, const pinocchio::Configuration_t& com_target,
unsigned short maxNumeSamples) throw(hpp::Error) {
unsigned short maxNumeSamples) {
try {
// /hpp::pinocchio::Configuration_t c = rbprm::interpolation::projectOnCom(fullBody(),
// problemSolver()->problem(),s,com_target,succes);
......@@ -687,7 +672,7 @@ double RbprmBuilder::projectStateToCOMEigen(State& s, const pinocchio::Configura
}
CORBA::Short RbprmBuilder::createState(const hpp::floatSeq& configuration,
const hpp::Names_t& contactLimbs) throw(hpp::Error) {
const hpp::Names_t& contactLimbs) {
pinocchio::Configuration_t config = dofArrayToConfig(fullBody()->device_, configuration);
fullBody()->device_->currentConfiguration(config);
fullBody()->device_->computeForwardKinematics();
......@@ -710,7 +695,7 @@ CORBA::Short RbprmBuilder::createState(const hpp::floatSeq& configuration,
return (CORBA::Short)(lastStatesComputed_.size() - 1);
}
CORBA::Short RbprmBuilder::cloneState(unsigned short stateId) throw(hpp::Error) {
CORBA::Short RbprmBuilder::cloneState(unsigned short stateId) {
try {
if (lastStatesComputed_.size() <= stateId) {
throw std::runtime_error("Can't clone state: invalid state id : " + std::string("" + stateId) +
......@@ -726,13 +711,13 @@ CORBA::Short RbprmBuilder::cloneState(unsigned short stateId) throw(hpp::Error)
}
double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com,
unsigned short max_num_sample) throw(hpp::Error) {
unsigned short max_num_sample) {
pinocchio::Configuration_t com_target = dofArrayToConfig(3, com);
return projectStateToCOMEigen(stateId, com_target, max_num_sample);
}
double RbprmBuilder::projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root,
const hpp::floatSeq& offset) throw(hpp::Error) {
const hpp::floatSeq& offset) {
pinocchio::Configuration_t root_target = dofArrayToConfig(7, root);
pinocchio::Configuration_t offset_target = dofArrayToConfig(3, offset);
if (lastStatesComputed_.size() <= stateId) {
......@@ -755,7 +740,7 @@ double RbprmBuilder::projectStateToRoot(unsigned short stateId, const hpp::float
rbprm::State RbprmBuilder::generateContacts_internal(const hpp::floatSeq& configuration,
const hpp::floatSeq& direction, const hpp::floatSeq& acceleration,
const double robustnessThreshold) throw(hpp::Error) {
const double robustnessThreshold) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
try {
fcl::Vec3f dir, acc;
......@@ -780,7 +765,7 @@ rbprm::State RbprmBuilder::generateContacts_internal(const hpp::floatSeq& config
hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration, const hpp::floatSeq& direction,
const hpp::floatSeq& acceleration,
const double robustnessThreshold) throw(hpp::Error) {
const double robustnessThreshold) {
try {
rbprm::State state = generateContacts_internal(configuration, direction, acceleration, robustnessThreshold);
hpp::floatSeq* dofArray = new hpp::floatSeq();
......@@ -795,7 +780,7 @@ hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration
CORBA::Short RbprmBuilder::generateStateInContact(const hpp::floatSeq& configuration, const hpp::floatSeq& direction,
const hpp::floatSeq& acceleration,
const double robustnessThreshold) throw(hpp::Error) {
const double robustnessThreshold) {
try {
rbprm::State state = generateContacts_internal(configuration, direction, acceleration, robustnessThreshold);
lastStatesComputed_.push_back(state);
......@@ -806,7 +791,7 @@ CORBA::Short RbprmBuilder::generateStateInContact(const hpp::floatSeq& configura
}
}
hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLimbs) throw(hpp::Error) {
hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLimbs) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
try {
fcl::Vec3f z(0, 0, 1);
......@@ -885,7 +870,7 @@ hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLi
}
hpp::floatSeq* RbprmBuilder::getContactSamplesIds(const char* limbname, const hpp::floatSeq& configuration,
const hpp::floatSeq& direction) throw(hpp::Error) {
const hpp::floatSeq& direction) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
try {
fcl::Vec3f dir;
......@@ -933,7 +918,7 @@ hpp::floatSeq* RbprmBuilder::getContactSamplesIds(const char* limbname, const hp
}
short RbprmBuilder::generateContactState(::CORBA::UShort cId, const char* name,
const ::hpp::floatSeq& direction) throw(hpp::Error) {
const ::hpp::floatSeq& direction) {
try {
if (lastStatesComputed_.size() <= (std::size_t)(cId)) {
throw std::runtime_error("Unexisting state " + std::string("" + (cId + 1)));
......@@ -999,7 +984,7 @@ short RbprmBuilder::generateContactState(::CORBA::UShort cId, const char* name,
hpp::floatSeqSeq* RbprmBuilder::getContactSamplesProjected(const char* limbname, const hpp::floatSeq& configuration,
const hpp::floatSeq& direction,
unsigned short numSamples) throw(hpp::Error) {
unsigned short numSamples) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
try {
fcl::Vec3f dir;
......@@ -1080,7 +1065,7 @@ hpp::floatSeqSeq* RbprmBuilder::getContactSamplesProjected(const char* limbname,
}
}
hpp::floatSeq* RbprmBuilder::getSamplesIdsInOctreeNode(const char* limb, double octreeNodeId) throw(hpp::Error) {
hpp::floatSeq* RbprmBuilder::getSamplesIdsInOctreeNode(const char* limb, double octreeNodeId) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
try {
long ocId((long)octreeNodeId);
......@@ -1115,7 +1100,7 @@ void RbprmBuilder::addLimb(const char* id, const char* limb, const char* effecto
const hpp::floatSeq& normal, double x, double y, unsigned int samples,
const char* heuristicName, double resolution, const char* contactType,
double disableEffectorCollision, double grasp, const hpp::floatSeq& limbOffset,
const char* kinematicConstraintsPath, double kinematicConstraintsMin) throw(hpp::Error) {
const char* kinematicConstraintsPath, double kinematicConstraintsMin) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
try {
fcl::Vec3f off, norm, limbOff;
......@@ -1138,7 +1123,7 @@ void RbprmBuilder::addLimb(const char* id, const char* limb, const char* effecto
}
void RbprmBuilder::addNonContactingLimb(const char* id, const char* limb, const char* effector,
unsigned int samples) throw(hpp::Error) {
unsigned int samples) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
try {
fullBody()->AddNonContactingLimb(std::string(id), std::string(limb), std::string(effector),
......@@ -1150,7 +1135,7 @@ void RbprmBuilder::addNonContactingLimb(const char* id, const char* limb, const
void RbprmBuilder::addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName,
double loadValues, double disableEffectorCollision,
double grasp) throw(hpp::Error) {
double grasp) {
if (!fullBodyLoaded_) throw Error("No full body robot was loaded");
try {
std::string fileName(databasePath);
......@@ -1195,7 +1180,7 @@ void SetPositionAndNormal(rbprm::State& state, hpp::rbprm::RbPrmFullBodyPtr_t fu
}
void RbprmBuilder::setStartState(const hpp::floatSeq& configuration,
const hpp::Names_t& contactLimbs) throw(hpp::Error) {
const hpp::Names_t& contactLimbs) {
try {
std::vector<std::string> names = stringConversion(contactLimbs);
core::ValidationReportPtr_t validationReport;
......@@ -1215,7 +1200,7 @@ void RbprmBuilder::setStartState(const hpp::floatSeq& configuration,
}
hpp::floatSeq* RbprmBuilder::computeContactForConfig(const hpp::floatSeq& configuration,
const char* limbNam) throw(hpp::Error) {
const char* limbNam) {
State state;
std::string limb(limbNam);
try {
......@@ -1240,7 +1225,7 @@ hpp::floatSeq* RbprmBuilder::computeContactForConfig(const hpp::floatSeq& config
}
void RbprmBuilder::setEndState(const hpp::floatSeq& configuration,
const hpp::Names_t& contactLimbs) throw(hpp::Error) {
const hpp::Names_t& contactLimbs) {
try {
std::vector<std::string> names = stringConversion(contactLimbs);
core::ValidationReportPtr_t validationReport;
......@@ -1259,7 +1244,7 @@ void RbprmBuilder::setEndState(const hpp::floatSeq& configuration,
}
}
void RbprmBuilder::setStartStateId(unsigned short stateId) throw(hpp::Error) {
void RbprmBuilder::setStartStateId(unsigned short stateId) {
try {
if (lastStatesComputed_.size() == 0) {
throw std::runtime_error("states not yet computed, call interpolate() first.");
......@@ -1274,7 +1259,7 @@ void RbprmBuilder::setStartStateId(unsigned short stateId) throw(hpp::Error) {
}
}
void RbprmBuilder::setEndStateId(unsigned short stateId) throw(hpp::Error) {
void RbprmBuilder::setEndStateId(unsigned short stateId) {
try {
if (lastStatesComputed_.size() == 0) {
throw std::runtime_error("states not yet computed, call interpolate() first.");
......@@ -1290,7 +1275,7 @@ void RbprmBuilder::setEndStateId(unsigned short stateId) throw(hpp::Error) {
}
}
double RbprmBuilder::getTimeAtState(unsigned short stateId) throw(hpp::Error) {
double RbprmBuilder::getTimeAtState(unsigned short stateId) {
try {
if (lastStatesComputed_.size() == 0) {
throw std::runtime_error("states not yet computed, call interpolate() first.");
......@@ -1305,7 +1290,7 @@ double RbprmBuilder::getTimeAtState(unsigned short stateId) throw(hpp::Error) {
}
}
Names_t* RbprmBuilder::getContactsVariations(unsigned short stateIdFrom, unsigned short stateIdTo) throw(hpp::Error) {
Names_t* RbprmBuilder::getContactsVariations(unsigned short stateIdFrom, unsigned short stateIdTo) {
try {
if (lastStatesComputed_.size() == 0) {
throw std::runtime_error("states not yet computed, call interpolate() first.");
......@@ -1321,21 +1306,14 @@ Names_t* RbprmBuilder::getContactsVariations(unsigned short stateIdFrom, unsigne
State stateFrom = lastStatesComputed_[stateIdFrom];
State stateTo = lastStatesComputed_[stateIdTo];
std::vector<std::string> variations_s = stateTo.contactVariations(stateFrom);
CORBA::ULong size = (CORBA::ULong)variations_s.size();
char** nameList = Names_t::allocbuf(size);
Names_t* variations = new Names_t(size, size, nameList);
for (std::size_t i = 0; i < variations_s.size(); ++i) {
nameList[i] = (char*)malloc(sizeof(char) * (variations_s[i].length() + 1));
strcpy(nameList[i], variations_s[i].c_str());
}
return variations;
return toNames_t(variations_s.begin(), variations_s.end());
} catch (std::runtime_error& e) {
throw Error(e.what());
}
}
Names_t* RbprmBuilder::getCollidingObstacleAtConfig(const ::hpp::floatSeq& configuration,
const char* limbName) throw(hpp::Error) {
const char* limbName) {
try {
std::vector<std::string> res;
std::string name(limbName);
......@@ -1373,21 +1351,14 @@ Names_t* RbprmBuilder::getCollidingObstacleAtConfig(const ::hpp::floatSeq& confi
}
}
}
CORBA::ULong size = (CORBA::ULong)res.size();
char** nameList = Names_t::allocbuf(size);
Names_t* variations = new Names_t(size, size, nameList);
for (std::size_t i = 0; i < res.size(); ++i) {
nameList[i] = (char*)malloc(sizeof(char) * (res[i].length() + 1));
strcpy(nameList[i], res[i].c_str());
}
return variations;
return toNames_t(res.begin(), res.end());
} catch (std::runtime_error& e) {
throw Error(e.what());
}
}
floatSeqSeq* RbprmBuilder::getContactSurfacesAtConfig(const ::hpp::floatSeq& configuration,
const char* limbName) throw(hpp::Error) {
const char* limbName) {
try {
hppDout(notice, "begin getContactSurfacesAtConfig");
std::string name(limbName);
......@@ -1504,7 +1475,7 @@ std::vector<State> TimeStatesToStates(const T_StateFrame& ref) {
floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold,
unsigned short filterStates, bool testReachability, bool quasiStatic,
bool erasePreviousStates) throw(hpp::Error) {
bool erasePreviousStates) {
try {
if (startState_.configuration_.rows() == 0) {
throw std::runtime_error("Start state not initialized, can not interpolate ");
......@@ -1576,7 +1547,7 @@ hpp::floatSeqSeq* contactCone(RbPrmFullBodyPtr_t fullBody, State& state, const d
return res;
}
hpp::floatSeqSeq* RbprmBuilder::getContactCone(unsigned short stateId, double friction) throw(hpp::Error) {
hpp::floatSeqSeq* RbprmBuilder::getContactCone(unsigned short stateId, double friction) {
try {
if (lastStatesComputed_.size() <= stateId) {
throw std::runtime_error("Unexisting state " + std::string("" + (stateId)));
......@@ -1606,7 +1577,7 @@ State intermediary(const State& firstState, const State& thirdState, unsigned sh
return firstState;
}
hpp::floatSeqSeq* RbprmBuilder::getContactIntermediateCone(unsigned short stateId, double friction) throw(hpp::Error) {
hpp::floatSeqSeq* RbprmBuilder::getContactIntermediateCone(unsigned short stateId, double friction) {
try {
if (lastStatesComputed_.size() <= (std::size_t)(stateId + 1)) {
throw std::runtime_error("Unexisting state " + std::string("" + (stateId + 1)));
......@@ -1625,7 +1596,7 @@ hpp::floatSeqSeq* RbprmBuilder::getContactIntermediateCone(unsigned short stateI
}
}
Names_t* RbprmBuilder::getEffectorsTrajectoriesNames(unsigned short pathId) throw(hpp::Error) {
Names_t* RbprmBuilder::getEffectorsTrajectoriesNames(unsigned short pathId) {
try {
if (!fullBodyLoaded_) throw std::runtime_error("No Fullbody loaded");
EffectorTrajectoriesMap_t map;
......@@ -1637,15 +1608,7 @@ Names_t* RbprmBuilder::getEffectorsTrajectoriesNames(unsigned short pathId) thro
for (EffectorTrajectoriesMap_t::const_iterator it = map.begin(); it != map.end(); ++it) {
names.push_back(it->first);
}
// convert names (vector of string) to corba Names_t
CORBA::ULong size = (CORBA::ULong)names.size();
char** nameList = Names_t::allocbuf(size);
Names_t* limbsNames = new Names_t(size, size, nameList);
for (std::size_t i = 0; i < names.size(); ++i) {
nameList[i] = (char*)malloc(sizeof(char) * (names[i].length() + 1));
strcpy(nameList[i], names[i].c_str());
}
return limbsNames;
return toNames_t(names.begin(), names.end());
} catch (std::runtime_error& e) {
std::cout << "ERROR " << e.what() << std::endl;
throw Error(e.what());
......@@ -1653,7 +1616,7 @@ Names_t* RbprmBuilder::getEffectorsTrajectoriesNames(unsigned short pathId) thro
}
hpp::floatSeqSeqSeq* RbprmBuilder::getEffectorTrajectoryWaypoints(unsigned short pathId,
const char* effectorName) throw(hpp::Error) {
const char* effectorName) {
try {
if (!fullBodyLoaded_) throw std::runtime_error("No Fullbody loaded");
std::vector<bezier_Ptr> curvesVector;
......@@ -1686,7 +1649,7 @@ hpp::floatSeqSeqSeq* RbprmBuilder::getEffectorTrajectoryWaypoints(unsigned short
std::size_t i = 1;
for (bezier_t::t_point_t::const_iterator wit = waypoints.begin(); wit != waypoints.end(); ++wit, ++i) {
const bezier_t::point_t position = *wit;
(*curveWp)[(_CORBA_ULong)i] = vectorToFloatseq(position);
vectorToFloatSeq(position, (*curveWp)[(_CORBA_ULong)i]);
}
(*res)[(_CORBA_ULong)curveId] = (*curveWp);
}
......@@ -1697,7 +1660,7 @@ hpp::floatSeqSeqSeq* RbprmBuilder::getEffectorTrajectoryWaypoints(unsigned short
}
}
hpp::floatSeqSeq* RbprmBuilder::getPathAsBezier(unsigned short pathId) throw(hpp::Error) {
hpp::floatSeqSeq* RbprmBuilder::getPathAsBezier(unsigned short pathId) {
try {
if (!fullBodyLoaded_) throw std::runtime_error("No Fullbody loaded");
if (problemSolver()->paths().size() <= pathId)
......@@ -1726,7 +1689,7 @@ hpp::floatSeqSeq* RbprmBuilder::getPathAsBezier(unsigned short pathId) throw(hpp
// now add the waypoints :
std::size_t i = 1;
for (bezier_t::t_point_t::const_iterator wit = waypoints.begin(); wit != waypoints.end(); ++wit, ++i) {
(*res)[(_CORBA_ULong)i] = vectorToFloatseq(*wit);
vectorToFloatSeq(*wit, (*res)[(_CORBA_ULong)i]);
}
return res;
} catch (std::runtime_error& e) {
......@@ -1772,7 +1735,7 @@ core::PathVectorPtr_t addRotations(const T_Configuration& positions, pinocchio::
core::PathVectorPtr_t generateTrunkPath(RbPrmFullBodyPtr_t fullBody, core::ProblemSolverPtr_t problemSolver,
const hpp::floatSeqSeq& rootPositions, const pinocchio::Configuration_t q1,
const pinocchio::Configuration_t q2) throw(hpp::Error) {
const pinocchio::Configuration_t q2) {
try {
T_Configuration positions = doubleDofArrayToConfig(3, rootPositions);
if (positions.size() < 2) {
......@@ -1786,7 +1749,7 @@ core::PathVectorPtr_t generateTrunkPath(RbPrmFullBodyPtr_t fullBody, core::Probl
}
CORBA::Short RbprmBuilder::generateRootPath(const hpp::floatSeqSeq& rootPositions, const hpp::floatSeq& q1Seq,
const hpp::floatSeq& q2Seq) throw(hpp::Error) {
const hpp::floatSeq& q2Seq) {
try {
pinocchio::Configuration_t q1 = dofArrayToConfig(4, q1Seq), q2 = dofArrayToConfig(4, q2Seq);
return (CORBA::Short)problemSolver()->addPath(
......@@ -1796,7 +1759,7 @@ CORBA::Short RbprmBuilder::generateRootPath(const hpp::floatSeqSeq& rootPosition
}
}
CORBA::Short RbprmBuilder::straightPath(const hpp::floatSeqSeq& positions) throw(hpp::Error) {
CORBA::Short RbprmBuilder::straightPath(const hpp::floatSeqSeq& positions) {
try {
T_Configuration c = doubleDofArrayToConfig(3, positions);
if (c.size() < 2) {
......@@ -1817,7 +1780,7 @@ CORBA::Short RbprmBuilder::straightPath(const hpp::floatSeqSeq& positions) throw
}
}
CORBA::Short RbprmBuilder::generateCurveTraj(const hpp::floatSeqSeq& positions) throw(hpp::Error) {
CORBA::Short RbprmBuilder::generateCurveTraj(const hpp::floatSeqSeq& positions) {
try {
T_Configuration c = doubleDofArrayToConfig(3, positions);
bezier_t* curve = new bezier_t(c.begin(), c.end());
......@@ -1833,7 +1796,7 @@ CORBA::Short RbprmBuilder::generateCurveTraj(const hpp::floatSeqSeq& positions)
}
CORBA::Short RbprmBuilder::generateCurveTrajParts(const hpp::floatSeqSeq& positions,
const hpp::floatSeq& partitions) throw