diff --git a/unitree_guide/CMakeLists.txt b/unitree_guide/CMakeLists.txt index 4779d83..49f75af 100755 --- a/unitree_guide/CMakeLists.txt +++ b/unitree_guide/CMakeLists.txt @@ -143,4 +143,38 @@ target_link_libraries(junior_ctrl -pthread lcm) if(NOT CATKIN_MAKE) set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -endif() \ No newline at end of file +endif() + +# PlotTest +add_executable(myTest src/test.cpp ${SRC_LIST}) +if (CATKIN_MAKE) + target_link_libraries(myTest ${catkin_LIBRARIES}) + add_dependencies(myTest ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +endif () +find_package(Python3 COMPONENTS Interpreter Development REQUIRED) +target_link_libraries(myTest + Python3::Python + Python3::Module +) +find_package(Python3 COMPONENTS NumPy) +if(Python3_NumPy_FOUND) + target_link_libraries(myTest + Python3::NumPy + ) +else() + target_compile_definitions(myTest WITHOUT_NUMPY) +endif() +if(REAL_ROBOT) + if(${ROBOT_TYPE} STREQUAL "A1") + if(${PLATFORM} STREQUAL "amd64") + target_link_libraries(myTest libunitree_legged_sdk_amd64.so) + elseif(${PLATFORM} STREQUAL "arm64") + target_link_libraries(myTest libunitree_legged_sdk_arm64.so) + endif() + elseif(${ROBOT_TYPE} STREQUAL "Go1") + target_link_libraries(myTest libunitree_legged_sdk.a) + endif() +endif() +target_link_libraries(myTest -pthread lcm) + + diff --git a/unitree_guide/include/thirdParty/matplotlibcpp.h b/unitree_guide/include/thirdParty/matplotlibcpp.h index e017e47..d95d46a 100755 --- a/unitree_guide/include/thirdParty/matplotlibcpp.h +++ b/unitree_guide/include/thirdParty/matplotlibcpp.h @@ -13,6 +13,7 @@ #include #include // requires c++11 support #include +#include // std::stod #ifndef WITHOUT_NUMPY # define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION @@ -74,6 +75,7 @@ struct _interpreter { PyObject *s_python_function_ylim; PyObject *s_python_function_title; PyObject *s_python_function_axis; + PyObject *s_python_function_axhline; PyObject *s_python_function_axvline; PyObject *s_python_function_axvspan; PyObject *s_python_function_xlabel; @@ -99,7 +101,8 @@ struct _interpreter { PyObject *s_python_function_barh; PyObject *s_python_function_colorbar; PyObject *s_python_function_subplots_adjust; - + PyObject *s_python_function_rcparams; + PyObject *s_python_function_spy; /* For now, _interpreter is implemented as a singleton since its currently not possible to have multiple independent embedded python interpreters without patching the python source code @@ -174,10 +177,12 @@ struct _interpreter { wchar_t const *dummy_args[] = {L"Python", NULL}; // const is needed because literals must not be modified wchar_t const **argv = dummy_args; int argc = sizeof(dummy_args)/sizeof(dummy_args[0])-1; - // Python3: - // PySys_SetArgv(argc, const_cast(argv)); - // Python2: - char** argm = (char **)(argv); PySys_SetArgv(argc, argm); + +#if PY_MAJOR_VERSION >= 3 + PySys_SetArgv(argc, const_cast(argv)); +#else + PySys_SetArgv(argc, (char **)(argv)); +#endif #ifndef WITHOUT_NUMPY import_numpy(); // initialize numpy C-API @@ -192,6 +197,7 @@ struct _interpreter { } PyObject* matplotlib = PyImport_Import(matplotlibname); + Py_DECREF(matplotlibname); if (!matplotlib) { PyErr_Print(); @@ -204,6 +210,8 @@ struct _interpreter { PyObject_CallMethod(matplotlib, const_cast("use"), const_cast("s"), s_backend.c_str()); } + + PyObject* pymod = PyImport_Import(pyplotname); Py_DECREF(pyplotname); if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } @@ -237,9 +245,11 @@ struct _interpreter { s_python_function_subplot = safe_import(pymod, "subplot"); s_python_function_subplot2grid = safe_import(pymod, "subplot2grid"); s_python_function_legend = safe_import(pymod, "legend"); + s_python_function_xlim = safe_import(pymod, "xlim"); s_python_function_ylim = safe_import(pymod, "ylim"); s_python_function_title = safe_import(pymod, "title"); s_python_function_axis = safe_import(pymod, "axis"); + s_python_function_axhline = safe_import(pymod, "axhline"); s_python_function_axvline = safe_import(pymod, "axvline"); s_python_function_axvspan = safe_import(pymod, "axvspan"); s_python_function_xlabel = safe_import(pymod, "xlabel"); @@ -250,7 +260,6 @@ struct _interpreter { s_python_function_margins = safe_import(pymod, "margins"); s_python_function_tick_params = safe_import(pymod, "tick_params"); s_python_function_grid = safe_import(pymod, "grid"); - s_python_function_xlim = safe_import(pymod, "xlim"); s_python_function_ion = safe_import(pymod, "ion"); s_python_function_ginput = safe_import(pymod, "ginput"); s_python_function_save = safe_import(pylabmod, "savefig"); @@ -267,6 +276,8 @@ struct _interpreter { s_python_function_barh = safe_import(pymod, "barh"); s_python_function_colorbar = PyObject_GetAttrString(pymod, "colorbar"); s_python_function_subplots_adjust = safe_import(pymod,"subplots_adjust"); + s_python_function_rcparams = PyObject_GetAttrString(pymod, "rcParams"); + s_python_function_spy = PyObject_GetAttrString(pymod, "spy"); #ifndef WITHOUT_NUMPY s_python_function_imshow = safe_import(pymod, "imshow"); #endif @@ -343,7 +354,6 @@ static_assert(sizeof(long long) == 8); template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; static_assert(sizeof(unsigned long long) == 8); template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; -// TODO: add int, long, etc. template PyObject* get_array(const std::vector& v) @@ -359,7 +369,7 @@ PyObject* get_array(const std::vector& v) PyArray_UpdateFlags(reinterpret_cast(varray), NPY_ARRAY_OWNDATA); return varray; } - + PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data())); return varray; } @@ -426,7 +436,7 @@ PyObject* get_listlist(const std::vector>& ll) } // namespace detail /// Plot a line through the given x and y data points.. -/// +/// /// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html template bool plot(const std::vector &x, const std::vector &y, const std::map& keywords) @@ -468,7 +478,8 @@ void plot_surface(const std::vector<::std::vector> &x, const std::vector<::std::vector> &y, const std::vector<::std::vector> &z, const std::map &keywords = - std::map()) + std::map(), + const long fig_number=0) { detail::_interpreter::get(); @@ -519,14 +530,29 @@ void plot_surface(const std::vector<::std::vector> &x, for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), - PyString_FromString(it->second.c_str())); + if (it->first == "linewidth" || it->first == "alpha") { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyFloat_FromDouble(std::stod(it->second))); + } else { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } } - - PyObject *fig = - PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, - detail::_interpreter::get().s_python_empty_tuple); + PyObject *fig_args = PyTuple_New(1); + PyObject* fig = nullptr; + PyTuple_SetItem(fig_args, 0, PyLong_FromLong(fig_number)); + PyObject *fig_exists = + PyObject_CallObject( + detail::_interpreter::get().s_python_function_fignum_exists, fig_args); + if (!PyObject_IsTrue(fig_exists)) { + fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + } else { + fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + fig_args); + } + Py_DECREF(fig_exists); if (!fig) throw std::runtime_error("Call to figure() failed."); PyObject *gca_kwargs = PyDict_New(); @@ -556,6 +582,78 @@ void plot_surface(const std::vector<::std::vector> &x, Py_DECREF(kwargs); if (res) Py_DECREF(res); } + +template +void contour(const std::vector<::std::vector> &x, + const std::vector<::std::vector> &y, + const std::vector<::std::vector> &z, + const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + // using numpy arrays + PyObject *xarray = detail::get_2darray(x); + PyObject *yarray = detail::get_2darray(y); + PyObject *zarray = detail::get_2darray(z); + + // construct positional args + PyObject *args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + PyTuple_SetItem(args, 2, zarray); + + // Build up the kw args. + PyObject *kwargs = PyDict_New(); + + PyObject *python_colormap_coolwarm = PyObject_GetAttrString( + detail::_interpreter::get().s_python_colormap, "coolwarm"); + + PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm); + + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_contour, args, kwargs); + if (!res) + throw std::runtime_error("failed contour"); + + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} + +template +void spy(const std::vector<::std::vector> &x, + const double markersize = -1, // -1 for default matplotlib size + const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + PyObject *xarray = detail::get_2darray(x); + + PyObject *kwargs = PyDict_New(); + if (markersize != -1) { + PyDict_SetItemString(kwargs, "markersize", PyFloat_FromDouble(markersize)); + } + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + PyObject *plot_args = PyTuple_New(1); + PyTuple_SetItem(plot_args, 0, xarray); + + PyObject *res = PyObject_Call( + detail::_interpreter::get().s_python_function_spy, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} #endif // WITHOUT_NUMPY template @@ -563,13 +661,14 @@ void plot3(const std::vector &x, const std::vector &y, const std::vector &z, const std::map &keywords = - std::map()) + std::map(), + const long fig_number=0) { detail::_interpreter::get(); - // Same as with plot_surface: We lazily load the modules here the first time - // this function is called because I'm not sure that we can assume "matplotlib - // installed" implies "mpl_toolkits installed" on all platforms, and we don't + // Same as with plot_surface: We lazily load the modules here the first time + // this function is called because I'm not sure that we can assume "matplotlib + // installed" implies "mpl_toolkits installed" on all platforms, and we don't // want to require it for people who don't need 3d plots. static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; if (!mpl_toolkitsmod) { @@ -610,9 +709,18 @@ void plot3(const std::vector &x, PyString_FromString(it->second.c_str())); } - PyObject *fig = - PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, - detail::_interpreter::get().s_python_empty_tuple); + PyObject *fig_args = PyTuple_New(1); + PyObject* fig = nullptr; + PyTuple_SetItem(fig_args, 0, PyLong_FromLong(fig_number)); + PyObject *fig_exists = + PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, fig_args); + if (!PyObject_IsTrue(fig_exists)) { + fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + } else { + fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + fig_args); + } if (!fig) throw std::runtime_error("Call to figure() failed."); PyObject *gca_kwargs = PyDict_New(); @@ -914,6 +1022,141 @@ bool scatter(const std::vector& x, return res; } +template + bool scatter_colored(const std::vector& x, + const std::vector& y, + const std::vector& colors, + const double s=1.0, // The marker size in points**2 + const std::map & keywords = {}) + { + detail::_interpreter::get(); + + assert(x.size() == y.size()); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + PyObject* colors_array = detail::get_array(colors); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s)); + PyDict_SetItemString(kwargs, "c", colors_array); + + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; + } + + +template +bool scatter(const std::vector& x, + const std::vector& y, + const std::vector& z, + const double s=1.0, // The marker size in points**2 + const std::map & keywords = {}, + const long fig_number=0) { + detail::_interpreter::get(); + + // Same as with plot_surface: We lazily load the modules here the first time + // this function is called because I'm not sure that we can assume "matplotlib + // installed" implies "mpl_toolkits installed" on all platforms, and we don't + // want to require it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + detail::_interpreter::get(); + + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + assert(x.size() == y.size()); + assert(y.size() == z.size()); + + PyObject *xarray = detail::get_array(x); + PyObject *yarray = detail::get_array(y); + PyObject *zarray = detail::get_array(z); + + // construct positional args + PyObject *args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + PyTuple_SetItem(args, 2, zarray); + + // Build up the kw args. + PyObject *kwargs = PyDict_New(); + + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + PyObject *fig_args = PyTuple_New(1); + PyObject* fig = nullptr; + PyTuple_SetItem(fig_args, 0, PyLong_FromLong(fig_number)); + PyObject *fig_exists = + PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, fig_args); + if (!PyObject_IsTrue(fig_exists)) { + fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + } else { + fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + fig_args); + } + Py_DECREF(fig_exists); + if (!fig) throw std::runtime_error("Call to figure() failed."); + + PyObject *gca_kwargs = PyDict_New(); + PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); + + PyObject *gca = PyObject_GetAttrString(fig, "gca"); + if (!gca) throw std::runtime_error("No gca"); + Py_INCREF(gca); + PyObject *axis = PyObject_Call( + gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); + + if (!axis) throw std::runtime_error("No axis"); + Py_INCREF(axis); + + Py_DECREF(gca); + Py_DECREF(gca_kwargs); + + PyObject *plot3 = PyObject_GetAttrString(axis, "scatter"); + if (!plot3) throw std::runtime_error("No 3D line plot"); + Py_INCREF(plot3); + PyObject *res = PyObject_Call(plot3, args, kwargs); + if (!res) throw std::runtime_error("Failed 3D line plot"); + Py_DECREF(plot3); + + Py_DECREF(axis); + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(fig); + if (res) Py_DECREF(res); + return res; + +} + template bool boxplot(const std::vector>& data, const std::vector& labels = {}, @@ -1142,9 +1385,9 @@ bool contour(const std::vector& x, const std::vector& y, const std::map& keywords = {}) { assert(x.size() == y.size() && x.size() == z.size()); - PyObject* xarray = get_array(x); - PyObject* yarray = get_array(y); - PyObject* zarray = get_array(z); + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + PyObject* zarray = detail::get_array(z); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); @@ -1205,6 +1448,92 @@ bool quiver(const std::vector& x, const std::vector& y, cons return res; } +template +bool quiver(const std::vector& x, const std::vector& y, const std::vector& z, const std::vector& u, const std::vector& w, const std::vector& v, const std::map& keywords = {}) +{ + //set up 3d axes stuff + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + detail::_interpreter::get(); + + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + //assert sizes match up + assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size() && x.size() == z.size() && x.size() == v.size() && u.size() == v.size()); + + //set up parameters + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + PyObject* zarray = detail::get_array(z); + PyObject* uarray = detail::get_array(u); + PyObject* warray = detail::get_array(w); + PyObject* varray = detail::get_array(v); + + PyObject* plot_args = PyTuple_New(6); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, zarray); + PyTuple_SetItem(plot_args, 3, uarray); + PyTuple_SetItem(plot_args, 4, warray); + PyTuple_SetItem(plot_args, 5, varray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + //get figure gca to enable 3d projection + PyObject *fig = + PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + if (!fig) throw std::runtime_error("Call to figure() failed."); + + PyObject *gca_kwargs = PyDict_New(); + PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); + + PyObject *gca = PyObject_GetAttrString(fig, "gca"); + if (!gca) throw std::runtime_error("No gca"); + Py_INCREF(gca); + PyObject *axis = PyObject_Call( + gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); + + if (!axis) throw std::runtime_error("No axis"); + Py_INCREF(axis); + Py_DECREF(gca); + Py_DECREF(gca_kwargs); + + //plot our boys bravely, plot them strongly, plot them with a wink and clap + PyObject *plot3 = PyObject_GetAttrString(axis, "quiver"); + if (!plot3) throw std::runtime_error("No 3D line plot"); + Py_INCREF(plot3); + PyObject* res = PyObject_Call( + plot3, plot_args, kwargs); + if (!res) throw std::runtime_error("Failed 3D plot"); + Py_DECREF(plot3); + Py_DECREF(axis); + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + template bool stem(const std::vector& x, const std::vector& y, const std::string& s = "") { @@ -1370,8 +1699,8 @@ bool named_plot(const std::string& name, const std::vector& y, const st return res; } -template -bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +template +bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); @@ -1397,8 +1726,8 @@ bool named_plot(const std::string& name, const std::vector& x, const st return res; } -template -bool named_semilogx(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +template +bool named_semilogx(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); @@ -1424,8 +1753,8 @@ bool named_semilogx(const std::string& name, const std::vector& x, cons return res; } -template -bool named_semilogy(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +template +bool named_semilogy(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); @@ -1451,8 +1780,8 @@ bool named_semilogy(const std::string& name, const std::vector& x, cons return res; } -template -bool named_loglog(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +template +bool named_loglog(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); @@ -1637,7 +1966,63 @@ inline void legend(const std::map& keywords) if(!res) throw std::runtime_error("Call to legend() failed."); Py_DECREF(kwargs); - Py_DECREF(res); + Py_DECREF(res); +} + +template +inline void set_aspect(Numeric ratio) +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(ratio)); + PyObject* kwargs = PyDict_New(); + + PyObject *ax = + PyObject_CallObject(detail::_interpreter::get().s_python_function_gca, + detail::_interpreter::get().s_python_empty_tuple); + if (!ax) throw std::runtime_error("Call to gca() failed."); + Py_INCREF(ax); + + PyObject *set_aspect = PyObject_GetAttrString(ax, "set_aspect"); + if (!set_aspect) throw std::runtime_error("Attribute set_aspect not found."); + Py_INCREF(set_aspect); + + PyObject *res = PyObject_Call(set_aspect, args, kwargs); + if (!res) throw std::runtime_error("Call to set_aspect() failed."); + Py_DECREF(set_aspect); + + Py_DECREF(ax); + Py_DECREF(args); + Py_DECREF(kwargs); +} + +inline void set_aspect_equal() +{ + // expect ratio == "equal". Leaving error handling to matplotlib. + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyString_FromString("equal")); + PyObject* kwargs = PyDict_New(); + + PyObject *ax = + PyObject_CallObject(detail::_interpreter::get().s_python_function_gca, + detail::_interpreter::get().s_python_empty_tuple); + if (!ax) throw std::runtime_error("Call to gca() failed."); + Py_INCREF(ax); + + PyObject *set_aspect = PyObject_GetAttrString(ax, "set_aspect"); + if (!set_aspect) throw std::runtime_error("Attribute set_aspect not found."); + Py_INCREF(set_aspect); + + PyObject *res = PyObject_Call(set_aspect, args, kwargs); + if (!res) throw std::runtime_error("Call to set_aspect() failed."); + Py_DECREF(set_aspect); + + Py_DECREF(ax); + Py_DECREF(args); + Py_DECREF(kwargs); } template @@ -1679,43 +2064,33 @@ void xlim(Numeric left, Numeric right) } -inline double* xlim() +inline std::array xlim() { - detail::_interpreter::get(); - PyObject* args = PyTuple_New(0); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); - PyObject* left = PyTuple_GetItem(res,0); - PyObject* right = PyTuple_GetItem(res,1); - - double* arr = new double[2]; - arr[0] = PyFloat_AsDouble(left); - arr[1] = PyFloat_AsDouble(right); if(!res) throw std::runtime_error("Call to xlim() failed."); Py_DECREF(res); - return arr; + + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + return { PyFloat_AsDouble(left), PyFloat_AsDouble(right) }; } -inline double* ylim() +inline std::array ylim() { - detail::_interpreter::get(); - PyObject* args = PyTuple_New(0); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); - PyObject* left = PyTuple_GetItem(res,0); - PyObject* right = PyTuple_GetItem(res,1); - - double* arr = new double[2]; - arr[0] = PyFloat_AsDouble(left); - arr[1] = PyFloat_AsDouble(right); if(!res) throw std::runtime_error("Call to ylim() failed."); Py_DECREF(res); - return arr; + + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + return { PyFloat_AsDouble(left), PyFloat_AsDouble(right) }; } template @@ -1877,7 +2252,7 @@ inline void tick_params(const std::map& keywords, cons inline void subplot(long nrows, long ncols, long plot_number) { detail::_interpreter::get(); - + // construct positional args PyObject* args = PyTuple_New(3); PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows)); @@ -1942,7 +2317,7 @@ inline void title(const std::string &titlestr, const std::map &keywords = {}) { detail::_interpreter::get(); - + PyObject* pysuptitlestr = PyString_FromString(suptitlestr.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pysuptitlestr); @@ -1975,6 +2350,31 @@ inline void axis(const std::string &axisstr) Py_DECREF(res); } +inline void axhline(double y, double xmin = 0., double xmax = 1., const std::map& keywords = std::map()) +{ + detail::_interpreter::get(); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(y)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmin)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(xmax)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axhline, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); +} + inline void axvline(double x, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) { detail::_interpreter::get(); @@ -2011,12 +2411,14 @@ inline void axvspan(double xmin, double xmax, double ymin = 0., double ymax = 1. // construct keyword args PyObject* kwargs = PyDict_New(); - for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) - { - if (it->first == "linewidth" || it->first == "alpha") - PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(std::stod(it->second))); - else - PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + if (it->first == "linewidth" || it->first == "alpha") { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyFloat_FromDouble(std::stod(it->second))); + } else { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvspan, args, kwargs); @@ -2072,9 +2474,9 @@ inline void set_zlabel(const std::string &str, const std::map 0) + { + PyDict_SetItemString(kwargs, "dpi", PyLong_FromLong(dpi)); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_save, args, kwargs); if (!res) throw std::runtime_error("Call to save() failed."); Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void rcparams(const std::map& keywords = {}) { + detail::_interpreter::get(); + PyObject* args = PyTuple_New(0); + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + if ("text.usetex" == it->first) + PyDict_SetItemString(kwargs, it->first.c_str(), PyLong_FromLong(std::stoi(it->second.c_str()))); + else PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject * update = PyObject_GetAttrString(detail::_interpreter::get().s_python_function_rcparams, "update"); + PyObject * res = PyObject_Call(update, args, kwargs); + if(!res) throw std::runtime_error("Call to rcParams.update() failed."); + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(update); Py_DECREF(res); } diff --git a/unitree_guide/src/test.cpp b/unitree_guide/src/test.cpp new file mode 100644 index 0000000..feaf03b --- /dev/null +++ b/unitree_guide/src/test.cpp @@ -0,0 +1,412 @@ +#include "control/BalanceCtrl.h" +#include "common/timeMarker.h" +#include "common/unitreeRobot.h" +#include "common/PyPlot.h" +#include "Gait/WaveGenerator.h" +#include "common/mathTools.h" +#include "control/Estimator.h" + +/********************************/ +/***** Test *****/ +/********************************/ +// PyPlot多个静态图测试 +int main(){ + PyPlot plot; + plot.addPlot("test", 1, std::vector{"y"}); // 输入标量 + plot.addPlot("testArray", 3, std::vector{"y1", "y2", "y3"}); // 输入数组 + plot.addPlot("testEigen", 4, std::vector{"y1", "y2", "y3", "y4"}); // 输入Eigen格式的向量 + plot.addPlot("testVector", 5); // 输入Vector格式的向量，如果不指定各个物理量的名称，则默认用数字依次编号 + + float array[3]; + Eigen::Matrix eigenVec; eigenVec.setZero(); + std::vector vec(5); + + for(int i(0); i < 50; ++i){ + plot.addFrame("test", i, i+1); // 给对应名称的Plot输入数据，三个参数分别为图名、x轴坐标，数据值。输入数据的类型可以为标量、数组、Eigen格式的向量、Vector格式的向量 + + array[0] = i-10; + array[1] = i; + array[2] = i+10; + plot.addFrame("testArray", i, array); + + eigenVec << i-10, i, i+10, i+20; + plot.addFrame("testEigen", eigenVec); // 不给定x轴坐标的话，可以默认以运行该函数的时间为x轴坐标 + + vec[0] = i-20; + vec[1] = i-10; + vec[2] = i; + vec[3] = i+10; + vec[4] = i+20; + plot.addFrame("testVector", vec); + + usleep(100); + } + + // plot.showPlot("test"); // 显示某个Plot + // plot.showPlot("testArray"); + // plot.showPlot(std::vector{"test", "testVector"}); // 显示多个Plot + plot.showPlotAll(); // 显示全部Plot +} + + +// /*pyPlot时间对齐*/ +// int main(){ +// PyPlot plt; +// plt.addPlot("lead", 1); +// plt.addPlot("lag", 1); + +// for(int i(0); i<1000; ++i){ +// plt.addFrame("lead", i); +// if(i>500){ +// plt.addFrame("lag", i); +// } +// usleep(1000); +// } + +// plt.showPlotAll(); +// } + +// /* Eigen, Matrix - Vector */ +// int main(){ +// Mat3 mat; +// Vec3 vec; + +// mat.setZero(); +// vec.setIdentity(); + +// // std::cout << "result: " << mat - vec << std::endl; +// } + +// /*Class array size*/ +// int main(){ +// Estimator* estTest[6]; +// std::cout << "size: " << sizeof(estTest)/sizeof(estTest[0]) << std::endl; +// } + +// /* windowFunc */ +// int main(){ +// double a1 = windowFunc(0.1*M_PI, M_PI, 1.0, 0.4); +// double a2 = windowFunc(0.2*M_PI, M_PI, 1.0, 0.4); +// double a3 = windowFunc(0.6*M_PI, M_PI, 1.0, 0.4); +// double a4 = windowFunc(0.9*M_PI, M_PI, 1.0, 0.4); + +// std::cout << a1 << ", " << a2 << ", " << a3 << ", " << a4 << std::endl; +// } + + +// /*updateAvgCov*/ +// int main(){ +// Vec3 p0, p1, p2, p3; +// Vec3 exp; +// Mat3 Cp; + +// p0 << 0, 2, 4; +// p1 << -0.2, 1.6, 2.5; +// p2 << 0.01, 2.11, 5.80; +// p3 << 0.13, 1.90, 4.30; + +// AvgCov test(3, "test", 1, 0, 1); +// test.measure(p0); +// test.measure(p1); +// test.measure(p2); +// test.measure(p3); + +// // for(int i(0); i<4; ++i){ +// // std::cout << "p: " << p[i] << std::endl; //注释掉会计算错误，不明原因 +// // // updateAvgCov(Cp, exp, p[i], i+1); +// // test.measure(p[i]); +// // } +// // std::cout << "exp: " << std::endl << exp << std::endl; +// // std::cout << "Cp: " << std::endl << Cp << std::endl; + +// } + + + +// // WaveGenerator +// int main(){ +// WaveGenerator *wave; +// Vec4 phase; +// VecInt4 contact; + +// wave = new WaveGenerator(0.3, 0.5, Vec4(0, 1, 1, 0)); + +// // wave->calcContactPhase(phase, contact); +// // std::cout << "contact: " << contact.transpose() << std::endl; +// // std::cout << "phase: " << phase.transpose() << std::endl; + +// for(int i(0); i<5; ++i){ +// wave->calcContactPhase(phase, contact); +// usleep(2000); +// std::cout << "contact: " << contact.transpose() << std::endl; +// std::cout << "phase: " << phase.transpose() << std::endl; +// } + +// delete wave; + +// return 0; +// } + + +// // Test rotMatToExp +// int main(){ +// RotMat rm = rotx(M_PI); +// Vec3 w = rotMatToExp(rm); + +// std::cout << "rm:" << std::endl << rm << std::endl; +// std::cout << "w: " << w.transpose() << std::endl; +// } + + + +// // Test leg kinematics +// int main(){ +// std::cout << std::fixed << std::setprecision(3); + +// A1Robot robModel; +// Vec34 feetPos; +// feetPos << 0.180, 0.180, -0.180, -0.180, +// -0.131, 0.131, -0.131, 0.131, +// -0.200, -0.400, -0.400, -0.400; +// Vec12 q = robModel.getQ(feetPos, FrameType::BODY); +// LowlevelState state; +// for(int i(0); i<12; ++i){ +// state.motorState[i].q = q(i); +// } +// Vec34 calFeetPos = robModel.getFeet2BPositions(state, FrameType::BODY); + +// std::cout << "feetPos: " << std::endl << feetPos << std::endl; +// std::cout << "state q: " << std::endl << state.getQ() << std::endl; +// std::cout << "calFeetPos: " << std::endl << calFeetPos << std::endl; + + + +// // A1Leg leg(0, Vec3( 0.1805, -0.047, 0)); +// // Vec3 ql = leg.calcQ(Vec3(0.18, -0.131, -0.6), FrameType::BODY); + +// // ql.setZero(); +// // Vec3 pl = leg.calcPEe2B(ql); +// // std::cout << "ql: " << ql.transpose() << std::endl; +// // std::cout << "pl: " << pl.transpose() << std::endl; +// } + + +/********************************/ +/***** Test *****/ +/********************************/ +// // test rotMatToExp +// int main(){ +// Mat3 rm; +// rm.setIdentity(); + +// Vec3 so3 = rotMatToExp(rm); + +// std::cout << "so3: " << so3.transpose() << std::endl; +// std::cout << "1/sin: " << 1.0f/sin(1e-6) << std::endl; +// std::cout << "acos 0: " << acos(0) << std::endl; +// std::cout << "acos 1: " << acos(1) << std::endl; +// } + + +/********************************/ +/***** Test *****/ +/********************************/ +// // test balance controller +// int main(){ +// // double mass = 1; +// // Mat3 Ig; +// // Mat6 S; +// // double alpha = 0.01; +// // double beta = 0.01; +// // Ig << 0.1, 0, 0, +// // 0, 0.1, 0, +// // 0, 0, 0.1; +// // S << 1, 0, 0, 0, 0, 0, +// // 0, 1, 0, 0, 0, 0, +// // 0, 0, 1, 0, 0, 0, +// // 0, 0, 0, 1, 0, 0, +// // 0, 0, 0, 0, 1, 0, +// // 0, 0, 0, 0, 0, 1; + +// // BalanceCtrl bCtrl(mass, Ig, S, alpha, beta); +// BalanceCtrl bCtrl; + +// Vec34 F; +// Vec3 ddPcd, dwbd; +// Vec34 feetPos2B; +// VecInt4 contact; +// RotMat rotM; +// double x = 0.1805; +// double y = 0.047; +// double h = 0.3; + +// ddPcd << 0.0, 0.0, 0.0; +// dwbd << 0.0, 0.0, 0.0; + +// // ddPcd.setZero(); +// // dwbd.setZero(); + +// feetPos2B << x, x, -x, -x, +// -y, y, -y, y, +// -h, -h, -h, -h; +// contact << 1, 1, 1, 1; + +// rotM.setIdentity(); +// // rotM = rotz(M_PI/4); + +// // long long startT = getSystemTime(); +// F = bCtrl.calF(ddPcd, dwbd, rotM, rotM*feetPos2B, contact); +// std::cout << "F: " << std::endl << F << std::endl; + +// // F = bCtrl.calF(ddPcd, dwbd, rotM, feetPos2B, contact); +// // std::cout << "F: " << std::endl << F << std::endl; +// // F = bCtrl.calF(ddPcd, dwbd, rotM, feetPos2B, contact); +// // std::cout << "F: " << std::endl << F << std::endl; +// // F = bCtrl.calF(ddPcd, dwbd, rotM, feetPos2B, contact); +// // std::cout << "F: " << std::endl << F << std::endl; +// // F = bCtrl.calF(ddPcd, dwbd, rotM, feetPos2B, contact); +// // std::cout << "F: " << std::endl << F << std::endl; + +// A1Robot robModel; +// Vec12 q; +// q << 0.0, 0.67, -1.3, 0.0, 0.67, -1.3, 0.0, 0.67, -1.3, 0.0, 0.67, -1.3; +// Vec12 tau = robModel.getTau(q, -F); + +// // std::cout << "tau: " << tau.transpose() << std::endl; + +// Eigen::Matrix A; +// A.block(0, 0, 3, 3) = I3; +// A.block(0, 3, 3, 3) = I3; +// A.block(0, 6, 3, 3) = I3; +// A.block(0, 9, 3, 3) = I3; +// A.block(3, 0, 3, 3) = skew(rotM*feetPos2B.col(0)); +// A.block(3, 3, 3, 3) = skew(rotM*feetPos2B.col(1)); +// A.block(3, 6, 3, 3) = skew(rotM*feetPos2B.col(2)); +// A.block(3, 9, 3, 3) = skew(rotM*feetPos2B.col(3)); + +// std::cout << "Fb: " << (A * vec34ToVec12(F)).transpose() << std::endl; + +// return 0; +// } + +/********************************/ +/***** multi-anime *****/ +/********************************/ +// class AnimePlot{ +// public: +// AnimePlot(std::string plotName); +// ~AnimePlot(); +// void addFrame(double x, double y); +// private: +// Curve _curve; +// plt::Plot plot; +// void _threadFunc(); +// std::thread _animeThread; +// long long _pointNum; +// bool _start; +// }; + +// AnimePlot::AnimePlot(std::string plotName) +// :plot(plotName){ +// plt::figure(); +// plt::plot(); +// } + +// AnimePlot::~AnimePlot(){ +// _animeThread.join(); +// } + +// void AnimePlot::addFrame(double x, double y){ +// if(!_start){ +// _start = true; +// _animeThread = std::thread(&AnimePlot::_threadFunc, this); +// } +// _curve.x.push_back(x); +// _curve.y.push_back(y); +// } + +// void AnimePlot::_threadFunc(){ +// while(true){ +// // Just update data for this plot. +// plot.update(_curve.x, _curve.y); + +// // Small pause so the viewer has a chance to enjoy the animation. +// plt::pause(0.1); +// } +// } + +// // PyPlot多个动态图测试 +// int main(){ +// int n = 1000; +// AnimePlot plot1("p1"); +// // AnimePlot plot2; + +// for(int i=0; i &xt, std::vector &yt) +// { +// const double target_length = 300; +// const double half_win = (target_length/(2.*sqrt(1.+t*t))); + +// xt[0] = x - half_win; +// xt[1] = x + half_win; +// yt[0] = y - half_win*t; +// yt[1] = y + half_win*t; +// } + + +// int main() +// { +// size_t n = 1000; +// std::vector x, y; + +// const double w = 0.05; +// const double a = n/2; + +// for (size_t i=0; i xt(2), yt(2); + +// plt::title("Tangent of a sine curve"); +// plt::xlim(x.front(), x.back()); +// plt::ylim(-a, a); +// plt::axis("equal"); + +// // Plot sin once and for all. +// plt::named_plot("sin", x, y); + +// // Prepare plotting the tangent. +// plt::Plot plot("tangent"); + +// plt::legend(); + +// for (size_t i=0; i