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