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