void HeadSampler::doLoop()
{
int ret1,ret2;
ret1 = head->getPositions(positions.data());
ret2 = head->getRefPositions(references.data());
positionsPort.Content()=positions;
#ifdef __HEAD_SAMPLER_VERBOSE__
static int count=0;
count++;
if (count%50==0)
{
count = 0;
int x = (int) (positions[0]+0.5);
int y = (int) (positions[1]+0.5);
fprintf(stderr, "current pos: %d %d\n", x, y);
}
#endif
if (dumpFile!=NULL)
{
for(int k=0;k<_nj;k++)
fprintf(dumpFile, "%lf\t", positions[k]);
for(int k=0;k<_nj;k++)
fprintf(dumpFile, "%lf\t", references[k]);
fprintf(dumpFile, "\n");
}
positionsPort.Write();
}
void _parseFile(char *path, myList &list, YVector &home)
{
YARPConfigFile cfg(path, "demo.cfg");
printf("%s\n", path);
int n = __nj;
int length;
home.Resize(__nj);
if (cfg.get("[POS]", "N=", &length)==YARP_FAIL)
printf("Error reading file\n");
cfg.get("[POS]", "Home=", home.data(),__nj);
YVector tmp(__nj);
for(int i = 1; i <= length; i++)
{
char k[80];
sprintf(k, "WayPoint%d=", i);
cfg.get("[POS]", k, tmp.data(),__nj);
list.push_back(tmp);
}
/// BEGIN DEBUG
printf("N=%d\n", length);
printf("Home: ");
for(int i = 1; i<=__nj; i++)
printf("%lf\t", home(i));
printf("\n");
myList::iterator it,end;
it = list.begin();
end = list.end();
while(it!=end)
{
for(int i = 1; i<=__nj; i++)
printf("%lf\t", (*it)(i));
printf("\n");
it++;
}
}
void YARPDebugUtils::print(const YVector &v, const char *prefix)
{
if (prefix != NULL)
ACE_OS::printf("%s ", prefix);
const int n = v.Length();
ACE_OS::printf("<");
if (n == 0)
{
ACE_OS::printf("empty");
}
else
{
int k = 1;
for(k = 1; k <= n-1; k++)
{
ACE_OS::printf("(%.3lf) ", v(k));
}
ACE_OS::printf("(%.3lf)", v(k));
}
ACE_OS::printf(">\n");
}
void _setPositions(YVector &v)
{
arm->setPositions(__nj, v.data());
}
void _setPositions(YVector &v)
{
arm->setPositions(v.Length(), v.data());
}
void
spmv(const char mode[],
const AlphaType& alpha,
const AMatrix& A,
const XVector& x,
const BetaType& beta,
const YVector& y,
const RANK_ONE)
{
#ifdef KOKKOS_HAVE_CXX11
// Make sure that both x and y have the same rank.
static_assert ((int) XVector::rank == (int) YVector::rank,
"KokkosBlas::spmv: Vector ranks do not match.");
// Make sure that x (and therefore y) is rank 1.
static_assert ((int) XVector::rank == 1, "KokkosBlas::spmv: "
"Both Vector inputs must have rank 1 or 2.");
// Make sure that y is non-const.
static_assert (Kokkos::Impl::is_same<typename YVector::value_type,
typename YVector::non_const_value_type>::value,
"KokkosBlas::spmv: Output Vector must be non-const.");
#else
// We prefer to use C++11 static_assert, because it doesn't give
// "unused typedef" warnings, like the constructs below do.
//
// Make sure that both x and y have the same rank.
typedef typename
Kokkos::Impl::StaticAssert<(int) XVector::rank == (int) YVector::rank>::type
Blas1_spmv_vector_ranks_do_not_match;
// Make sure that x (and therefore y) is rank 1.
typedef typename
Kokkos::Impl::StaticAssert<(int) XVector::rank == 1 >::type
Blas1_spmv_vector_rank_not_one;
#endif // KOKKOS_HAVE_CXX11
// Check compatibility of dimensions at run time.
if((mode[0]==NoTranspose[0])||(mode[0]==Conjugate[0])) {
if ((x.dimension_1 () != y.dimension_1 ()) ||
(static_cast<size_t> (A.numCols ()) > static_cast<size_t> (x.dimension_0 ())) ||
(static_cast<size_t> (A.numRows ()) > static_cast<size_t> (y.dimension_0 ()))) {
std::ostringstream os;
os << "KokkosBlas::spmv: Dimensions do not match: "
<< ", A: " << A.numRows () << " x " << A.numCols()
<< ", x: " << x.dimension_0 () << " x " << x.dimension_1()
<< ", y: " << y.dimension_0 () << " x " << y.dimension_1()
;
Kokkos::Impl::throw_runtime_exception (os.str ());
}
} else {
if ((x.dimension_1 () != y.dimension_1 ()) ||
(static_cast<size_t> (A.numCols ()) > static_cast<size_t> (y.dimension_0 ())) ||
(static_cast<size_t> (A.numRows ()) > static_cast<size_t> (x.dimension_0()))) {
std::ostringstream os;
os << "KokkosBlas::spmv: Dimensions do not match (transpose): "
<< ", A: " << A.numRows () << " x " << A.numCols()
<< ", x: " << x.dimension_0 () << " x " << x.dimension_1()
<< ", y: " << y.dimension_0 () << " x " << y.dimension_1()
;
Kokkos::Impl::throw_runtime_exception (os.str ());
}
}
typedef KokkosSparse::CrsMatrix<typename AMatrix::const_value_type,
typename AMatrix::non_const_ordinal_type,
typename AMatrix::device_type,
Kokkos::MemoryTraits<Kokkos::Unmanaged>,
typename AMatrix::const_size_type> AMatrix_Internal;
typedef Kokkos::View<typename XVector::const_value_type*,
typename XVector::array_layout,
typename XVector::device_type,
Kokkos::MemoryTraits<Kokkos::Unmanaged|Kokkos::RandomAccess> > XVector_Internal;
typedef Kokkos::View<typename YVector::non_const_value_type*,
typename YVector::array_layout,
typename YVector::device_type,
Kokkos::MemoryTraits<Kokkos::Unmanaged> > YVector_Internal;
AMatrix_Internal A_i = A;
XVector_Internal x_i = x;
YVector_Internal y_i = y;
return Impl::SPMV<typename AMatrix_Internal::value_type,
typename AMatrix_Internal::ordinal_type,
typename AMatrix_Internal::device_type,
typename AMatrix_Internal::memory_traits,
typename AMatrix_Internal::size_type,
typename XVector_Internal::value_type*,
typename XVector_Internal::array_layout,
typename XVector_Internal::device_type,
typename XVector_Internal::memory_traits,
typename YVector_Internal::value_type*,
typename YVector_Internal::array_layout,
typename YVector_Internal::device_type,
typename YVector_Internal::memory_traits>::spmv(mode,alpha,A,x,beta,y);
}
void
spmv(const char mode[],
const AlphaType& alpha_in,
const AMatrix& A,
const XVector& x,
const BetaType& beta_in,
const YVector& y,
const RANK_TWO) {
typedef typename Impl::GetCoeffView<AlphaType, typename XVector::device_type>::view_type alpha_view_type;
typedef typename Impl::GetCoeffView<BetaType, typename XVector::device_type>::view_type beta_view_type;
//alpha_view_type alpha = Impl::GetCoeffView<AlphaType, typename XVector::device_type>::get_view(alpha_in,x.dimension_1());
//beta_view_type beta = Impl::GetCoeffView<AlphaType, typename XVector::device_type>::get_view(beta_in, x.dimension_1());
#ifdef KOKKOS_HAVE_CXX11
// Make sure that both x and y have the same rank.
static_assert (XVector::rank == YVector::rank, "KokkosBlas::spmv: Vector ranks do not match.");
// Make sure that y is non-const.
static_assert (Kokkos::Impl::is_same<typename YVector::value_type,
typename YVector::non_const_value_type>::value,
"KokkosBlas::spmv: Output Vector must be non-const.");
#else
// We prefer to use C++11 static_assert, because it doesn't give
// "unused typedef" warnings, like the constructs below do.
//
// Make sure that both x and y have the same rank.
typedef typename
Kokkos::Impl::StaticAssert<XVector::rank == YVector::rank>::type Blas1_spmv_vector_ranks_do_not_match;
#endif // KOKKOS_HAVE_CXX11
// Check compatibility of dimensions at run time.
if((mode[0]==NoTranspose[0])||(mode[0]==Conjugate[0])) {
if ((x.dimension_1 () != y.dimension_1 ()) ||
(static_cast<size_t> (A.numCols ()) > static_cast<size_t> (x.dimension_0 ())) ||
(static_cast<size_t> (A.numRows ()) > static_cast<size_t> (y.dimension_0 ()))) {
std::ostringstream os;
os << "KokkosBlas::spmv: Dimensions do not match: "
<< ", A: " << A.numRows () << " x " << A.numCols()
<< ", x: " << x.dimension_0 () << " x " << x.dimension_1 ()
<< ", y: " << y.dimension_0 () << " x " << y.dimension_1 ();
Kokkos::Impl::throw_runtime_exception (os.str ());
}
} else {
if ((x.dimension_1 () != y.dimension_1 ()) ||
(static_cast<size_t> (A.numCols ()) > static_cast<size_t> (y.dimension_0 ())) ||
(static_cast<size_t> (A.numRows ()) > static_cast<size_t> (x.dimension_0 ()))) {
std::ostringstream os;
os << "KokkosBlas::spmv: Dimensions do not match (transpose): "
<< ", A: " << A.numRows () << " x " << A.numCols()
<< ", x: " << x.dimension_0 () << " x " << x.dimension_1 ()
<< ", y: " << y.dimension_0 () << " x " << y.dimension_1 ();
Kokkos::Impl::throw_runtime_exception (os.str ());
}
}
typedef KokkosSparse::CrsMatrix<typename AMatrix::const_value_type,
typename AMatrix::const_ordinal_type,
typename AMatrix::device_type,
typename AMatrix::memory_traits,
typename AMatrix::const_size_type> AMatrix_Internal;
AMatrix_Internal A_i = A;
// Call single vector version if appropriate
if( x.dimension_1() == 1) {
typedef Kokkos::View<typename XVector::const_value_type*,
typename Kokkos::Impl::if_c<Kokkos::Impl::is_same<typename YVector::array_layout,Kokkos::LayoutLeft>::value,
Kokkos::LayoutLeft,Kokkos::LayoutStride>::type,
typename XVector::device_type,
Kokkos::MemoryTraits<Kokkos::Unmanaged|Kokkos::RandomAccess> > XVector_SubInternal;
typedef Kokkos::View<typename YVector::non_const_value_type*,
typename Kokkos::Impl::if_c<Kokkos::Impl::is_same<typename YVector::array_layout,Kokkos::LayoutLeft>::value,
Kokkos::LayoutLeft,Kokkos::LayoutStride>::type,
typename YVector::device_type,
Kokkos::MemoryTraits<Kokkos::Unmanaged> > YVector_SubInternal;
XVector_SubInternal x_i = Kokkos::subview(x,Kokkos::ALL(),0);
YVector_SubInternal y_i = Kokkos::subview(y,Kokkos::ALL(),0);
alpha_view_type alpha = Impl::GetCoeffView<AlphaType, typename XVector::device_type>::get_view(alpha_in,x.dimension_1());
beta_view_type beta = Impl::GetCoeffView<BetaType, typename XVector::device_type>::get_view(beta_in, x.dimension_1());
typename alpha_view_type::non_const_type::HostMirror h_alpha =
Kokkos::create_mirror_view(alpha);
Kokkos::deep_copy(h_alpha,alpha);
typename beta_view_type::non_const_type::HostMirror h_beta =
Kokkos::create_mirror_view(beta);
Kokkos::deep_copy(h_beta,beta);
spmv(mode,h_alpha(0),A,x_i,h_beta(0),y_i);
return;
} else {
typedef Kokkos::View<typename XVector::const_value_type**,
typename XVector::array_layout,
typename XVector::device_type,
Kokkos::MemoryTraits<Kokkos::Unmanaged|Kokkos::RandomAccess> > XVector_Internal;
typedef Kokkos::View<typename YVector::non_const_value_type**,
typename YVector::array_layout,
typename YVector::device_type,
Kokkos::MemoryTraits<Kokkos::Unmanaged> > YVector_Internal;
XVector_Internal x_i = x;
YVector_Internal y_i = y;
typedef Kokkos::View<typename alpha_view_type::const_value_type*,
typename alpha_view_type::array_layout,
typename alpha_view_type::device_type,
Kokkos::MemoryTraits<Kokkos::Unmanaged> > alpha_view_type_Internal;
typedef Kokkos::View<typename beta_view_type::const_value_type*,
typename beta_view_type::array_layout,
typename beta_view_type::device_type,
Kokkos::MemoryTraits<Kokkos::Unmanaged> > beta_view_type_Internal;
//alpha_view_type_Internal alpha_c = alpha;
//beta_view_type_Internal beta_c = beta;
return Impl::SPMV_MV<typename alpha_view_type_Internal::value_type*,
typename alpha_view_type_Internal::array_layout,
typename alpha_view_type_Internal::device_type,
typename alpha_view_type_Internal::memory_traits,
typename AMatrix_Internal::value_type,
typename AMatrix_Internal::ordinal_type,
typename AMatrix_Internal::device_type,
typename AMatrix_Internal::memory_traits,
typename AMatrix_Internal::size_type,
typename XVector_Internal::value_type**,
typename XVector_Internal::array_layout,
typename XVector_Internal::device_type,
typename XVector_Internal::memory_traits,
typename beta_view_type_Internal::value_type*,
typename beta_view_type_Internal::array_layout,
typename beta_view_type_Internal::device_type,
typename beta_view_type_Internal::memory_traits,
typename YVector_Internal::value_type**,
typename YVector_Internal::array_layout,
typename YVector_Internal::device_type,
typename YVector_Internal::memory_traits>::spmv_mv(mode,alpha_in,A,x,beta_in,y);
}
}