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 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);
}
