Make IkParameterization Get/SetValues Template

cielavenir commented 10 months ago

With this patch, IkParameterization GetValues/SetValues can accept anything that behaves as iterator, including vector/C-array/array/span.

GraspingCommon extracts std::vector<dReal> from std::vector<unsigned char> by hacking stdc++, which is not universal.

Actually there are some other functions whose arg is vector::iterator, but I have not touch those as template functions have to be inline.

cielavenir commented 10 months ago

AutoTester Pipeline #632835

cielavenir commented 10 months ago

The feature itself has been tested by the following C++ (with bash bootstrap)

About graspingcommon, unfortunately,

std::span is C++20
boost::beast::span::begin is const_iterator
boost::span is shipped with Boost 1.78

so we would need to stick with raw pointer...

//usr/bin/env true; tmpfile=$(mktemp)
//usr/bin/env true; srcdir=$(dirname $(realpath "$0"))
//usr/bin/env true; if [ ! -f $srcdir/span.hpp ]; then wget -q -O $srcdir/span.hpp https://github.com/boostorg/core/raw/boost-1.78.0/include/boost/core/span.hpp; fi
//usr/bin/env true; g++ -O2 -I $MUJIN_INSTALL_DIR/include -I $MUJIN_INSTALL_DIR/include/openrave-* -L $MUJIN_INSTALL_DIR/lib -xc++ -o $tmpfile $0 -llog4cxx && $tmpfile "$@"; rm $tmpfile; exit

#include <openrave/openrave.h>
#include <iostream>

#include <vector>
#include <array>

// #if __cplusplus >= 202002L
// #include <span>
// using std::span;
// #else
#include "span.hpp"
using boost::span;
// #endif

namespace OpenRAVE {
    // some definition to avoid linking openrave while using headers
    dReal RaveSqrt(dReal f){return sqrt(f);}

    // partly copied from https://github.com/rdiankov/openrave/blob/tw2019_production/include/openrave/openravejson.h
    OpenRAVEException::OpenRAVEException(const std::string& s, OpenRAVEErrorCode error) : std::exception() {
        _error = error;
        _s = "openrave (";
        _s += "): ";
        _s += s;
    }
    char const* OpenRAVEException::what() const throw()   {return _s.c_str();}
    const std::string& OpenRAVEException::message() const {return _s;}
    OpenRAVEErrorCode OpenRAVEException::GetCode() const  {return _error;}

    int RaveGetDebugLevel()            {return Level_Info;}
    log4cxx::LoggerPtr RaveGetLogger() {return log4cxx::Logger::getLogger("openrave");}
}

using OpenRAVE::dReal;
using OpenRAVE::IkParameterization;
using OpenRAVE::Transform;

IkParameterization CopyByVector(const IkParameterization &src){
    IkParameterization dst;
    std::vector<dReal> v(7);
    src.GetValues(v.begin());
    dst.SetValues(v.cbegin(), src.GetType());
    return dst;
}

IkParameterization CopyByPointer(const IkParameterization &src){
    IkParameterization dst;
    dReal *v = new dReal[7];
    src.GetValues(v);
    dst.SetValues(v, src.GetType());
    delete []v;
    return dst;
}

IkParameterization CopyByArray(const IkParameterization &src){
    IkParameterization dst;
    std::array<dReal, 7> v;
    src.GetValues(v.begin());
    dst.SetValues(v.cbegin(), src.GetType());
    return dst;
}

IkParameterization CopyBySpan(const IkParameterization &src){
    IkParameterization dst;
    char buf[8*7];
    span<dReal> v((dReal*)buf, 7);
    src.GetValues(v.begin());
    dst.SetValues(v.begin(), src.GetType()); // cbegin is C++23
    return dst;
}

int main(){
    Transform t;
    t.rot.x = 0;
    t.rot.y = 1;
    t.trans.x = 1;
    t.trans.y = 2;
    t.trans.z = 3;
    IkParameterization src(t);

    {
        IkParameterization dst = CopyByVector(src);
        std::cout << (t == dst.GetTransform6D()) << std::endl;
    }
    {
        IkParameterization dst = CopyByPointer(src);
        std::cout << (t == dst.GetTransform6D()) << std::endl;
    }
    {
        IkParameterization dst = CopyByArray(src);
        std::cout << (t == dst.GetTransform6D()) << std::endl;
    }
    {
        IkParameterization dst = CopyBySpan(src);
        std::cout << (t == dst.GetTransform6D()) << std::endl;
    }
}

cielavenir commented 10 months ago

/cc @ntohge

(could you add him to the member?)

cielavenir commented 9 months ago

Neither robotics team nor testing team gave me clue, so I'm completely lost, but for iterator type, https://github.com/llvm/llvm-project/blob/main/libcxx/include/__algorithm/any_of.h and https://cplusplus.com/reference/algorithm/fill/ uses InputIterator/OutputIterator term, so I changed to that terms.

Please note that I'm still lost about T (value type) term. libcxx uses Tp term. If that two-letter name is unacceptable I will not be able to add new template function to mujin code base.

This is super-complicated problem so I will let product team (@felixvd) subscribe it as well.

cielavenir commented 9 months ago

Pipeline #635575

rdiankov commented 9 months ago

@cielavenir thanks~ yes, the T is not great, we should rename it to something better

rdiankov / openrave

Make IkParameterization Get/SetValues Template #1312