// From DC->N for each thruster. Verify constant used
void SamplesInput::Forces(base::VectorXd &input, base::VectorXd &output, double pos_Cv, double neg_Cv)
{
//output = Eigen::VectorXd::Zero(6);
for(int i=0; i< input.size(); i++)
{
if( input[i] <= 0 )
output[i] = neg_Cv * input[i] * fabs(double (input[i]));
else
output[i] = pos_Cv * input[i] * fabs(double (input[i]));
}
}
void NMPC::updateStates(const base::VectorXd &_pose_samples, const base::Vector6d &_efforts)
{
base::VectorXd pose_samples = Eigen::VectorXd::Zero(12);
for(int i = 0; i < _pose_samples.size(); i++)
{
if(base::isUnset<double>(_pose_samples[i]))
pose_samples[i] = 0;
else
pose_samples[i] = _pose_samples[i];
}
for (int i = 0; i < 3; ++i)
{
acadoVariables.x0[i] = pose_samples[i + 6]; // linear velocity
acadoVariables.x0[i+3] = pose_samples[i + 9]; // angular velocity
acadoVariables.x0[i+6] = pose_samples[i]; // position
acadoVariables.x0[i+9] = pose_samples[i + 3]; // orientation
acadoVariables.x0[i+12] = _efforts[i]; // forces
acadoVariables.x0[i+15] = _efforts[i + 3]; // torques
}
}
// From PWM->DC. Verify constant used
void SamplesInput::PWMtoDC(base::VectorXd &input, base::VectorXd &output, double ThrusterVoltage)
{
//output = Eigen::VectorXd::Zero(6);
for(int i=0; i< input.size(); i++)
output[i] = ThrusterVoltage * input[i];
}