bool toiDynTree(const yarp::sig::Vector& yarpVector, VectorDynSize& iDynTreeVector)
{
iDynTreeVector.resize(yarpVector.size());
memcpy(iDynTreeVector.data(),yarpVector.data(),yarpVector.size()*sizeof(double));
return true;
}
std::string DynamicsRegressorGenerator::getDescriptionOfParameters(const VectorDynSize& values)
{
this->pimpl->m_parameters =
Eigen::Map<const Eigen::VectorXd>(values.data(),
values.size());
return this->pimpl->m_pLegacyGenerator->getDescriptionOfParameters(this->pimpl->m_parameters);
}
VectorDynSize::VectorDynSize(const VectorDynSize& vec): m_size(vec.size()), m_capacity(vec.capacity())
{
if( this->m_size == 0 )
{
this->m_data = 0;
}
else
{
this->m_data = new double[this->m_capacity];
std::memcpy(this->m_data,vec.data(),this->m_capacity*sizeof(double));
}
}
bool DynamicsRegressorGenerator::getModelParameters(VectorDynSize& values)
{
if( values.size() != this->getNrOfParameters() )
{
std::cerr << "[ERROR] error for getModelParameters" << std::endl;
return false;
}
this->pimpl->m_pLegacyGenerator->getModelParameters(this->pimpl->m_parameters);
Eigen::Map<Eigen::VectorXd>(values.data(),
values.size()) =
this->pimpl->m_parameters;
return true;
}
bool DynamicalSystem::setInitialState(const VectorDynSize &state)
{
if (state.size() != m_initialState.size()){
reportError("DynamicalSystem", "setInitialState", "Wrong initial state dimension.");
return false;
}
m_initialState = state;
return true;
}
bool DynamicalSystem::setControlInput(const VectorDynSize &control)
{
if (control.size() != controlSpaceSize()) {
reportError("DynamicalSystem", "setControlInput", "Wrong control dimension.");
return false;
}
m_controlInput = control;
return true;
}
bool JetsVisualization::setJetsIntensity(const VectorDynSize &jetsIntensity)
{
if( jetsIntensity.size() != getNrOfJets() ) return false;
// Inefficient implementation: create a new mesh whenever the intensity are updated
for(size_t i=0; i < getNrOfJets(); i++)
{
drawJet(i,jetsIntensity(i));
}
return true;
}
bool DynamicsRegressorGenerator::computeRegressor(MatrixDynSize& regressor, VectorDynSize& known_terms)
{
if( !this->isValid() )
{
return false;
}
int ret_value =
this->pimpl->m_pLegacyGenerator->computeRegressor(this->pimpl->m_regressor,
this->pimpl->m_knownTerms);
// \todo TODO write proper header for conversion from/to Eigen
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(regressor.data(),
regressor.rows(),
regressor.cols()) = this->pimpl->m_regressor;
Eigen::Map<Eigen::VectorXd>(known_terms.data(),
known_terms.size()) = this->pimpl->m_knownTerms;
return (ret_value == 0);
}
bool SensorsMeasurements::toVector(VectorDynSize & measurementVector) const
{
unsigned int itr;
LinAcceleration thisLinAcc;
AngVelocity thisAngVel;
Wrench thisWrench;
unsigned int numFT = this->pimpl->SixAxisFTSensorsMeasurements.size();
unsigned int numAcc = this->pimpl->AccelerometerMeasurements.size();
unsigned int numGyro = this->pimpl->GyroscopeMeasurements.size();
bool ok = true;
measurementVector.resize(6*numFT + 3*numAcc + 3*numGyro);
for(itr = 0; itr<numFT; itr++)
{
thisWrench = this->pimpl->SixAxisFTSensorsMeasurements.at(itr);
ok && measurementVector.setVal(6*itr, thisWrench.getVal(0));
ok && measurementVector.setVal(6*itr+1,thisWrench.getVal(1));
ok && measurementVector.setVal(6*itr+2,thisWrench.getVal(2));
ok && measurementVector.setVal(6*itr+3,thisWrench.getVal(3));
ok && measurementVector.setVal(6*itr+4,thisWrench.getVal(4));
ok && measurementVector.setVal(6*itr+5,thisWrench.getVal(5));
}
for(itr = 0; itr<numAcc; itr++)
{
thisLinAcc = this->pimpl->AccelerometerMeasurements.at(itr);
ok && measurementVector.setVal(6*numFT + 3*itr,thisLinAcc.getVal(0));
ok && measurementVector.setVal(6*numFT + 3*itr+1,thisLinAcc.getVal(1));
ok && measurementVector.setVal(6*numFT + 3*itr+2,thisLinAcc.getVal(2));
}
for(itr = 0; itr<numGyro; itr++)
{
thisAngVel = this->pimpl->GyroscopeMeasurements.at(itr);
ok && measurementVector.setVal(6*numFT + 3*numAcc + 3*itr,thisAngVel.getVal(0));
ok && measurementVector.setVal(6*numFT + 3*numAcc + 3*itr+1,thisAngVel.getVal(1));
ok && measurementVector.setVal(6*numFT + 3*numAcc + 3*itr+2,thisAngVel.getVal(2));
}
return ok;
}
