void KukaRMControllerRTNET::nextDesVal() { Eigen::VectorXd period = m_freq*m_time; Eigen::VectorXd amp = Eigen::VectorXd::Zero(LWRDOF); if(m_time<1.0){ amp = m_amp*m_time; } else{ amp = m_amp; } for(unsigned int i=0;i<LWRDOF;i++) { m_q_des(i) = amp(i)*(period.array().sin())(i)+m_bias(i); m_qp_des(i) = amp(i)*m_freq(i)*(period.array().cos())(i); m_qpp_des(i) = -amp(i)*m_freq(i)*m_freq(i)*(period.array().sin())(i); } }
void NETLIB_NAME(solver)::post_start() { std::vector<analog_net_t::list_t> groups; const bool use_specific = true; m_params.m_pivot = m_pivot(); m_params.m_accuracy = m_accuracy(); /* FIXME: Throw when negative */ m_params.m_gs_loops = static_cast<unsigned>(m_gs_loops()); m_params.m_nr_loops = static_cast<unsigned>(m_nr_loops()); m_params.m_nt_sync_delay = netlist_time::from_double(m_sync_delay()); m_params.m_lte = m_lte(); m_params.m_sor = m_sor(); m_params.m_min_timestep = m_min_timestep(); m_params.m_dynamic = (m_dynamic() == 1 ? true : false); m_params.m_max_timestep = netlist_time::from_double(1.0 / m_freq()).as_double(); if (m_params.m_dynamic) { m_params.m_max_timestep *= 1;//NL_FCONST(1000.0); } else { m_params.m_min_timestep = m_params.m_max_timestep; } //m_params.m_max_timestep = std::max(m_params.m_max_timestep, m_params.m_max_timestep::) // Override log statistics pstring p = plib::util::environment("NL_STATS"); if (p != "") m_params.m_log_stats = p.as_long(); else m_params.m_log_stats = m_log_stats(); netlist().log().verbose("Scanning net groups ..."); // determine net groups for (auto & net : netlist().m_nets) { netlist().log().debug("processing {1}\n", net->name()); if (!net->isRailNet()) { netlist().log().debug(" ==> not a rail net\n"); /* Must be an analog net */ analog_net_t *n = static_cast<analog_net_t *>(net.get()); if (!n->already_processed(groups)) { groups.push_back(analog_net_t::list_t()); n->process_net(groups); } } } // setup the solvers netlist().log().verbose("Found {1} net groups in {2} nets\n", groups.size(), netlist().m_nets.size()); for (auto & grp : groups) { std::unique_ptr<matrix_solver_t> ms; unsigned net_count = static_cast<unsigned>(grp.size()); switch (net_count) { case 1: ms = create_solver<1,1>(1, use_specific); break; case 2: ms = create_solver<2,2>(2, use_specific); break; case 3: ms = create_solver<3,3>(3, use_specific); break; case 4: ms = create_solver<4,4>(4, use_specific); break; case 5: ms = create_solver<5,5>(5, use_specific); break; case 6: ms = create_solver<6,6>(6, use_specific); break; case 7: ms = create_solver<7,7>(7, use_specific); break; case 8: ms = create_solver<8,8>(8, use_specific); break; case 10: ms = create_solver<10,10>(10, use_specific); break; case 11: ms = create_solver<11,11>(11, use_specific); break; case 12: ms = create_solver<12,12>(12, use_specific); break; case 15: ms = create_solver<15,15>(15, use_specific); break; case 31: ms = create_solver<31,31>(31, use_specific); break; case 49: ms = create_solver<49,49>(49, use_specific); break; #if 0 case 87: ms = create_solver<87,87>(87, use_specific); break; #endif default: netlist().log().warning("No specific solver found for netlist of size {1}", net_count); if (net_count <= 16) { ms = create_solver<0,16>(net_count, use_specific); } else if (net_count <= 32) { ms = create_solver<0,32>(net_count, use_specific); } else if (net_count <= 64) { ms = create_solver<0,64>(net_count, use_specific); } else if (net_count <= 128) { ms = create_solver<0,128>(net_count, use_specific); } else { netlist().log().fatal("Encountered netgroup with > 128 nets"); ms = nullptr; /* tease compilers */ } break; } // FIXME ... ms->set_delegate_pointer(); ms->setup(grp); netlist().log().verbose("Solver {1}", ms->name()); netlist().log().verbose(" ==> {2} nets", grp.size()); netlist().log().verbose(" has {1} elements", ms->has_dynamic_devices() ? "dynamic" : "no dynamic"); netlist().log().verbose(" has {1} elements", ms->has_timestep_devices() ? "timestep" : "no timestep"); for (auto &n : grp) { netlist().log().verbose("Net {1}", n->name()); for (const auto &pcore : n->m_core_terms) { netlist().log().verbose(" {1}", pcore->name()); } } m_mat_solvers.push_back(std::move(ms)); } }