int main(int argc, char *argv[])
{
// Declare potential array, initialise with command line arguments:
double potentials[4] = {atof(argv[2]), atof(argv[3]), atof(argv[4]), atof(argv[5])};
// Initialise other starting variables:
int iterations = atoi(argv[6]);
double relaxation = atof(argv[7]);
double convergence = atof(argv[8]);
// Store current CPU time:
clock_t tm;
tm = clock();
// Run solver:
array_data * data = fdm(argv[1], potentials, relaxation, iterations, convergence);
// Calculate elapsed CPU time:
tm = clock() - tm;
// Convert to seconds:
double runtime = double(tm) / CLOCKS_PER_SEC;
// Measure memory used by process:
int mem_used = mem_measure();
cout << "Memory used: " << mem_used << " KB\n";
// Output data to files:
data_out(&data, runtime);
// Store width and height of image for plotting purposes
int columns = data->columns;
int rows = data->rows;
// Clean up memory:
delete data -> prev_values;
delete data -> values;
delete data -> mask;
delete data -> xgrad;
delete data -> ygrad;
delete data;
cout << "Plotting data...\n";
// Plot potential:
plot(0,"pngcairo","png",columns,rows);
// Plot E-field:
plot(1,"pngcairo","png",columns,rows);
// Plot equipotential lines:
plot(2,"pngcairo","png",columns,rows);
// Plot E-field and equipotential lines:
plot(3,"pngcairo","png",columns,rows);
cout << "Done!\n";
return 0;
}
void KukaRMControllerRTNET::updateHook()
{
std::string fri_mode("e_fri_unkown_mode");
bool fri_cmd_mode = false;
RTT::FlowStatus fs_event = iport_events.read(fri_mode);
if (fri_mode == "e_fri_cmd_mode")
fri_cmd_mode = true;
else if (fri_mode == "e_fri_mon_mode")
fri_cmd_mode = false;
Eigen::VectorXd tau_dyn(LWRDOF);
Eigen::VectorXd F(LWRDOF);
std::vector<double> jntPos(LWRDOF);
std::vector<double> jntVel(LWRDOF);
std::vector<double> jntTrq(LWRDOF);
std::vector<double> tau_FRI(LWRDOF);
/* Get des mesures des capteurs */
RTT::FlowStatus joint_pos_fs = iport_msr_joint_pos.read(jntPos);
RTT::FlowStatus joint_vel_fs = iport_msr_joint_vel.read(jntVel);
RTT::FlowStatus joint_trq_fs = iport_msr_joint_trq.read(jntTrq);
if(fri_cmd_mode){
if(m_compteur<5000)
{
/* Mise a jour des mesures */
if(joint_trq_fs==RTT::NewData){
for(unsigned int i=0;i<LWRDOF;i++){
m_tau_mes[i] = jntTrq[i];
}
}
if(joint_pos_fs==RTT::NewData){
for(unsigned int i=0;i<LWRDOF;i++){
m_q_mes[i] = jntPos[i];
}
m_model->setJointPositions(m_q_mes); //Mise a jour des positions dans le modele
}
if(joint_vel_fs==RTT::NewData){
fdm(jntVel); //calcule l acceleration qpp
for(unsigned int i=0;i<LWRDOF;i++){
m_qp_mes[i] = jntVel[i];
}
m_model->setJointVelocities(m_qp_mes); //Mise a jour des vitesses dans le modele
}
/* Mise a jour des attributs */
tau_dyn = m_tau_mes-m_tau_FRI;
m_H = m_model->getInertiaMatrix();
m_c = m_model->getNonLinearTerms();
m_g = m_model->getGravityTerms();
/* Estimation de F */
F = m_qpp_mes-m_H.inverse()*(m_tau_FRI-m_c-m_g-m_pid);
//F = m_qpp_mes-m_H.inverse()*(m_tau_FRI+tau_dyn-m_c-m_g-m_pid);
estimateF(F);
//estimateAMAF(F);
/* Calcul des positions, vitesses et accelerations desirees */
m_time = m_time+m_dt; //Mise a jour du temps
nextDesVal();
/* Calcul du PID */
pidCalc();
/* Calcul de la commande */
m_tau_FRI.setZero(); //m_H*(m_qpp_des-F)+m_c+m_g+m_pid;
//m_tau_FRI = m_H*(m_qpp_des-F)+m_c+m_g-tau_dyn+m_pid;
if(m_writeInFile)
{
m_flux << std::endl;
m_flux << "temps : " << m_time << std::endl;
m_flux << "q_des : " << m_q_des.transpose() << std::endl;
m_flux << "q_mes : " << m_q_mes.transpose() << std::endl;
m_flux << "qp_des : " << m_qp_des.transpose() << std::endl;
m_flux << "qp_mes : " << m_qp_mes.transpose() << std::endl;
m_flux << "qpp_des : " << m_qpp_des.transpose() << std::endl;
m_flux << "qpp_mes : " << m_qpp_mes.transpose() << std::endl;
//m_flux << "F_est_sat : " << F.transpose() << std::endl;
//m_flux << "PID : " << m_pid.transpose() << std::endl;
//m_flux << "Hqpp : " << (m_H*m_qpp_des).transpose() << std::endl;
//m_flux << "coriolis : " << m_c.transpose() << std::endl;
//m_flux << "gravity : " << m_g.transpose() << std::endl;
m_flux << "tau_FRI : " << m_tau_FRI.transpose() << std::endl;
m_flux << "f_dynamics : " << tau_dyn.transpose() << std::endl;
m_flux << "gravity : " << m_g.transpose() << std::endl;
//m_flux << "tau_mes : " << m_tau_mes.transpose() << std::endl;
m_fluxTrq << std::endl << m_tau_mes.transpose() << ";...";
m_fluxErr << std::endl << (m_q_des-m_q_mes).transpose() << ";...";
}
for(unsigned int i=0;i<LWRDOF;i++){
tau_FRI[i] = m_tau_FRI[i];
}
oport_add_joint_trq.write(tau_FRI);
oport_joint_position.write(jntPos);
}
else{
m_compteur++;
}
}
}
