From 4ed93511dfc91646f70b34a9739f0ccc6725806c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 7 Dec 2020 11:08:52 -0800 Subject: [PATCH 01/13] enable USE_CERES=ON --- build_libs_unix.sh | 2 +- build_libs_windows.bat | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/build_libs_unix.sh b/build_libs_unix.sh index 72658171..47306968 100755 --- a/build_libs_unix.sh +++ b/build_libs_unix.sh @@ -57,6 +57,6 @@ cd $ROOT mkdir build_cmake cd build_cmake -cmake -DBullet_DIR=$ROOT/third_party/bullet3/build_cmake -Dgflags_DIR=$ROOT/third_party/gflags/build_cmake -Dglog_DIR=$ROOT/third_party/glog/build_cmake -DEigen3_DIR=$ROOT/third_party/eigen3/build_cmake -DCeres_DIR=$ROOT/third_party/ceres-solver/build_cmake/local_install/lib/cmake/Ceres -DUSE_CPPAD=ON .. +cmake -DBullet_DIR=$ROOT/third_party/bullet3/build_cmake -Dgflags_DIR=$ROOT/third_party/gflags/build_cmake -Dglog_DIR=$ROOT/third_party/glog/build_cmake -DEigen3_DIR=$ROOT/third_party/eigen3/build_cmake -DCeres_DIR=$ROOT/third_party/ceres-solver/build_cmake/local_install/lib/cmake/Ceres -DUSE_CPPAD=ON -DUSE_CERES=ON .. make -j4 diff --git a/build_libs_windows.bat b/build_libs_windows.bat index 3cde8b76..30252351 100644 --- a/build_libs_windows.bat +++ b/build_libs_windows.bat @@ -86,7 +86,7 @@ del third_party\gflags\build_cmake\local_install\lib\*.lib mkdir build_cmake cd build_cmake -cmake -DCMAKE_CXX_FLAGS="/MP" -DUSE_MSVC_RUNTIME_LIBRARY_DLL=ON -DBullet_DIR=%ROOT%\third_party\bullet3\build_cmake -Dgflags_DIR=%ROOT%\third_party\gflags\build_cmake -Dglog_DIR=%ROOT%\third_party\glog\build_cmake -DEigen3_DIR=%ROOT%\third_party\eigen3\build_cmake -DCeres_DIR=%ROOT%\third_party\ceres-solver\build_cmake\local_install\cmake -DUSE_CPPAD=ON .. +cmake -DCMAKE_CXX_FLAGS="/MP" -DUSE_CERES=ON -DUSE_MSVC_RUNTIME_LIBRARY_DLL=ON -DBullet_DIR=%ROOT%\third_party\bullet3\build_cmake -Dgflags_DIR=%ROOT%\third_party\gflags\build_cmake -Dglog_DIR=%ROOT%\third_party\glog\build_cmake -DEigen3_DIR=%ROOT%\third_party\eigen3\build_cmake -DCeres_DIR=%ROOT%\third_party\ceres-solver\build_cmake\local_install\cmake -DUSE_CPPAD=ON .. cmake --build . --target INSTALL --config Release start DIFF_PHYSICS.sln From 5e99488731c8fbe9c5fb124179cfa0b121091d59 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 10 Dec 2020 09:13:47 -0800 Subject: [PATCH 02/13] expose TinyPosef and quat_integrate --- python/pytinydiffsim.inl | 1 + python/pytinydiffsim_includes.h | 11 +++++++++++ python/pytinyopengl3.cc | 9 +++++++++ 3 files changed, 21 insertions(+) diff --git a/python/pytinydiffsim.inl b/python/pytinydiffsim.inl index 9b4f790b..e9314f8a 100644 --- a/python/pytinydiffsim.inl +++ b/python/pytinydiffsim.inl @@ -329,6 +329,7 @@ m.def("integrate_euler_qdd", &MyIntegrateEulerQdd); m.def("compute_inertia_dyad", &MyComputeInertia); m.def("point_jacobian", &MyPointJacobian); + m.def("quat_integrate", &MyQuatIntegrate); m.def("inverse_kinematics", &MyInverseKinematics); m.def("link_transform_base_frame", &MyGetLinkTransformInBase); m.def("find_file", &MyFindFile); diff --git a/python/pytinydiffsim_includes.h b/python/pytinydiffsim_includes.h index 9c23d584..2901a0de 100644 --- a/python/pytinydiffsim_includes.h +++ b/python/pytinydiffsim_includes.h @@ -101,6 +101,17 @@ inline tds::RigidBodyInertia MyComputeInertia(const MyScalar& mass, return rb_inertia; } +inline MyAlgebra::Quaternion MyQuatIntegrate(const MyAlgebra::Quaternion& start_orn, const MyAlgebra::Vector3& ang_vel, MyScalar dt) +{ + MyAlgebra::Quaternion orn = start_orn; + MyAlgebra::Quaternion orn2 = MyAlgebra::quat_velocity(orn, ang_vel, dt); + MyAlgebra::quat_increment(orn, orn2); + orn = MyAlgebra::normalize(orn); + return orn; +} + + + inline MyAlgebra::Matrix3X MyPointJacobian(tds::MultiBody& mb, int link_index, const MyAlgebra::Vector3& point, bool is_local) { return tds::point_jacobian2(mb, link_index, point, is_local); diff --git a/python/pytinyopengl3.cc b/python/pytinyopengl3.cc index 7c6b0fca..fa446fdb 100644 --- a/python/pytinyopengl3.cc +++ b/python/pytinyopengl3.cc @@ -19,6 +19,7 @@ #include "math/tiny/tiny_float_utils.h" #include "math/tiny/tiny_vector3.h" +#include "math/tiny/tiny_pose.h" #include "visualizer/opengl/tiny_opengl3_app.h" #include "visualizer/opengl/tiny_camera.h" #include @@ -198,6 +199,14 @@ PYBIND11_MODULE(pytinyopengl3, m) { m.def("load_obj_shapes", &my_load_obj_shapes); + + py::class_>(m, "TinyPosef") + .def(py::init<>()) + .def(py::self * py::self) + .def_readwrite("position", &TinyPose::m_position) + .def_readwrite("orientation", &TinyPose::m_orientation) + .def("set_identity", &TinyPose::set_identity); + py::class_>(m, "TinyVector3f") .def(py::init()) .def("set_zero", &TinyVector3::set_zero) From ca8f8ac4a662afee26292e229e5f2849e67f2d57 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 11 Dec 2020 09:16:18 -0800 Subject: [PATCH 03/13] rename radius -> sim_spacing and enlarge it to make independent simulations more clear --- examples/cuda_codegen.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/examples/cuda_codegen.cpp b/examples/cuda_codegen.cpp index 13e47040..655c944a 100644 --- a/examples/cuda_codegen.cpp +++ b/examples/cuda_codegen.cpp @@ -186,7 +186,8 @@ int main(int argc, char* argv[]) { const int link_pos_id_offset = simulation.system->dof() + simulation.system->dof_qd(); const int square_id = (int)std::sqrt((double)num_total_threads); - const float radius = 1.f; + //sim_spacing is the visual distance between independent parallel simulations + const float sim_spacing = 5.f; for (int run = 0; run < 40; ++run) { for (int i = 0; i < num_total_threads; ++i) { inputs[i] = std::vector(simulation.input_dim(), Scalar(0)); @@ -218,8 +219,8 @@ int main(int argc, char* argv[]) { pos[0] = outputs[i][link_pos_id_offset + l * 3 + 0]; pos[1] = outputs[i][link_pos_id_offset + l * 3 + 1]; pos[2] = outputs[i][link_pos_id_offset + l * 3 + 2]; - pos[0] += radius * (i % square_id) - square_id * radius / 2; - pos[1] += radius * (i / square_id) - square_id * radius / 2; + pos[0] += sim_spacing * (i % square_id) - square_id * sim_spacing / 2; + pos[1] += sim_spacing * (i / square_id) - square_id * sim_spacing / 2; TinyQuaternionf orn(0, 0, 0, 1); if (l > 0) { //app.m_renderer->draw_line(prev_pos, pos, line_color, line_width); From 97e05bfad3825d35f577f1263abcb03d2de95ba5 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 14 Dec 2020 15:48:34 -0800 Subject: [PATCH 04/13] ignore updates to submodules --- .gitmodules | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.gitmodules b/.gitmodules index 9efff2dc..0c1fcee7 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,18 +1,24 @@ [submodule "third_party/CppAD"] path = third_party/CppAD url = https://github.com/coin-or/CppAD.git + ignore = all [submodule "third_party/CppADCodeGen"] path = third_party/CppADCodeGen url = https://github.com/eric-heiden/CppADCodeGen.git + ignore = all [submodule "third_party/glog"] path = third_party/glog url = https://github.com/google/glog.git + ignore = all [submodule "third_party/ceres-solver"] path = third_party/ceres-solver url = https://github.com/ceres-solver/ceres-solver + ignore = all [submodule "third_party/gflags"] path = third_party/gflags url = https://github.com/gflags/gflags.git + ignore = all [submodule "third_party/bullet3"] path = third_party/bullet3 url = https://github.com/bulletphysics/bullet3 + ignore = all From 3b599e699ffc890ce3a4414a89bfaf4f028c7ade Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 15 Dec 2020 20:51:45 -0800 Subject: [PATCH 05/13] refactor to allow instancing of multi bodies towards cuda_codegen (for now, see examples/tiny_urdf_parser_opengl_example.cpp) rename TinyMultiBody.size() -> num_links() --- examples/ceres_estimation_real.cpp | 4 +- examples/cuda_codegen.cpp | 6 +- examples/laikago_footstep_ik.cpp | 9 +- examples/opengl_urdf_visualizer.h | 104 +++--- examples/pybullet_visual_instance_generator.h | 28 ++ examples/tiny_urdf_parser_meshcat_example.cpp | 14 +- examples/tiny_urdf_parser_opengl_example.cpp | 353 +++++++++++++++++- examples/tiny_visual_instance_generator.h | 34 ++ python/pytinydiffsim.inl | 12 +- python/pytinydiffsim_includes.h | 2 +- src/dynamics/forward_dynamics.hpp | 4 +- src/dynamics/jacobian.hpp | 2 +- src/dynamics/kinematics.hpp | 8 +- src/dynamics/mass_matrix.hpp | 2 +- src/link.hpp | 2 +- src/multi_body.hpp | 16 +- src/urdf/pybullet_urdf_import.hpp | 40 +- src/urdf/urdf_cache.hpp | 2 +- src/urdf/urdf_structures.hpp | 7 +- src/urdf/urdf_to_multi_body.hpp | 19 +- .../meshcat/meshcat_urdf_visualizer.h | 22 +- .../opengl/tiny_gl_instancing_renderer.cpp | 8 +- test/rbdl_test_utils.hpp | 2 +- test/test_rbdl.cpp | 2 +- 24 files changed, 556 insertions(+), 146 deletions(-) create mode 100644 examples/pybullet_visual_instance_generator.h create mode 100644 examples/tiny_visual_instance_generator.h diff --git a/examples/ceres_estimation_real.cpp b/examples/ceres_estimation_real.cpp index e390a6b6..a01920e6 100644 --- a/examples/ceres_estimation_real.cpp +++ b/examples/ceres_estimation_real.cpp @@ -111,7 +111,7 @@ void visualize_trajectory(const std::vector> &states, std::vector mbvisuals; if (visualizer->canSubmitCommand()) { - for (int i = 0; i < mb->size(); i++) + for (int i = 0; i < mb->num_links(); i++) { int sphereId = visualizer->loadURDF("sphere_small.urdf"); mbvisuals.push_back(sphereId); @@ -135,7 +135,7 @@ void visualize_trajectory(const std::vector> &states, std::chrono::duration(Utils::getDouble(dt * 20.))); // sync transforms int visual_index = 0; - for (std::size_t l = 0; l < mb->size(); l++) + for (std::size_t l = 0; l < mb->num_links(); l++) { int sphereId = mbvisuals[visual_index++]; typename Algebra::Quaternion rot; diff --git a/examples/cuda_codegen.cpp b/examples/cuda_codegen.cpp index 655c944a..7e364179 100644 --- a/examples/cuda_codegen.cpp +++ b/examples/cuda_codegen.cpp @@ -49,7 +49,7 @@ struct ContactSimulation { int input_dim() const { return system->dof() + system->dof_qd(); } int state_dim() const { - return system->dof() + system->dof_qd() + system->size() * 3; + return system->dof() + system->dof_qd() + system->num_links() * 3; } int output_dim() const { return num_timesteps * state_dim(); } @@ -166,7 +166,7 @@ int main(int argc, char* argv[]) { int sphere_shape = app.register_graphics_unit_sphere_shape(SPHERE_LOD_LOW); // typedef tds::Conversion Conversion; std::vector visuals; - for (int i = 0; i < num_total_threads * simulation.system->size(); ++i) { + for (int i = 0; i < num_total_threads * simulation.system->num_links(); ++i) { TinyVector3f pos(0, 0, 0); TinyQuaternionf orn(0, 0, 0, 1); TinyVector3f color(0.5, 0.6, 1); @@ -213,7 +213,7 @@ int main(int argc, char* argv[]) { int visual_id = 0; for (int i = 0; i < num_total_threads; ++i) { TinyVector3f prev_pos(0, 0, 0); - for (std::size_t l = 0; l < simulation.system->size(); ++l) { + for (std::size_t l = 0; l < simulation.system->num_links(); ++l) { int sphereId = visuals[visual_id++]; TinyVector3f pos; pos[0] = outputs[i][link_pos_id_offset + l * 3 + 0]; diff --git a/examples/laikago_footstep_ik.cpp b/examples/laikago_footstep_ik.cpp index e9c4bb59..f81f7c3a 100644 --- a/examples/laikago_footstep_ik.cpp +++ b/examples/laikago_footstep_ik.cpp @@ -35,6 +35,8 @@ #include "visualizer/pybullet/pybullet_visualizer_api.h" #include "utils/file_utils.hpp" +#include "pybullet_visual_instance_generator.h" + using namespace TINY; using namespace tds; #undef max @@ -152,6 +154,8 @@ struct GaitGenerator { } }; + + int main(int argc, char* argv[]) { double dt = 1. / 1000; @@ -232,6 +236,7 @@ int main(int argc, char* argv[]) { int kd_id = sim->addUserDebugParameter("kd", 0, 13, 3.); int force_id = sim->addUserDebugParameter("max force", 0, 1500, 550); + PyBulletInstanceGenerator gen(sim); { MultiBody >* mb = world.create_multi_body(); int robotId = sim->loadURDF(plane_filename); @@ -239,7 +244,7 @@ int main(int argc, char* argv[]) { PyBulletUrdfImport< TinyAlgebra >::extract_urdf_structs( urdf_data, robotId, sim, sim); UrdfToMultiBody< TinyAlgebra >::convert_to_multi_body(urdf_data, - world, *mb); + world, *mb, &gen); mb->initialize(); sim->removeBody(robotId); } @@ -255,7 +260,7 @@ int main(int argc, char* argv[]) { PyBulletUrdfImport< TinyAlgebra >::extract_urdf_structs( urdf_data, robotId, sim, sim); UrdfToMultiBody< TinyAlgebra >::convert_to_multi_body(urdf_data, - world, *mb); + world, *mb, &gen); mbbodies.push_back(mb); mb->set_floating_base(false); diff --git a/examples/opengl_urdf_visualizer.h b/examples/opengl_urdf_visualizer.h index ed6fde83..7f29bcdd 100644 --- a/examples/opengl_urdf_visualizer.h +++ b/examples/opengl_urdf_visualizer.h @@ -50,7 +50,8 @@ struct OpenGLUrdfVisualizer { TinyVector3 origin_xyz; TinyVector3 inertia_xyz; TinyVector3 inertia_rpy; - std::vector instance_ids; + std::vector visual_shape_uids; + std::vector < ::TINY::TinyVector3f> shape_colors; }; std::map m_link_name_to_index; @@ -64,7 +65,7 @@ struct OpenGLUrdfVisualizer { TinyOpenGL3App m_opengl_app; - OpenGLUrdfVisualizer(int width=1024, int height=768, const char* title = "test") + OpenGLUrdfVisualizer(int width=1024, int height=768, const char* title = "Tiny Differentiable Simulator") : m_uid(1234) , m_opengl_app(title, width, height) { @@ -80,7 +81,8 @@ struct OpenGLUrdfVisualizer { //todo } - void load_obj(const std::string& obj_filename, const ::TINY::TinyVector3f& pos, const ::TINY::TinyQuaternionf& orn, const ::TINY::TinyVector3f& scaling, std::vector& instance_ids) + + void load_obj_shapes(const std::string& obj_filename, std::vector& shape_ids, std::vector<::TINY::TinyVector3f>& colors) { tinyobj::attrib_t attrib; std::vector shapes; @@ -91,12 +93,11 @@ struct OpenGLUrdfVisualizer { std::string err; char basepath[1024]; bool triangulate = true; - + ::tds::FileUtils::extract_path(obj_filename.c_str(), basepath, 1024); bool ret = tinyobj::LoadObj(&attrib, &shapes, &materials, &warn, &err, obj_filename.c_str(), basepath, triangulate); - for (int i = 0; i < shapes.size(); i++) { std::vector indices; @@ -121,7 +122,21 @@ struct OpenGLUrdfVisualizer { } } int shape = m_opengl_app.m_renderer->register_shape(&vertices[0].x, vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); - int instance_id = m_opengl_app.m_renderer->register_graphics_instance(shape, pos, orn, color, scaling); + shape_ids.push_back(shape); + colors.push_back(color); + } + } + + void load_obj(const std::string& obj_filename, const ::TINY::TinyVector3f& pos, const ::TINY::TinyQuaternionf& orn, const ::TINY::TinyVector3f& scaling, std::vector& instance_ids) + { + std::vector shape_ids; + std::vector<::TINY::TinyVector3f> colors; + load_obj_shapes(obj_filename, shape_ids, colors); + + for (int i = 0; i < shape_ids.size(); i++) + { + int shape = shape_ids[i]; + int instance_id = m_opengl_app.m_renderer->register_graphics_instance(shape, pos, orn, colors[i], scaling); instance_ids.push_back(instance_id); } @@ -156,16 +171,31 @@ struct OpenGLUrdfVisualizer { ::TINY::TinyVector3f pos(0, 0, 0); ::TINY::TinyVector3f scaling(v.geometry.mesh.scale[0], v.geometry.mesh.scale[1], v.geometry.mesh.scale[2]); ::TINY::TinyQuaternionf orn(0, 0, 0, 1); - load_obj(obj_filename, pos, orn, scaling, b2v.instance_ids); + load_obj_shapes(obj_filename, b2v.visual_shape_uids, b2v.shape_colors); } } - v.sync_visual_body_uid1 = m_uid; + v.visual_shape_uid = m_uid; m_b2vis[m_uid++] = b2v; } } - void convert_visuals(TinyUrdfStructures &urdf, - const std::string &texture_path) { +#if 0 + void create_visual_instances(TinyMultiBody& body) { + for (int i = 0; i < b2v.visual_shape_uids.size(); i++) { + int shape = b2v.visual_shape_uids[i]; + int instance_id = m_opengl_app.m_renderer->register_graphics_instance(shape, pos, orn, b2v.shape_colors[i], scaling); + if (link_index == -1) { + body.visual_instance_uids().push_back(instance_id); + } + else { + body.links_[link_index].visual_instance_uids.push_back(instance_id); + } + } + } +#endif + + void convert_visuals(TinyUrdfStructures& urdf, + const std::string& texture_path) { m_link_name_to_index.clear(); { int link_index = -1; @@ -182,49 +212,39 @@ struct OpenGLUrdfVisualizer { } } + void sync_visual_transforms(const TinyMultiBody* body) { // sync base transform - for (int v = 0; v < body->visual_ids().size(); v++) { - int visual_id = body->visual_ids()[v]; - if (m_b2vis.find(visual_id) != m_b2vis.end()) { - Quaternion rot; - Transform geom_X_world = - body->base_X_world() * body->X_visuals()[v]; - - const TinyMatrix3& m = - geom_X_world.rotation; - m.getRotation(rot); - const TinyVisualLinkInfo& viz = m_b2vis.at(visual_id); - for (int i = 0; i < viz.instance_ids.size(); i++) - { - int instance_id = viz.instance_ids[i]; - ::TINY::TinyVector3f pos(geom_X_world.translation[0], geom_X_world.translation[1], geom_X_world.translation[2]); - ::TINY::TinyQuaternionf orn(rot[0], rot[1], rot[2], rot[3]); - m_opengl_app.m_renderer->write_single_instance_transform_to_cpu(pos, orn, instance_id); - } - } + for (int v = 0; v < body->visual_instance_uids().size(); v++) { + int visual_instance_id = body->visual_instance_uids()[v]; + Quaternion rot; + Transform geom_X_world = + body->base_X_world() * body->X_visuals()[v]; + + const TinyMatrix3& m = + geom_X_world.rotation; + m.getRotation(rot); + + ::TINY::TinyVector3f pos(geom_X_world.translation[0], geom_X_world.translation[1], geom_X_world.translation[2]); + ::TINY::TinyQuaternionf orn(rot[0], rot[1], rot[2], rot[3]); + m_opengl_app.m_renderer->write_single_instance_transform_to_cpu(pos, orn, visual_instance_id); } for (int l = 0; l < body->links().size(); l++) { - for (int v = 0; v < body->links()[l].visual_ids.size(); v++) { - int visual_id = body->links()[l].visual_ids[v]; - if (m_b2vis.find(visual_id) != m_b2vis.end()) { + for (int v = 0; v < body->links()[l].visual_instance_uids.size(); v++) { + int visual_instance_id = body->links()[l].visual_instance_uids[v]; + if (visual_instance_id >= 0) + { Quaternion rot; Transform geom_X_world = body->links()[l].X_world * body->links()[l].X_visuals[v]; - + TinyMatrix3& m = geom_X_world.rotation; - const TinyVisualLinkInfo& viz = m_b2vis.at(visual_id); m.getRotation(rot); - for (int i = 0; i < viz.instance_ids.size(); i++) - { - int instance_id = viz.instance_ids[i]; - ::TINY::TinyVector3f pos(geom_X_world.translation[0], geom_X_world.translation[1], geom_X_world.translation[2]); - ::TINY::TinyQuaternionf orn(rot[0], rot[1], rot[2], rot[3]); - m_opengl_app.m_renderer->write_single_instance_transform_to_cpu(pos, orn, instance_id); - } - + ::TINY::TinyVector3f pos(geom_X_world.translation[0], geom_X_world.translation[1], geom_X_world.translation[2]); + ::TINY::TinyQuaternionf orn(rot[0], rot[1], rot[2], rot[3]); + m_opengl_app.m_renderer->write_single_instance_transform_to_cpu(pos, orn, visual_instance_id); } } } diff --git a/examples/pybullet_visual_instance_generator.h b/examples/pybullet_visual_instance_generator.h new file mode 100644 index 00000000..1feb6a11 --- /dev/null +++ b/examples/pybullet_visual_instance_generator.h @@ -0,0 +1,28 @@ +#ifndef VISUAL_INSTANCE_GENERATOR_H +#define VISUAL_INSTANCE_GENERATOR_H + +#include + +struct PyBulletInstanceGenerator : public VisualInstanceGenerator +{ + PyBulletVisualizerAPI* sim_; + PyBulletInstanceGenerator(PyBulletVisualizerAPI* sim) : sim_(sim) { + } + + virtual void create_visual_instance(int shape_uid, std::vector& visual_instances) + { + b3RobotSimulatorCreateMultiBodyArgs args2; + args2.m_baseVisualShapeIndex = shape_uid; + args2.m_baseMass = 0; + int visual_instance = sim_->createMultiBody(args2); + visual_instances.push_back(visual_instance); + b3RobotSimulatorChangeVisualShapeArgs args_change; + args_change.m_objectUniqueId = visual_instance; + args_change.m_linkIndex = -1; + args_change.m_hasRgbaColor = true; + args_change.m_rgbaColor.setValue(1, 1, 1, 1); + sim_->changeVisualShape(args_change); + } +}; + +#endif //VISUAL_INSTANCE_GENERATOR_H diff --git a/examples/tiny_urdf_parser_meshcat_example.cpp b/examples/tiny_urdf_parser_meshcat_example.cpp index 2734b59e..1c993dd2 100644 --- a/examples/tiny_urdf_parser_meshcat_example.cpp +++ b/examples/tiny_urdf_parser_meshcat_example.cpp @@ -65,10 +65,10 @@ int main(int argc, char* argv[]) { UrdfStructures plane_urdf_structures = parser.load_urdf(plane_file_name); UrdfToMultiBody::convert_to_multi_body( - plane_urdf_structures, world, plane_mb); + plane_urdf_structures, world, plane_mb,0); std::string texture_path = "checker_purple.png"; meshcat_viz.m_path_prefix = plane_search_path; - meshcat_viz.convert_visuals(plane_urdf_structures, texture_path); + meshcat_viz.convert_visuals(plane_urdf_structures, texture_path, 0); } char search_path[TINY_MAX_EXE_PATH_LEN]; @@ -93,14 +93,14 @@ int main(int argc, char* argv[]) { // create graphics structures std::string texture_path = "laikago_tex.jpg"; meshcat_viz.m_path_prefix = search_path; - meshcat_viz.convert_visuals(urdf_structures, texture_path); + bool floating_base = true; MultiBody& mb = *world.create_multi_body(); mb.set_floating_base(true); UrdfToMultiBody::convert_to_multi_body( - urdf_structures, world, mb); + urdf_structures, world, mb, 0); mb.initialize(); - + meshcat_viz.convert_visuals(urdf_structures, texture_path, &mb); int start_index = 0; if (floating_base) { start_index = 7; @@ -178,8 +178,8 @@ int main(int argc, char* argv[]) { forward_dynamics(mb, grav); - //integrate_euler(mb, dt); - + integrate_euler_qdd(mb, dt); + world.step(dt); integrate_euler(mb, dt); diff --git a/examples/tiny_urdf_parser_opengl_example.cpp b/examples/tiny_urdf_parser_opengl_example.cpp index c5fea747..d2c98c6c 100644 --- a/examples/tiny_urdf_parser_opengl_example.cpp +++ b/examples/tiny_urdf_parser_opengl_example.cpp @@ -29,6 +29,9 @@ #include "dynamics/forward_dynamics.hpp" #include "dynamics/integrator.hpp" +#include "urdf/urdf_cache.hpp" +#include "tiny_visual_instance_generator.h" + using namespace TINY; using namespace tds; @@ -43,8 +46,7 @@ typedef TinyQuaternion Quaternion; double knee_angle = -0.5; double abduction_angle = 0.2; -int frameskip_gfx_sync = - 15; // only sync every 10 frames (sim at 1000 Hz, gfx at ~60hz) +int frameskip_gfx_sync =1; // only sync every 10 frames (sim at 1000 Hz, gfx at ~60hz) double initial_poses[] = { abduction_angle, 0., knee_angle, abduction_angle, 0., knee_angle, @@ -53,6 +55,7 @@ double initial_poses[] = { bool do_sim = true; + TinyKeyboardCallback prev_keyboard_callback = 0; void my_keyboard_callback(int keycode, int state) @@ -62,7 +65,134 @@ void my_keyboard_callback(int keycode, int state) prev_keyboard_callback(keycode, state); } + + +template +struct ContactSimulation { + using Scalar = typename Algebra::Scalar; + tds::UrdfCache cache; + std::string m_urdf_filename; + tds::World world; + tds::MultiBody* mb_ = nullptr; + + int num_timesteps{ 1 }; + Scalar dt{ Algebra::from_double(1e-3) }; + + int input_dim() const { return mb_->dof() + mb_->dof_qd(); } + int state_dim() const { + return mb_->dof() + mb_->dof_qd() + mb_->num_links() * 7; + } + int output_dim() const { return num_timesteps * state_dim(); } + + ContactSimulation() { + std::string plane_filename; + tds::FileUtils::find_file("plane_implicit.urdf", plane_filename); + cache.construct(plane_filename, world, false, false); + tds::FileUtils::find_file("laikago/laikago_toes_zup.urdf", m_urdf_filename); + mb_ = cache.construct(m_urdf_filename, world, false, true); + mb_->base_X_world().translation = Algebra::unit3_z(); + } + + std::vector operator()(const std::vector& v) { + assert(static_cast(v.size()) == input_dim()); + mb_->initialize(); + //copy input into q, qd + for (int i = 0; i < mb_->dof(); ++i) { + mb_->q(i) = v[i]; + } + for (int i = 0; i < mb_->dof_qd(); ++i) { + mb_->qd(i) = v[i + mb_->dof()]; + } + std::vector result(output_dim()); + for (int t = 0; t < num_timesteps; ++t) { + + // pd control + if (1) { + // use PD controller to compute tau + int qd_offset = mb_->is_floating() ? 6 : 0; + int q_offset = mb_->is_floating() ? 7 : 0; + int num_targets = mb_->tau_.size() - qd_offset; + std::vector q_targets; + q_targets.resize(mb_->tau_.size()); + + double kp = 150; + double kd = 3; + double max_force = 550; + int param_index = 0; + + for (int i = 0; i < mb_->tau_.size(); i++) { + mb_->tau_[i] = 0; + } + int tau_index = 0; + int pose_index = 0; + for (int i = 0; i < mb_->links_.size(); i++) { + if (mb_->links_[i].joint_type != JOINT_FIXED) { + double q_desired = initial_poses[pose_index++]; + double q_actual = mb_->q_[q_offset]; + double qd_actual = mb_->qd_[qd_offset]; + double position_error = (q_desired - q_actual); + double desired_velocity = 0; + double velocity_error = (desired_velocity - qd_actual); + double force = kp * position_error + kd * velocity_error; + + if (force < -max_force) force = -max_force; + if (force > max_force) force = max_force; + mb_->tau_[tau_index] = force; + q_offset++; + qd_offset++; + param_index++; + tau_index++; + } + } + } + + tds::forward_dynamics(*mb_, world.get_gravity()); + mb_->clear_forces(); + + integrate_euler_qdd(*mb_, dt); + + world.step(dt); + + tds::integrate_euler(*mb_, dt); + + //copy q, qd, link world poses (for rendering) to output + int j = 0; + for (int i = 0; i < mb_->dof(); ++i, ++j) { + result[j] = mb_->q(i); + } + for (int i = 0; i < mb_->dof_qd(); ++i, ++j) { + result[j] = mb_->qd(i); + } + for (const auto link : *mb_) { + if (link.X_visuals.size()) + { + Transform visual_X_world = link.X_world * link.X_visuals[0]; + result[j++] = visual_X_world.translation[0]; + result[j++] = visual_X_world.translation[1]; + result[j++] = visual_X_world.translation[2]; + auto orn = Algebra::matrix_to_quat(visual_X_world.rotation); + result[j++] = orn[0]; + result[j++] = orn[1]; + result[j++] = orn[2]; + result[j++] = orn[3]; + } + else + { + //check if we have links without visuals + assert(0); + j += 7; + } + } + } + return result; + } +}; + + + int main(int argc, char* argv[]) { + int sync_counter = 0; + int frame = 0; World world; UrdfParser parser; @@ -70,8 +200,196 @@ int main(int argc, char* argv[]) { OpenGLUrdfVisualizer visualizer; + visualizer.delete_all(); +#if 1 + ContactSimulation contact_sim; + + int input_dim = contact_sim.input_dim(); + std::vector prep_inputs; + + std::vector prep_outputs; + prep_inputs.resize(input_dim); + prep_inputs[3] = 1; + prep_inputs[6] = 1; + + //int sphere_shape = visualizer.m_opengl_app.register_graphics_unit_sphere_shape(SPHERE_LOD_LOW); + + { + std::vector shape_ids; + std::string plane_filename; + FileUtils::find_file("plane100.obj", plane_filename); + TinyVector3f pos(0, 0, 0); + TinyQuaternionf orn(0, 0, 0, 1); + TinyVector3f scaling(1, 1, 1); + visualizer.load_obj(plane_filename, pos, orn, scaling, shape_ids); + } + + //int sphere_shape = shape_ids[0]; + //TinyVector3f color = colors[0]; + // typedef tds::Conversion Conversion; + + bool create_instances = false; + char search_path[TINY_MAX_EXE_PATH_LEN]; + std::string texture_path = ""; + std::string file_and_path; + tds::FileUtils::find_file("laikago/laikago_toes_zup.urdf", file_and_path); + auto urdf_structures = contact_sim.cache.retrieve(file_and_path);// contact_sim.m_urdf_filename); + FileUtils::extract_path(file_and_path.c_str(), search_path, + TINY_MAX_EXE_PATH_LEN); + visualizer.m_path_prefix = search_path; + visualizer.convert_visuals(urdf_structures, texture_path); + + + int num_total_threads = 256; + std::vector visual_instances; + std::vector num_instances; + int num_base_instances; + + + for (int t = 0;t< num_total_threads;t++) + { + TinyVector3f pos(0, 0, 0); + TinyQuaternionf orn(0, 0, 0, 1); + TinyVector3f scaling(1, 1, 1); + int uid = urdf_structures.base_links[0].urdf_visual_shapes[0].visual_shape_uid; + OpenGLUrdfVisualizer::TinyVisualLinkInfo& vis_link = visualizer.m_b2vis[uid]; + int instance = -1; + int num_instances_per_link = 0; + for (int v = 0; v < vis_link.visual_shape_uids.size(); v++) + { + int sphere_shape = vis_link.visual_shape_uids[v]; + ::TINY::TinyVector3f color(1, 1, 1); + //visualizer.m_b2vis + instance = visualizer.m_opengl_app.m_renderer->register_graphics_instance( + sphere_shape, pos, orn, color, scaling); + visual_instances.push_back(instance); + num_instances_per_link++; + contact_sim.mb_->visual_instance_uids().push_back(instance); + } + num_base_instances = num_instances_per_link; + + for (int i = 0; i < contact_sim.mb_->num_links(); ++i) { + + + int uid = urdf_structures.links[i].urdf_visual_shapes[0].visual_shape_uid; + OpenGLUrdfVisualizer::TinyVisualLinkInfo& vis_link = visualizer.m_b2vis[uid]; + int instance = -1; + int num_instances_per_link = 0; + for (int v = 0; v < vis_link.visual_shape_uids.size(); v++) + { + int sphere_shape = vis_link.visual_shape_uids[v]; + ::TINY::TinyVector3f color(1, 1, 1); + //visualizer.m_b2vis + instance = visualizer.m_opengl_app.m_renderer->register_graphics_instance( + sphere_shape, pos, orn, color, scaling); + visual_instances.push_back(instance); + num_instances_per_link++; + + contact_sim.mb_->links_[i].visual_instance_uids.push_back(instance); + } + num_instances.push_back(num_instances_per_link); + } + } + + //app.m_renderer->write_single_instance_transform_to_cpu(pos, orn, sphereId); + + + std::vector> parallel_outputs( + num_total_threads, std::vector(contact_sim.output_dim())); + + std::vector> parallel_inputs(num_total_threads); + + for (int i = 0; i < num_total_threads; ++i) { + parallel_inputs[i] = std::vector(contact_sim.input_dim(), MyScalar(0)); + parallel_inputs[i][3] = 1; + parallel_inputs[i][6] = 1; + + } + + while (!visualizer.m_opengl_app.m_window->requested_exit()) { + + for (int i = 0; i < num_total_threads; ++i) { + parallel_outputs[i] = contact_sim(parallel_inputs[i]); + for (int j = 0; j < contact_sim.input_dim(); ++j) { + parallel_inputs[i][j] = parallel_outputs[i][j]; + } + } + + sync_counter++; + frame += 1; + if (sync_counter > frameskip_gfx_sync) { + sync_counter = 0; + if (1) { + bool manual_sync = false; + if (manual_sync) + { + visualizer.sync_visual_transforms(contact_sim.mb_); + } + else + { + float sim_spacing = 2; + const int square_id = (int)std::sqrt((double)num_total_threads); + int instance_index = 0; + int offset = contact_sim.mb_->dof() + contact_sim.mb_->dof_qd(); + for (int s = 0; s < num_total_threads; s++) + { + + + for (int v = 0; v < num_base_instances; v++) + { + int visual_instance_id = visual_instances[instance_index++]; + if (visual_instance_id >= 0) + { + + ::TINY::TinyVector3f pos(parallel_outputs[s][4 + 0], + parallel_outputs[s][4 + 1], + parallel_outputs[s][4 + 2]); + ::TINY::TinyQuaternionf orn(parallel_outputs[s][0], + parallel_outputs[s][1], + parallel_outputs[s][2], + parallel_outputs[s][3]); + + pos[0] += sim_spacing * (s % square_id) - square_id * sim_spacing / 2; + pos[1] += sim_spacing * (s / square_id) - square_id * sim_spacing / 2; + + visualizer.m_opengl_app.m_renderer->write_single_instance_transform_to_cpu(pos, orn, visual_instance_id); + } + } + + for (int l = 0; l < contact_sim.mb_->links_.size(); l++) { + for (int v = 0; v < num_instances[l]; v++) + { + int visual_instance_id = visual_instances[instance_index++]; + if (visual_instance_id >= 0) + { + + ::TINY::TinyVector3f pos(parallel_outputs[s][offset + l * 7 + 0], + parallel_outputs[s][offset + l * 7 + 1], + parallel_outputs[s][offset + l * 7 + 2]); + ::TINY::TinyQuaternionf orn(parallel_outputs[s][offset + l * 7 + 3], + parallel_outputs[s][offset + l * 7 + 4], + parallel_outputs[s][offset + l * 7 + 5], + parallel_outputs[s][offset + l * 7 + 6]); + + pos[0] += sim_spacing * (s % square_id) - square_id * sim_spacing / 2; + pos[1] += sim_spacing * (s / square_id) - square_id * sim_spacing / 2; + + visualizer.m_opengl_app.m_renderer->write_single_instance_transform_to_cpu(pos, orn, visual_instance_id); + } + } + } + } + } + } + visualizer.render(); + std::this_thread::sleep_for(std::chrono::duration(frameskip_gfx_sync* contact_sim.dt)); + } + } + + +#else std::string plane_file_name; FileUtils::find_file("plane_implicit.urdf", plane_file_name); char plane_search_path[TINY_MAX_EXE_PATH_LEN]; @@ -79,14 +397,19 @@ int main(int argc, char* argv[]) { TINY_MAX_EXE_PATH_LEN); MultiBody& plane_mb = *world.create_multi_body(); plane_mb.set_floating_base(false); + { + TinyVisualInstanceGenerator vig(visualizer); UrdfStructures plane_urdf_structures = parser.load_urdf(plane_file_name); - UrdfToMultiBody::convert_to_multi_body( - plane_urdf_structures, world, plane_mb); std::string texture_path = "checker_purple.png"; visualizer.m_path_prefix = plane_search_path; visualizer.convert_visuals(plane_urdf_structures, texture_path); + + UrdfToMultiBody::convert_to_multi_body( + plane_urdf_structures, world, plane_mb, &vig); + + // } prev_keyboard_callback = visualizer.m_opengl_app.m_window->get_keyboard_callback(); visualizer.m_opengl_app.m_window->set_keyboard_callback(my_keyboard_callback); @@ -113,14 +436,23 @@ int main(int argc, char* argv[]) { // create graphics structures std::string texture_path = "laikago_tex.jpg"; visualizer.m_path_prefix = search_path; - visualizer.convert_visuals(urdf_structures, texture_path); - bool floating_base = true; MultiBody& mb = *world.create_multi_body(); + bool floating_base = true; + visualizer.convert_visuals(urdf_structures, texture_path); + mb.set_floating_base(true); - UrdfToMultiBody::convert_to_multi_body( - urdf_structures, world, mb); + { + TinyVisualInstanceGenerator vig(visualizer); + UrdfToMultiBody::convert_to_multi_body( + urdf_structures, world, mb, &vig); + } mb.initialize(); + //visualizer.create_visual_instances(mb); + + + + int start_index = 0; if (floating_base) { start_index = 7; @@ -151,8 +483,7 @@ int main(int argc, char* argv[]) { DoubleUtils::zero(), DoubleUtils::fraction(-1000, 100)); double dt = 1. / 1000.; - int sync_counter = 0; - int frame = 0; + while (!visualizer.m_opengl_app.m_window->requested_exit()) { forward_kinematics(mb); @@ -222,7 +553,7 @@ int main(int argc, char* argv[]) { std::this_thread::sleep_for(std::chrono::duration(frameskip_gfx_sync*dt)); } } - +#endif printf("finished\n"); return EXIT_SUCCESS; diff --git a/examples/tiny_visual_instance_generator.h b/examples/tiny_visual_instance_generator.h new file mode 100644 index 00000000..7bb28944 --- /dev/null +++ b/examples/tiny_visual_instance_generator.h @@ -0,0 +1,34 @@ + +#ifndef TINY_VISUAL_INSTANCE_GENERATOR_H +#define TINY_VISUAL_INSTANCE_GENERATOR_H + +#include "opengl_urdf_visualizer.h" + +template +struct TinyVisualInstanceGenerator : public VisualInstanceGenerator +{ + OpenGLUrdfVisualizer& viz_; + + TinyVisualInstanceGenerator(OpenGLUrdfVisualizer& viz) : viz_(viz) { + + } + virtual void create_visual_instance(int shape_uid, std::vector& visual_instances ) + { + OpenGLUrdfVisualizer::TinyVisualLinkInfo& vis_link = viz_.m_b2vis[shape_uid]; + for (int v = 0; v < vis_link.visual_shape_uids.size(); v++) + { + int visual_shape = vis_link.visual_shape_uids[v]; + ::TINY::TinyVector3f color(1, 1, 1); + ::TINY::TinyVector3f pos(0, 0, 0); + ::TINY::TinyQuaternionf orn(0, 0, 0, 1); + ::TINY::TinyVector3f scaling(1, 1, 1); + + //visualizer.m_b2vis + int instance = viz_.m_opengl_app.m_renderer->register_graphics_instance( + visual_shape, pos, orn, color, scaling); + visual_instances.push_back(instance); + } + } +}; + +#endif \ No newline at end of file diff --git a/python/pytinydiffsim.inl b/python/pytinydiffsim.inl index e9314f8a..806f9eda 100644 --- a/python/pytinydiffsim.inl +++ b/python/pytinydiffsim.inl @@ -598,16 +598,16 @@ py::class_>(m, "TinyUrdfVisual") .def(py::init<>()) .def_readwrite("origin_xyz", - &UrdfVisual::origin_xyz) + &UrdfVisual::origin_xyz) .def_readwrite("origin_rpy", - &UrdfVisual::origin_rpy) + &UrdfVisual::origin_rpy) .def_readwrite("geometry", &UrdfVisual::geometry) .def_readwrite( "sync_visual_body_uid1", - &UrdfVisual::sync_visual_body_uid1) - .def_readwrite( - "sync_visual_body_uid2", - &UrdfVisual::sync_visual_body_uid2); + &UrdfVisual::visual_shape_uid); + //.def_readwrite( + // "sync_visual_body_uid2", + // &UrdfVisual::sync_visual_body_uid2); py::class_>(m, "TinyUrdfJoint") .def(py::init<>()) diff --git a/python/pytinydiffsim_includes.h b/python/pytinydiffsim_includes.h index 2901a0de..4de851b9 100644 --- a/python/pytinydiffsim_includes.h +++ b/python/pytinydiffsim_includes.h @@ -38,7 +38,7 @@ struct UrdfToMultiBody2 { ::tds::World* world, ::tds::MultiBody* mb) { ::tds::UrdfToMultiBody::convert_to_multi_body( - *urdf_structures, *world, *mb); + *urdf_structures, *world, *mb, 0); mb->initialize(); } diff --git a/src/dynamics/forward_dynamics.hpp b/src/dynamics/forward_dynamics.hpp index c9093e6e..93770222 100644 --- a/src/dynamics/forward_dynamics.hpp +++ b/src/dynamics/forward_dynamics.hpp @@ -46,7 +46,7 @@ void forward_dynamics(MultiBody &mb, forward_kinematics(mb, q, qd); - for (int i = static_cast(mb.size()) - 1; i >= 0; i--) { + for (int i = static_cast(mb.num_links()) - 1; i >= 0; i--) { const Link &link = mb[i]; int parent = link.parent_index; link.U = link.abi * link.S; @@ -172,7 +172,7 @@ void forward_dynamics(MultiBody &mb, mb.base_acceleration() = -spatial_gravity; } - for (int i = 0; i < static_cast(mb.size()); i++) { + for (int i = 0; i < static_cast(mb.num_links()); i++) { const Link &link = mb[i]; int parent = link.parent_index; const Transform &X_parent = link.X_parent; diff --git a/src/dynamics/jacobian.hpp b/src/dynamics/jacobian.hpp index 038e6020..aec18352 100644 --- a/src/dynamics/jacobian.hpp +++ b/src/dynamics/jacobian.hpp @@ -24,7 +24,7 @@ typename Algebra::Matrix3X point_jacobian( assert(Algebra::size(q) == mb.dof()); - assert(link_index < static_cast(mb.size())); + assert(link_index < static_cast(mb.num_links())); Matrix3X jac( 3, mb.dof_qd()); Algebra::set_zero(jac); std::vector links_X_world; diff --git a/src/dynamics/kinematics.hpp b/src/dynamics/kinematics.hpp index edffa1eb..b25a9ad0 100644 --- a/src/dynamics/kinematics.hpp +++ b/src/dynamics/kinematics.hpp @@ -50,7 +50,7 @@ void forward_kinematics( Algebra::cross(mb.base_velocity(), I0_mul_v0) - mb.base_applied_force(); } - for (int i = 0; i < static_cast(mb.size()); i++) { + for (int i = 0; i < static_cast(mb.num_links()); i++) { const Link &link = mb[i]; int parent = link.parent_index; @@ -173,13 +173,13 @@ void forward_kinematics_q( } else { *base_X_world = mb.base_X_world(); } - if (links_X_world) links_X_world->resize(mb.size()); - if (links_X_base) links_X_base->resize(mb.size()); + if (links_X_world) links_X_world->resize(mb.num_links()); + if (links_X_base) links_X_base->resize(mb.num_links()); Transform x_j; Transform x_parent; Transform ident; ident.set_identity(); - for (std::size_t i = 0; i < mb.size(); i++) { + for (std::size_t i = 0; i < mb.num_links(); i++) { const Link &link = mb[i]; int parent = link.parent_index; diff --git a/src/dynamics/mass_matrix.hpp b/src/dynamics/mass_matrix.hpp index a9684b61..6e74c6b2 100644 --- a/src/dynamics/mass_matrix.hpp +++ b/src/dynamics/mass_matrix.hpp @@ -27,7 +27,7 @@ void mass_matrix(MultiBody &mb, const typename Algebra::VectorX &q, assert(Algebra::size(q) == mb.dof()); assert(M != nullptr); - int n = static_cast(mb.size()); + int n = static_cast(mb.num_links()); // printf("n is %i\n", n); assert(Algebra::num_rows(*M) == mb.dof_qd()); assert(Algebra::num_cols(*M) == mb.dof_qd()); diff --git a/src/link.hpp b/src/link.hpp index 447c6a28..979c04fd 100644 --- a/src/link.hpp +++ b/src/link.hpp @@ -62,7 +62,7 @@ struct Link { std::vector *> collision_geometries; std::vector X_collisions; // offset of collision geometries // (relative to this link frame) - std::vector visual_ids; + std::vector visual_instance_uids; std::vector X_visuals; // offset of geometry (relative to this link frame) diff --git a/src/multi_body.hpp b/src/multi_body.hpp index 075e5678..b0e76902 100644 --- a/src/multi_body.hpp +++ b/src/multi_body.hpp @@ -61,8 +61,8 @@ class MultiBody { mutable ArticulatedBodyInertia base_abi_; // IA_0 mutable Transform base_X_world_; - std::vector visual_ids_; - std::vector visual_ids2_; + std::vector visual_shape_uids_; + std::vector visual_instance_uids_; // offset of geometry (relative to the base frame) std::vector X_visuals_; @@ -94,7 +94,7 @@ class MultiBody { conv.links_.push_back(link.template clone()); } conv.control_indices_ = control_indices_; - conv.visual_ids_ = visual_ids_; + conv.visual_shape_ids_ = visual_shape_ids_; for (const auto &x : X_visuals_) { conv.X_visuals_.push_back(x.template clone()); } @@ -114,7 +114,7 @@ class MultiBody { const std::string &name() const { return name_; } TINY_INLINE const LinkCollection &links() const { return links_; } - TINY_INLINE std::size_t size() const { return links_.size(); } + TINY_INLINE std::size_t num_links() const { return links_.size(); } TINY_INLINE const Link &operator[](std::size_t i) const { return links_[i]; } TINY_INLINE Link &operator[](std::size_t i) { return links_[i]; } TINY_INLINE typename LinkCollection::iterator begin() { @@ -212,11 +212,11 @@ class MultiBody { return base_X_world_; } - TINY_INLINE std::vector &visual_ids() { return visual_ids_; } - TINY_INLINE const std::vector &visual_ids() const { return visual_ids_; } + TINY_INLINE std::vector &visual_shape_ids() { return visual_shape_ids_; } + TINY_INLINE const std::vector &visual_shape_ids() const { return visual_shape_ids_; } - TINY_INLINE std::vector& visual_ids2() { return visual_ids2_; } - TINY_INLINE const std::vector& visual_ids2() const { return visual_ids2_; } + TINY_INLINE std::vector& visual_instance_uids() { return visual_instance_uids_; } + TINY_INLINE const std::vector& visual_instance_uids() const { return visual_instance_uids_; } TINY_INLINE std::vector &X_visuals() { return X_visuals_; } TINY_INLINE const std::vector &X_visuals() const { diff --git a/src/urdf/pybullet_urdf_import.hpp b/src/urdf/pybullet_urdf_import.hpp index cf25adc6..b40fb569 100644 --- a/src/urdf/pybullet_urdf_import.hpp +++ b/src/urdf/pybullet_urdf_import.hpp @@ -150,8 +150,8 @@ struct PyBulletUrdfImport { static void sync_graphics_transforms(const MultiBody* body, PyBulletVisualizerAPI* viz_api) { - for (std::size_t v = 0; v < body->visual_ids().size(); v++) { - int visual_id = body->visual_ids()[v]; + for (std::size_t v = 0; v < body->visual_instance_uids().size(); v++) { + int visual_id = body->visual_instance_uids()[v]; Transform geom_X_world = body->base_X_world() * body->X_visuals()[v]; btVector3 base_pos(Algebra::to_double(geom_X_world.translation[0]), Algebra::to_double(geom_X_world.translation[1]), @@ -165,9 +165,9 @@ struct PyBulletUrdfImport { viz_api->resetBasePositionAndOrientation(visual_id, base_pos, base_orn); } - for (std::size_t l = 0; l < body->size(); l++) { - for (std::size_t v = 0; v < (*body)[l].visual_ids.size(); v++) { - int visual_id = (*body)[l].visual_ids[v]; + for (std::size_t l = 0; l < body->num_links(); l++) { + for (std::size_t v = 0; v < (*body)[l].visual_instance_uids.size(); v++) { + int visual_id = (*body)[l].visual_instance_uids[v]; Transform geom_X_world = (*body)[l].X_world * (*body)[l].X_visuals[v]; btVector3 base_pos(Algebra::to_double(geom_X_world.translation[0]), Algebra::to_double(geom_X_world.translation[1]), @@ -396,11 +396,7 @@ struct PyBulletUrdfImport { if (vizShape < 0) { printf("Couldn't create sphere shape\n"); } - b3RobotSimulatorCreateMultiBodyArgs args2; - args2.m_baseVisualShapeIndex = vizShape; - args2.m_baseMass = 0; - int viz_uid = viz_api->createMultiBody(args2); - visual_shape.sync_visual_body_uid1 = viz_uid; + visual_shape.visual_shape_uid = vizShape; break; } case TINY_CAPSULE_TYPE: { @@ -413,11 +409,7 @@ struct PyBulletUrdfImport { if (vizShape < 0) { printf("Couldn't create capsule shape\n"); } - b3RobotSimulatorCreateMultiBodyArgs args2; - args2.m_baseVisualShapeIndex = vizShape; - args2.m_baseMass = 0; - int viz_uid = viz_api->createMultiBody(args2); - visual_shape.sync_visual_body_uid1 = viz_uid; + visual_shape.visual_shape_uid = vizShape; break; } case TINY_BOX_TYPE: { @@ -428,11 +420,7 @@ struct PyBulletUrdfImport { Algebra::to_double(he[1]), Algebra::to_double(he[2])); int vizShape = viz_api->createVisualShape(GEOM_BOX, args); - b3RobotSimulatorCreateMultiBodyArgs args2; - args2.m_baseVisualShapeIndex = vizShape; - args2.m_baseMass = 0; - int viz_uid = viz_api->createMultiBody(args2); - visual_shape.sync_visual_body_uid1 = viz_uid; + visual_shape.visual_shape_uid = vizShape; break; } case TINY_MESH_TYPE: { @@ -454,16 +442,8 @@ struct PyBulletUrdfImport { args2.m_baseVisualShapeIndex = vizShape; args2.m_baseMass = 0; - int viz_uid = viz_api->createMultiBody(args2); - { - b3RobotSimulatorChangeVisualShapeArgs args_change; - args_change.m_objectUniqueId = viz_uid; - args_change.m_linkIndex = -1; - args_change.m_hasRgbaColor = true; - args_change.m_rgbaColor.setValue(1, 1, 1, 1); - viz_api->changeVisualShape(args_change); - } - visual_shape.sync_visual_body_uid1 = viz_uid; + visual_shape.visual_shape_uid = vizShape; + break; } default: { diff --git a/src/urdf/urdf_cache.hpp b/src/urdf/urdf_cache.hpp index fc747e63..5b2a5df2 100644 --- a/src/urdf/urdf_cache.hpp +++ b/src/urdf/urdf_cache.hpp @@ -79,7 +79,7 @@ class UrdfCache { const std::string& name = "") { MultiBody* mb = world.create_multi_body(name); const auto& urdf_data = retrieve(urdf_filename, ignore_cache); - UrdfToMultiBody::convert_to_multi_body(urdf_data, world, *mb); + UrdfToMultiBody::convert_to_multi_body(urdf_data, world, *mb, 0); mb->set_floating_base(is_floating); mb->initialize(); return mb; diff --git a/src/urdf/urdf_structures.hpp b/src/urdf/urdf_structures.hpp index a7e75dcf..4b1c04d7 100644 --- a/src/urdf/urdf_structures.hpp +++ b/src/urdf/urdf_structures.hpp @@ -148,9 +148,7 @@ struct UrdfVisual { : origin_rpy(Algebra::zero3()), origin_xyz(Algebra::zero3()), has_local_material(false), - sync_visual_body_uid1(-1), - sync_visual_body_uid2(-1) - + visual_shape_uid(-1) {} Vector3 origin_rpy; Vector3 origin_xyz; @@ -159,8 +157,7 @@ struct UrdfVisual { VisualMaterial material; std::string visual_name; bool has_local_material; - int sync_visual_body_uid1; - int sync_visual_body_uid2; + int visual_shape_uid; }; template diff --git a/src/urdf/urdf_to_multi_body.hpp b/src/urdf/urdf_to_multi_body.hpp index e0f96056..2a296566 100644 --- a/src/urdf/urdf_to_multi_body.hpp +++ b/src/urdf/urdf_to_multi_body.hpp @@ -22,6 +22,11 @@ #include "../world.hpp" #include "urdf_structures.hpp" +struct VisualInstanceGenerator +{ + virtual void create_visual_instance(int shape_uid, std::vector& instances)=0; +}; + namespace tds { template struct UrdfToMultiBody { @@ -33,7 +38,8 @@ struct UrdfToMultiBody { static int convert_to_multi_body(const UrdfStructures& urdf_structures, World& world, - MultiBody& mb) { + MultiBody& mb, + VisualInstanceGenerator* vig) { int return_code = kCONVERSION_OK; // start with base properties @@ -55,11 +61,13 @@ struct UrdfToMultiBody { Matrix3 inertia_C = rot * inertia_diag; mb.base_rbi_ = RigidBodyInertia(mass, com, inertia_C); } - for (std::size_t i = 0; i < base_link.urdf_visual_shapes.size(); i++) { const UrdfVisual& visual_shape = base_link.urdf_visual_shapes[i]; - mb.visual_ids_.push_back(visual_shape.sync_visual_body_uid1); + if (vig) + { + vig->create_visual_instance(visual_shape.visual_shape_uid, mb.visual_instance_uids_); + } Transform visual_offset; visual_offset.translation = Vector3(visual_shape.origin_xyz[0], visual_shape.origin_xyz[1], @@ -174,7 +182,10 @@ struct UrdfToMultiBody { for (std::size_t i = 0; i < link.urdf_visual_shapes.size(); i++) { const UrdfVisual& visual_shape = link.urdf_visual_shapes[i]; - l.visual_ids.push_back(visual_shape.sync_visual_body_uid1); + if (vig) + { + vig->create_visual_instance(visual_shape.visual_shape_uid, l.visual_instance_uids); + } Transform visual_offset; visual_offset.translation = Vector3(visual_shape.origin_xyz[0], visual_shape.origin_xyz[1], diff --git a/src/visualizer/meshcat/meshcat_urdf_visualizer.h b/src/visualizer/meshcat/meshcat_urdf_visualizer.h index 113bd993..ed9110f5 100644 --- a/src/visualizer/meshcat/meshcat_urdf_visualizer.h +++ b/src/visualizer/meshcat/meshcat_urdf_visualizer.h @@ -170,7 +170,7 @@ struct MeshcatUrdfVisualizer { } void convert_link_visuals(TinyUrdfLink &link, int link_index, - bool useTextureUuid) { + bool useTextureUuid, std::vector& visual_instance_uids) { for (int vis_index = 0; vis_index < (int)link.urdf_visual_shapes.size(); vis_index++) { UrdfVisual &v = @@ -231,13 +231,14 @@ struct MeshcatUrdfVisualizer { } } } - v.sync_visual_body_uid1 = m_uid; + + visual_instance_uids.push_back(m_uid); m_b2vis[m_uid++] = b2v; } } void convert_visuals(TinyUrdfStructures &urdf, - const std::string &texture_path) { + const std::string &texture_path, TinyMultiBody* body) { load_texture(texture_path); m_link_name_to_index.clear(); @@ -245,21 +246,24 @@ struct MeshcatUrdfVisualizer { int link_index = -1; std::string link_name = urdf.base_links[0].link_name; m_link_name_to_index[link_name] = link_index; - convert_link_visuals(urdf.base_links[0], link_index, false); + std::vector indices; + convert_link_visuals(urdf.base_links[0], link_index, false, body? body->visual_instance_uids() : indices); + } for (int link_index = 0; link_index < (int)urdf.links.size(); link_index++) { std::string link_name = urdf.links[link_index].link_name; m_link_name_to_index[link_name] = link_index; - convert_link_visuals(urdf.links[link_index], link_index, false); + std::vector indices; + convert_link_visuals(urdf.links[link_index], link_index, false, body? body->links_[link_index].visual_instance_uids : indices); } } void sync_visual_transforms(const TinyMultiBody *body) { // sync base transform - for (int v = 0; v < body->visual_ids().size(); v++) { - int visual_id = body->visual_ids()[v]; + for (int v = 0; v < body->visual_instance_uids().size(); v++) { + int visual_id = body->visual_instance_uids()[v]; if (m_b2vis.find(visual_id) != m_b2vis.end()) { Quaternion rot; Transform geom_X_world = @@ -283,8 +287,8 @@ struct MeshcatUrdfVisualizer { } for (int l = 0; l < body->links().size(); l++) { - for (int v = 0; v < body->links()[l].visual_ids.size(); v++) { - int visual_id = body->links()[l].visual_ids[v]; + for (int v = 0; v < body->links()[l].visual_instance_uids.size(); v++) { + int visual_id = body->links()[l].visual_instance_uids[v]; if (m_b2vis.find(visual_id) != m_b2vis.end()) { Quaternion rot; Transform geom_X_world = diff --git a/src/visualizer/opengl/tiny_gl_instancing_renderer.cpp b/src/visualizer/opengl/tiny_gl_instancing_renderer.cpp index 4066cc30..50069d8a 100644 --- a/src/visualizer/opengl/tiny_gl_instancing_renderer.cpp +++ b/src/visualizer/opengl/tiny_gl_instancing_renderer.cpp @@ -240,9 +240,9 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData { m_shadowMap(0), m_shadowTexture(0), m_renderFrameBuffer(0), - m_shadowMapWidth(4096), - m_shadowMapHeight(4096), - m_shadowMapWorldSize(10), + m_shadowMapWidth(8192), + m_shadowMapHeight(8192), + m_shadowMapWorldSize(25), m_updateShadowMap(true) { @@ -916,7 +916,7 @@ int TinyGLInstancingRenderer::register_graphics_instance_internal( printf("register_graphics_instance out of range, %d\n", maxElements); return -1; } - return newUid; // gfxObj->m_numGraphicsInstances; + return newUid; // gfxObj->m_numGraphicsInstances; } int TinyGLInstancingRenderer::register_graphics_instance( diff --git a/test/rbdl_test_utils.hpp b/test/rbdl_test_utils.hpp index e3ae0e64..35e32c31 100644 --- a/test/rbdl_test_utils.hpp +++ b/test/rbdl_test_utils.hpp @@ -259,7 +259,7 @@ bool is_equal(const MultiBody &tds, // floating-base models in RBDL have a TranslationXYZ and a spherical joint // first std::size_t id_offset = tds.is_floating() ? 3 : 1; - for (std::size_t j = 0; j < tds.size(); ++j) { + for (std::size_t j = 0; j < tds.num_links(); ++j) { std::size_t rbdl_j = j + id_offset; if (!is_equal(tds[j].X_world, rbdl.X_base[rbdl_j])) { fprintf(stderr, "Mismatch in X_base (X_world) at link %i.\n", diff --git a/test/test_rbdl.cpp b/test/test_rbdl.cpp index c0686462..1904b5fa 100644 --- a/test/test_rbdl.cpp +++ b/test/test_rbdl.cpp @@ -36,7 +36,7 @@ void TestOnURDF(std::string filename) UrdfStructures urdf_structures = parser.load_urdf(full_path); UrdfToMultiBody::convert_to_multi_body( - urdf_structures, world, mb1); + urdf_structures, world, mb1, 0); mb1.initialize(); #endif//USE_BULLET_URDF_PARSER From c5ec371f65e6a6bc89a5b07888bc617d244802b2 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 17 Dec 2020 09:40:59 -0800 Subject: [PATCH 06/13] add laikago_opengl_example, eigen and tiny --- examples/CMakeLists.txt | 12 + examples/laikago_opengl_example.cpp | 417 ++++++++++++++++++++++++++++ src/dynamics/integrator.hpp | 2 +- 3 files changed, 430 insertions(+), 1 deletion(-) create mode 100644 examples/laikago_opengl_example.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index c7440939..b9e12232 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -11,6 +11,18 @@ if (Eigen3_FOUND) add_executable(pendulum_example_eigen_gui pendulum_example_gui.cpp ${TDS_HDRS}) target_link_libraries(pendulum_example_eigen_gui opengl_window Eigen3::Eigen ${CMAKE_DL_LIBS}) target_include_directories(pendulum_example_eigen_gui PRIVATE ../src) + +add_executable(laikago_opengl_eigen_example laikago_opengl_example.cpp ../third_party/stb_image/stb_image.cpp ../third_party/tinyobjloader/tiny_obj_loader.cc ${TDS_HDRS}) +target_link_libraries(laikago_opengl_eigen_example Eigen3::Eigen tinyxml2 opengl_window ${CMAKE_DL_LIBS}) +target_include_directories(laikago_opengl_eigen_example PRIVATE ../third_party/tinyobjloader) +target_include_directories(laikago_opengl_eigen_example PRIVATE ../third_party ../src) + +add_executable(laikago_opengl_tiny_example laikago_opengl_example.cpp ../third_party/stb_image/stb_image.cpp ../third_party/tinyobjloader/tiny_obj_loader.cc ${TDS_HDRS}) +target_link_libraries(laikago_opengl_tiny_example Eigen3::Eigen tinyxml2 opengl_window ${CMAKE_DL_LIBS}) +target_include_directories(laikago_opengl_tiny_example PRIVATE ../third_party/tinyobjloader) +target_include_directories(laikago_opengl_tiny_example PRIVATE ../third_party ../src) +target_compile_definitions(laikago_opengl_tiny_example PRIVATE USE_TINY ) + endif() add_executable(billiard_opt_example_gui billiard_opt_example_gui.cpp ${TDS_HDRS}) diff --git a/examples/laikago_opengl_example.cpp b/examples/laikago_opengl_example.cpp new file mode 100644 index 00000000..887063df --- /dev/null +++ b/examples/laikago_opengl_example.cpp @@ -0,0 +1,417 @@ +// Copyright 2020 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +//#include "meshcat_urdf_visualizer.h" +#include "opengl_urdf_visualizer.h" + +#include "math/tiny/tiny_double_utils.h" +#include "utils/file_utils.hpp" +#include "urdf/urdf_parser.hpp" +#include "urdf/urdf_to_multi_body.hpp" +#include "dynamics/forward_dynamics.hpp" +#include "dynamics/integrator.hpp" + +#include "urdf/urdf_cache.hpp" +#include "tiny_visual_instance_generator.h" + +using namespace tds; + +#ifdef USE_TINY +#include "math/tiny/tiny_algebra.hpp" +using namespace TINY; +typedef double TinyDualScalar; +typedef double MyScalar; +typedef ::TINY::DoubleUtils MyTinyConstants; +typedef TinyVector3 Vector3; +typedef TinyQuaternion Quaternion; +typedef TinyAlgebra MyAlgebra; +#else +#include "math/eigen_algebra.hpp" +typedef EigenAlgebra MyAlgebra; +typedef typename MyAlgebra::Scalar MyScalar; +typedef typename MyAlgebra::Vector3 Vector3; +typedef typename MyAlgebra::Quaternion Quarternion; +typedef typename MyAlgebra::VectorX VectorX; +typedef typename MyAlgebra::Matrix3 Matrix3; +typedef typename MyAlgebra::Matrix3X Matrix3X; +typedef typename MyAlgebra::MatrixX MatrixX; +#endif + + + + + + +int frameskip_gfx_sync = 10; // only sync every 10 frames (sim at 1000 Hz, gfx at ~60hz) + + +bool do_sim = true; + + +TinyKeyboardCallback prev_keyboard_callback = 0; + +void my_keyboard_callback(int keycode, int state) +{ + if (keycode == 's') + do_sim = state; + prev_keyboard_callback(keycode, state); +} + +double knee_angle = -0.5; +double abduction_angle = 0.2; + + +double initial_poses[] = { + abduction_angle, 0., knee_angle, abduction_angle, 0., knee_angle, + abduction_angle, 0., knee_angle, abduction_angle, 0., knee_angle, +}; + + +template +struct LaikagoSimulation { + using Scalar = typename Algebra::Scalar; + tds::UrdfCache cache; + std::string m_urdf_filename; + tds::World world; + tds::MultiBody* system = nullptr; + + int num_timesteps{ 1 }; + Scalar dt{ Algebra::from_double(1e-3) }; + + int input_dim() const { return system->dof() + system->dof_qd(); } + int state_dim() const { + return system->dof() + system->dof_qd() + system->num_links() * 7; + } + int output_dim() const { return num_timesteps * state_dim(); } + + LaikagoSimulation() { + std::string plane_filename; + tds::FileUtils::find_file("plane_implicit.urdf", plane_filename); + cache.construct(plane_filename, world, false, false); + tds::FileUtils::find_file("laikago/laikago_toes_zup.urdf", m_urdf_filename); + system = cache.construct(m_urdf_filename, world, false, true); + system->base_X_world().translation = Algebra::unit3_z(); + } + + std::vector operator()(const std::vector& v) { + assert(static_cast(v.size()) == input_dim()); + system->initialize(); + //copy input into q, qd + for (int i = 0; i < system->dof(); ++i) { + system->q(i) = v[i]; + } + for (int i = 0; i < system->dof_qd(); ++i) { + system->qd(i) = v[i + system->dof()]; + } + std::vector result(output_dim()); + for (int t = 0; t < num_timesteps; ++t) { + +#if 1 + // pd control + if (1) { + // use PD controller to compute tau + int qd_offset = system->is_floating() ? 6 : 0; + int q_offset = system->is_floating() ? 7 : 0; + int num_targets = system->tau_.size() - qd_offset; + std::vector q_targets; + q_targets.resize(system->tau_.size()); + + Scalar kp = 150; + Scalar kd = 3; + Scalar max_force = 550; + int param_index = 0; + + for (int i = 0; i < system->tau_.size(); i++) { + system->tau_[i] = 0; + } + int tau_index = 0; + int pose_index = 0; + for (int i = 0; i < system->links_.size(); i++) { + if (system->links_[i].joint_type != tds::JOINT_FIXED) { + Scalar q_desired = initial_poses[pose_index++]; + Scalar q_actual = system->q_[q_offset]; + Scalar qd_actual = system->qd_[qd_offset]; + Scalar position_error = (q_desired - q_actual); + Scalar desired_velocity = 0; + Scalar velocity_error = (desired_velocity - qd_actual); + Scalar force = kp * position_error + kd * velocity_error; + + force = Algebra::max(force, -max_force); + force = Algebra::min(force, max_force); + + system->tau_[tau_index] = force; + q_offset++; + qd_offset++; + param_index++; + tau_index++; + } + } + } +#endif + tds::forward_dynamics(*system, world.get_gravity()); + system->clear_forces(); + + integrate_euler_qdd(*system, dt); + + world.step(dt); + + tds::integrate_euler(*system, dt); + + //copy q, qd, link world poses (for rendering) to output + int j = 0; + for (int i = 0; i < system->dof(); ++i, ++j) { + result[j] = system->q(i); + } + for (int i = 0; i < system->dof_qd(); ++i, ++j) { + result[j] = system->qd(i); + } + for (const auto link : *system) { + if (link.X_visuals.size()) + { + tds::Transform visual_X_world = link.X_world * link.X_visuals[0]; + result[j++] = visual_X_world.translation[0]; + result[j++] = visual_X_world.translation[1]; + result[j++] = visual_X_world.translation[2]; + Algebra::Quaternion orn = Algebra::matrix_to_quat(visual_X_world.rotation); + result[j++] = orn.x(); + result[j++] = orn.y(); + result[j++] = orn.z(); + result[j++] = orn.w(); + } + else + { + //check if we have links without visuals + assert(0); + j += 7; + } + } + } + return result; + } +}; + + + +int main(int argc, char* argv[]) { + int sync_counter = 0; + int frame = 0; + World world; + UrdfParser parser; + + // create graphics + OpenGLUrdfVisualizer visualizer; + + + + visualizer.delete_all(); + + LaikagoSimulation contact_sim; + + int input_dim = contact_sim.input_dim(); + std::vector prep_inputs; + + std::vector prep_outputs; + prep_inputs.resize(input_dim); + prep_inputs[3] = 1; + prep_inputs[6] = 1; + + //int sphere_shape = visualizer.m_opengl_app.register_graphics_unit_sphere_shape(SPHERE_LOD_LOW); + +#ifdef USE_TINY + { + std::vector shape_ids; + std::string plane_filename; + FileUtils::find_file("plane100.obj", plane_filename); + TinyVector3f pos(0, 0, 0); + TinyQuaternionf orn(0, 0, 0, 1); + TinyVector3f scaling(1, 1, 1); + visualizer.load_obj(plane_filename, pos, orn, scaling, shape_ids); + } +#endif + + //int sphere_shape = shape_ids[0]; + //TinyVector3f color = colors[0]; + // typedef tds::Conversion Conversion; + + bool create_instances = false; + char search_path[TINY_MAX_EXE_PATH_LEN]; + std::string texture_path = ""; + std::string file_and_path; + tds::FileUtils::find_file("laikago/laikago_toes_zup.urdf", file_and_path); + auto urdf_structures = contact_sim.cache.retrieve(file_and_path);// contact_sim.m_urdf_filename); + FileUtils::extract_path(file_and_path.c_str(), search_path, + TINY_MAX_EXE_PATH_LEN); + visualizer.m_path_prefix = search_path; + visualizer.convert_visuals(urdf_structures, texture_path); + + + int num_total_threads = 16; + std::vector visual_instances; + std::vector num_instances; + int num_base_instances; + +#ifdef USE_TINY + for (int t = 0;t< num_total_threads;t++) + { + TinyVector3f pos(0, 0, 0); + TinyQuaternionf orn(0, 0, 0, 1); + TinyVector3f scaling(1, 1, 1); + int uid = urdf_structures.base_links[0].urdf_visual_shapes[0].visual_shape_uid; + OpenGLUrdfVisualizer::TinyVisualLinkInfo& vis_link = visualizer.m_b2vis[uid]; + int instance = -1; + int num_instances_per_link = 0; + for (int v = 0; v < vis_link.visual_shape_uids.size(); v++) + { + int sphere_shape = vis_link.visual_shape_uids[v]; + ::TINY::TinyVector3f color(1, 1, 1); + //visualizer.m_b2vis + instance = visualizer.m_opengl_app.m_renderer->register_graphics_instance( + sphere_shape, pos, orn, color, scaling); + visual_instances.push_back(instance); + num_instances_per_link++; + contact_sim.system->visual_instance_uids().push_back(instance); + } + num_base_instances = num_instances_per_link; + + for (int i = 0; i < contact_sim.system->num_links(); ++i) { + + + int uid = urdf_structures.links[i].urdf_visual_shapes[0].visual_shape_uid; + OpenGLUrdfVisualizer::TinyVisualLinkInfo& vis_link = visualizer.m_b2vis[uid]; + int instance = -1; + int num_instances_per_link = 0; + for (int v = 0; v < vis_link.visual_shape_uids.size(); v++) + { + int sphere_shape = vis_link.visual_shape_uids[v]; + ::TINY::TinyVector3f color(1, 1, 1); + //visualizer.m_b2vis + instance = visualizer.m_opengl_app.m_renderer->register_graphics_instance( + sphere_shape, pos, orn, color, scaling); + visual_instances.push_back(instance); + num_instances_per_link++; + + contact_sim.system->links_[i].visual_instance_uids.push_back(instance); + } + num_instances.push_back(num_instances_per_link); + } + } +#endif + //app.m_renderer->write_single_instance_transform_to_cpu(pos, orn, sphereId); + + + + std::vector> parallel_outputs( + num_total_threads, std::vector(contact_sim.output_dim())); + + std::vector> parallel_inputs(num_total_threads); + + for (int i = 0; i < num_total_threads; ++i) { + parallel_inputs[i] = std::vector(contact_sim.input_dim(), MyScalar(0)); + parallel_inputs[i][3] = 1; + parallel_inputs[i][6] = 1; + + } + + while (!visualizer.m_opengl_app.m_window->requested_exit()) { + + for (int i = 0; i < num_total_threads; ++i) { + parallel_outputs[i] = contact_sim(parallel_inputs[i]); + for (int j = 0; j < contact_sim.input_dim(); ++j) { + parallel_inputs[i][j] = parallel_outputs[i][j]; + } + } + + sync_counter++; + frame += 1; + if (sync_counter > frameskip_gfx_sync) { + sync_counter = 0; + if (1) { + bool manual_sync = false; + if (manual_sync) + { + //visualizer.sync_visual_transforms(contact_sim.system); + } + else + { + float sim_spacing = 2; + const int square_id = (int)std::sqrt((double)num_total_threads); + int instance_index = 0; + int offset = contact_sim.system->dof() + contact_sim.system->dof_qd(); + for (int s = 0; s < num_total_threads; s++) + { + + + for (int v = 0; v < num_base_instances; v++) + { + int visual_instance_id = visual_instances[instance_index++]; + if (visual_instance_id >= 0) + { + + ::TINY::TinyVector3f pos(parallel_outputs[s][4 + 0], + parallel_outputs[s][4 + 1], + parallel_outputs[s][4 + 2]); + ::TINY::TinyQuaternionf orn(parallel_outputs[s][0], + parallel_outputs[s][1], + parallel_outputs[s][2], + parallel_outputs[s][3]); + + pos[0] += sim_spacing * (s % square_id) - square_id * sim_spacing / 2; + pos[1] += sim_spacing * (s / square_id) - square_id * sim_spacing / 2; + + visualizer.m_opengl_app.m_renderer->write_single_instance_transform_to_cpu(pos, orn, visual_instance_id); + } + } + + for (int l = 0; l < contact_sim.system->links_.size(); l++) { + for (int v = 0; v < num_instances[l]; v++) + { + int visual_instance_id = visual_instances[instance_index++]; + if (visual_instance_id >= 0) + { + + ::TINY::TinyVector3f pos(parallel_outputs[s][offset + l * 7 + 0], + parallel_outputs[s][offset + l * 7 + 1], + parallel_outputs[s][offset + l * 7 + 2]); + ::TINY::TinyQuaternionf orn(parallel_outputs[s][offset + l * 7 + 3], + parallel_outputs[s][offset + l * 7 + 4], + parallel_outputs[s][offset + l * 7 + 5], + parallel_outputs[s][offset + l * 7 + 6]); + + pos[0] += sim_spacing * (s % square_id) - square_id * sim_spacing / 2; + pos[1] += sim_spacing * (s / square_id) - square_id * sim_spacing / 2; + + visualizer.m_opengl_app.m_renderer->write_single_instance_transform_to_cpu(pos, orn, visual_instance_id); + } + } + } + } + } + } + visualizer.render(); + //std::this_thread::sleep_for(std::chrono::duration(frameskip_gfx_sync* contact_sim.dt)); + } + } + + printf("finished\n"); + return EXIT_SUCCESS; + +} + diff --git a/src/dynamics/integrator.hpp b/src/dynamics/integrator.hpp index 0e34ec91..a7333264 100644 --- a/src/dynamics/integrator.hpp +++ b/src/dynamics/integrator.hpp @@ -128,6 +128,6 @@ template void integrate_euler_qdd(MultiBody& mb, const typename Algebra::Scalar& dt) { integrate_euler_qdd(mb, mb.q(), mb.qd(), mb.qdd(), dt); - mb.qdd().set_zero(); + Algebra::set_zero(mb.qdd()); } } // namespace tds \ No newline at end of file From 553b1a4ecbee095b7e871d5f401797aca4fe6e0a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 17 Dec 2020 10:23:02 -0800 Subject: [PATCH 07/13] fix Eigen block assignment usage enable laikago_opengl_eigen_example. --- examples/laikago_opengl_example.cpp | 14 +++++++------- src/math/eigen_algebra.hpp | 7 +++++-- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/examples/laikago_opengl_example.cpp b/examples/laikago_opengl_example.cpp index 887063df..92b72710 100644 --- a/examples/laikago_opengl_example.cpp +++ b/examples/laikago_opengl_example.cpp @@ -33,10 +33,11 @@ #include "tiny_visual_instance_generator.h" using namespace tds; +using namespace TINY; +#include "math/tiny/tiny_algebra.hpp" #ifdef USE_TINY -#include "math/tiny/tiny_algebra.hpp" -using namespace TINY; + typedef double TinyDualScalar; typedef double MyScalar; typedef ::TINY::DoubleUtils MyTinyConstants; @@ -235,7 +236,7 @@ int main(int argc, char* argv[]) { //int sphere_shape = visualizer.m_opengl_app.register_graphics_unit_sphere_shape(SPHERE_LOD_LOW); -#ifdef USE_TINY + { std::vector shape_ids; std::string plane_filename; @@ -245,7 +246,7 @@ int main(int argc, char* argv[]) { TinyVector3f scaling(1, 1, 1); visualizer.load_obj(plane_filename, pos, orn, scaling, shape_ids); } -#endif + //int sphere_shape = shape_ids[0]; //TinyVector3f color = colors[0]; @@ -266,9 +267,8 @@ int main(int argc, char* argv[]) { int num_total_threads = 16; std::vector visual_instances; std::vector num_instances; - int num_base_instances; + int num_base_instances = 0; -#ifdef USE_TINY for (int t = 0;t< num_total_threads;t++) { TinyVector3f pos(0, 0, 0); @@ -313,7 +313,7 @@ int main(int argc, char* argv[]) { num_instances.push_back(num_instances_per_link); } } -#endif + //app.m_renderer->write_single_instance_transform_to_cpu(pos, orn, sphereId); diff --git a/src/math/eigen_algebra.hpp b/src/math/eigen_algebra.hpp index e9a49e42..10c5dbd8 100644 --- a/src/math/eigen_algebra.hpp +++ b/src/math/eigen_algebra.hpp @@ -417,8 +417,11 @@ struct EigenAlgebraT { EIGEN_ALWAYS_INLINE static void assign_row(MatrixX &m, Index i, const SpatialVector &v) { - m.block(i, 0, 1, 3) = v.top; - m.block(i, 3, 1, 3) = v.bottom; + + m.block<1, 3>(i, 0) = v.top; + m.block<1, 3>(i, 3) = v.bottom; + + } EIGEN_ALWAYS_INLINE static void assign_horizontal(MatrixX &mat, From 0e72a9de27e5c4c0905562da96b271315ca897ff Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 17 Dec 2020 18:35:38 -0800 Subject: [PATCH 08/13] add cuda_laikago_codegen example, still has some unknown issue (Laikago collapsing then flying off) CUDA kernel compiling takes ~3 minutes on a high-end AMD Ryzen 3900x add _DEBUG flag in debug mode Don't call Eigen .normalize, since it has a comparison > 0, which fails CppADCodegen Windows request 'proper' OpenGL 3.3 context, so that RenderDoc works forward dynamics: use mb.base_acceleration() = -mb.base_abi().inv_mul instead of Algebra::inverse(mb.base_abi().matrix()) * mb.base_bias_force()); (which fails the CppADCodegen) Use Algebra::to_double in visualizer. clarify input 1 (quaternion 'w') and 0.7 (start height) --- CMakeLists.txt | 1 + examples/CMakeLists.txt | 15 + examples/cuda_laikago_codegen.cpp | 431 ++++++++++++++++++ examples/laikago_opengl_example.cpp | 20 +- examples/opengl_urdf_visualizer.h | 4 +- src/dynamics/forward_dynamics.hpp | 6 +- src/math/eigen_algebra.hpp | 9 +- .../opengl/tiny_win32_opengl_window.cpp | 129 +++++- 8 files changed, 601 insertions(+), 14 deletions(-) create mode 100644 examples/cuda_laikago_codegen.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6c96cdbc..2f48d72a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) endif() endif() +set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG") if(COMMAND cmake_policy) cmake_policy(SET CMP0003 NEW) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index b9e12232..299c875e 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -54,6 +54,21 @@ IF(USE_CPPAD) if (USE_STAN) target_link_libraries(cuda_codegen PRIVATE ${STAN_LIBRARIES}) endif(USE_STAN) + + add_executable(cuda_laikago_codegen cuda_laikago_codegen.cpp ${TDS_HDRS}) + target_compile_definitions(cuda_laikago_codegen PRIVATE -DUSE_CPPAD) + target_link_libraries( + cuda_laikago_codegen + PRIVATE + ${CMAKE_DL_LIBS} + opengl_window + tinyxml2 + Eigen3::Eigen + differentiation + ) + if (USE_STAN) + target_link_libraries(cuda_laikago_codegen PRIVATE ${STAN_LIBRARIES}) + endif(USE_STAN) ENDIF(USE_CPPAD) IF(USE_CERES) diff --git a/examples/cuda_laikago_codegen.cpp b/examples/cuda_laikago_codegen.cpp new file mode 100644 index 00000000..392acfaf --- /dev/null +++ b/examples/cuda_laikago_codegen.cpp @@ -0,0 +1,431 @@ +// clang-format off +#include "utils/differentiation.hpp" +#include "utils/cuda_codegen.hpp" +#include "dynamics/forward_dynamics.hpp" +#include "dynamics/integrator.hpp" +#include "math/tiny/tiny_algebra.hpp" +#include "urdf/urdf_cache.hpp" +#include "utils/stopwatch.hpp" +#include "visualizer/opengl/visualizer.h" +#include "opengl_urdf_visualizer.h" +#include "utils/file_utils.hpp" + +#include +#include +#include +#include +#include +#include + +// clang-format on +using namespace TINY; + +double knee_angle = -0.5; +double abduction_angle = 0.2; +double initial_poses[] = { + abduction_angle, 0., knee_angle, abduction_angle, 0., knee_angle, + abduction_angle, 0., knee_angle, abduction_angle, 0., knee_angle, +}; + +// function to be converted to CUDA code +template +struct LaikagoSimulation { + using Scalar = typename Algebra::Scalar; + tds::UrdfCache cache; + std::string m_urdf_filename; + tds::World world; + tds::MultiBody* system = nullptr; + + int num_timesteps{ 1 }; + Scalar dt{ Algebra::from_double(1e-3) }; + + int input_dim() const { return system->dof() + system->dof_qd(); } + int state_dim() const { + return system->dof() + system->dof_qd() + system->num_links() * 7; + } + int output_dim() const { return num_timesteps * state_dim(); } + + LaikagoSimulation() { + std::string plane_filename; + tds::FileUtils::find_file("plane_implicit.urdf", plane_filename); + cache.construct(plane_filename, world, false, false); + tds::FileUtils::find_file("laikago/laikago_toes_zup.urdf", m_urdf_filename); + system = cache.construct(m_urdf_filename, world, false, true); + system->base_X_world().translation = Algebra::unit3_z(); + } + + std::vector operator()(const std::vector& v) { + assert(static_cast(v.size()) == input_dim()); + system->initialize(); + //copy input into q, qd + for (int i = 0; i < system->dof(); ++i) { + system->q(i) = v[i]; + } + for (int i = 0; i < system->dof_qd(); ++i) { + system->qd(i) = v[i + system->dof()]; + } + std::vector result(output_dim()); + for (int t = 0; t < num_timesteps; ++t) { + + + // pd control + if (1) { + // use PD controller to compute tau + int qd_offset = system->is_floating() ? 6 : 0; + int q_offset = system->is_floating() ? 7 : 0; + int num_targets = system->tau_.size() - qd_offset; + std::vector q_targets; + q_targets.resize(system->tau_.size()); + + Scalar kp = 150; + Scalar kd = 3; + Scalar max_force = 550; + int param_index = 0; + + for (int i = 0; i < system->tau_.size(); i++) { + system->tau_[i] = 0; + } + int tau_index = 0; + int pose_index = 0; + for (int i = 0; i < system->links_.size(); i++) { + if (system->links_[i].joint_type != tds::JOINT_FIXED) { + Scalar q_desired = initial_poses[pose_index++]; + Scalar q_actual = system->q_[q_offset]; + Scalar qd_actual = system->qd_[qd_offset]; + Scalar position_error = (q_desired - q_actual); + Scalar desired_velocity = 0; + Scalar velocity_error = (desired_velocity - qd_actual); + Scalar force = kp * position_error + kd * velocity_error; + + force = Algebra::max(force, -max_force); + force = Algebra::min(force, max_force); + + system->tau_[tau_index] = force; + q_offset++; + qd_offset++; + param_index++; + tau_index++; + } + } + } + + tds::forward_dynamics(*system, world.get_gravity()); + + system->clear_forces(); + + integrate_euler_qdd(*system, dt); + + world.step(dt); + + tds::integrate_euler(*system, dt); + + //copy q, qd, link world poses (for rendering) to output + int j = 0; + for (int i = 0; i < system->dof(); ++i, ++j) { + result[j] = system->q(i); + } + for (int i = 0; i < system->dof_qd(); ++i, ++j) { + result[j] = system->qd(i); + } + for (const auto link : *system) { + //assert(link.X_visuals.size()); + { + tds::Transform visual_X_world = link.X_world * link.X_visuals[0]; + result[j++] = visual_X_world.translation[0]; + result[j++] = visual_X_world.translation[1]; + result[j++] = visual_X_world.translation[2]; + Algebra::Quaternion orn = Algebra::matrix_to_quat(visual_X_world.rotation); + result[j++] = orn.x(); + result[j++] = orn.y(); + result[j++] = orn.z(); + result[j++] = orn.w(); + } + + } + } + return result; + } +}; + + + +int main(int argc, char* argv[]) { + using Scalar = double; + using CGScalar = typename CppAD::cg::CG; + using Dual = typename CppAD::AD; + + using DiffAlgebra = + tds::default_diff_algebra::type; + + LaikagoSimulation simulation; + + // trace function with all zeros as input + std::vector ax(simulation.input_dim(), Dual(0)); + //quaternion 'w' = 1 + ax[3] = 1; + //height of Laikago at 0.7 meter + ax[6] = 0.7; + + CppAD::Independent(ax); + std::vector ay(simulation.output_dim()); + std::cout << "Tracing function for code generation...\n"; + ay = simulation(ax); + + CppAD::ADFun tape; + tape.Dependent(ax, ay); + tape.optimize(); + + tds::Stopwatch timer; + timer.start(); + std::string model_name = "cuda_model"; + tds::CudaSourceGen cgen(tape, model_name); + cgen.setCreateForwardZero(true); + + tds::CudaLibraryProcessor p(&cgen); +#if CPPAD_CG_SYSTEM_WIN + std::string nvcc_path = tds::exec("where nvcc"); +#else + std::string nvcc_path = tds::exec("which nvcc"); +#endif + std::cout << "Using [" << nvcc_path << "]" << std::endl; + p.nvcc_path() = nvcc_path; + p.generate_code(); + + //comment-out to re-use previously build CUDA shared library + p.create_library(); + + // CppAD::cg::ModelLibraryCSourceGen libcgen(cgen); + // libcgen.setVerbose(true); + // CppAD::cg::DynamicModelLibraryProcessor p(libcgen); + // auto compiler = std::make_unique>(); + // compiler->setSourcesFolder("cgen_srcs"); + // compiler->setSaveToDiskFirst(true); + // compiler->addCompileFlag("-O" + std::to_string(1)); + // p.setLibraryName(model_name); + // p.createDynamicLibrary(*compiler, false); + + // create model to load shared library + tds::CudaModel model(model_name); + + // how many threads to run on the GPU + int num_total_threads = 1024; + + std::vector> outputs( + num_total_threads, std::vector(simulation.output_dim())); + + std::vector> inputs(num_total_threads); + + OpenGLUrdfVisualizer visualizer; + visualizer.delete_all(); + TinyOpenGL3App& app = visualizer.m_opengl_app; + //TinyOpenGL3App app("CUDA Pendulum", 1024, 768); + app.m_renderer->init(); + int upAxis = 2; + app.set_up_axis(upAxis); + app.m_renderer->get_active_camera()->set_camera_distance(4); + app.m_renderer->get_active_camera()->set_camera_pitch(-30); + app.m_renderer->get_active_camera()->set_camera_target_position(0, 0, 0); + // install ffmpeg in path and uncomment, to enable video recording + // app.dump_frames_to_video("test.mp4"); + + + + + { + std::vector shape_ids; + std::string plane_filename; + tds::FileUtils::find_file("plane100.obj", plane_filename); + TinyVector3f pos(0, 0, 0); + TinyQuaternionf orn(0, 0, 0, 1); + TinyVector3f scaling(1, 1, 1); + visualizer.load_obj(plane_filename, pos, orn, scaling, shape_ids); + } + + + bool create_instances = false; + char search_path[TINY_MAX_EXE_PATH_LEN]; + std::string texture_path = ""; + std::string file_and_path; + tds::FileUtils::find_file("laikago/laikago_toes_zup.urdf", file_and_path); + auto urdf_structures = simulation.cache.retrieve(file_and_path);// contact_sim.m_urdf_filename); + tds::FileUtils::extract_path(file_and_path.c_str(), search_path, + TINY_MAX_EXE_PATH_LEN); + visualizer.m_path_prefix = search_path; + visualizer.convert_visuals(urdf_structures, texture_path); + + + + std::vector visual_instances; + std::vector num_instances; + int num_base_instances = 0; + int sync_counter = 0; + int frameskip_gfx_sync = 1; + + for (int t = 0; t < num_total_threads; t++) + { + TinyVector3f pos(0, 0, 0); + TinyQuaternionf orn(0, 0, 0, 1); + TinyVector3f scaling(1, 1, 1); + int uid = urdf_structures.base_links[0].urdf_visual_shapes[0].visual_shape_uid; + OpenGLUrdfVisualizer::TinyVisualLinkInfo& vis_link = visualizer.m_b2vis[uid]; + int instance = -1; + int num_instances_per_link = 0; + for (int v = 0; v < vis_link.visual_shape_uids.size(); v++) + { + int sphere_shape = vis_link.visual_shape_uids[v]; + ::TINY::TinyVector3f color(1, 1, 1); + //visualizer.m_b2vis + instance = visualizer.m_opengl_app.m_renderer->register_graphics_instance( + sphere_shape, pos, orn, color, scaling); + visual_instances.push_back(instance); + num_instances_per_link++; + simulation.system->visual_instance_uids().push_back(instance); + } + num_base_instances = num_instances_per_link; + + for (int i = 0; i < simulation.system->num_links(); ++i) { + + + int uid = urdf_structures.links[i].urdf_visual_shapes[0].visual_shape_uid; + OpenGLUrdfVisualizer::TinyVisualLinkInfo& vis_link = visualizer.m_b2vis[uid]; + int instance = -1; + int num_instances_per_link = 0; + for (int v = 0; v < vis_link.visual_shape_uids.size(); v++) + { + int sphere_shape = vis_link.visual_shape_uids[v]; + ::TINY::TinyVector3f color(1, 1, 1); + //visualizer.m_b2vis + instance = visualizer.m_opengl_app.m_renderer->register_graphics_instance( + sphere_shape, pos, orn, color, scaling); + visual_instances.push_back(instance); + num_instances_per_link++; + + simulation.system->links_[i].visual_instance_uids.push_back(instance); + } + num_instances.push_back(num_instances_per_link); + } + } + + + model.forward_zero.allocate(num_total_threads); + + std::vector< TinyVector3f> positions; + std::vector indices; + + TinyVector3f line_color(0.3, 0.3, 0.3); + float line_width = 1; + const int link_pos_id_offset = + simulation.system->dof() + simulation.system->dof_qd(); + const int square_id = (int)std::sqrt((double)num_total_threads); + //sim_spacing is the visual distance between independent parallel simulations + const float sim_spacing = 5.f; + for (int run = 0; run < 40; ++run) { + for (int i = 0; i < num_total_threads; ++i) { + inputs[i] = std::vector(simulation.input_dim(), Scalar(0)); + //quaternion 'w' = 1 + inputs[i][3] = 1; + //height of Laikago at 0.7 meter + inputs[i][6] = 0.7; + } + for (int t = 0; t < 1000; ++t) { + + positions.resize(0); + indices.resize(0); + + timer.start(); + // call GPU kernel + model.forward_zero(&outputs, inputs, 64); + timer.stop(); + std::cout << "Kernel execution took " << timer.elapsed() << " seconds.\n"; + + for (int i = 0; i < num_total_threads; ++i) { + for (int j = 0; j < simulation.input_dim(); ++j) { + inputs[i][j] = outputs[i][j]; + } + } + + sync_counter++; + + if (sync_counter > frameskip_gfx_sync) { + sync_counter = 0; + if (1) { + bool manual_sync = false; + if (manual_sync) + { + //visualizer.sync_visual_transforms(contact_sim.system); + } + else + { + float sim_spacing = 2; + const int square_id = (int)std::sqrt((double)num_total_threads); + int instance_index = 0; + int offset = simulation.system->dof() + simulation.system->dof_qd(); + for (int s = 0; s < num_total_threads; s++) + { + + + for (int v = 0; v < num_base_instances; v++) + { + int visual_instance_id = visual_instances[instance_index++]; + if (visual_instance_id >= 0) + { + + ::TINY::TinyVector3f pos(outputs[s][4 + 0], + outputs[s][4 + 1], + outputs[s][4 + 2]); + ::TINY::TinyQuaternionf orn(outputs[s][0], + outputs[s][1], + outputs[s][2], + outputs[s][3]); + + pos[0] += sim_spacing * (s % square_id) - square_id * sim_spacing / 2; + pos[1] += sim_spacing * (s / square_id) - square_id * sim_spacing / 2; + + visualizer.m_opengl_app.m_renderer->write_single_instance_transform_to_cpu(pos, orn, visual_instance_id); + } + } + + for (int l = 0; l < simulation.system->links_.size(); l++) { + for (int v = 0; v < num_instances[l]; v++) + { + int visual_instance_id = visual_instances[instance_index++]; + if (visual_instance_id >= 0) + { + + ::TINY::TinyVector3f pos(outputs[s][offset + l * 7 + 0], + outputs[s][offset + l * 7 + 1], + outputs[s][offset + l * 7 + 2]); + ::TINY::TinyQuaternionf orn(outputs[s][offset + l * 7 + 3], + outputs[s][offset + l * 7 + 4], + outputs[s][offset + l * 7 + 5], + outputs[s][offset + l * 7 + 6]); + + pos[0] += sim_spacing * (s % square_id) - square_id * sim_spacing / 2; + pos[1] += sim_spacing * (s / square_id) - square_id * sim_spacing / 2; + + visualizer.m_opengl_app.m_renderer->write_single_instance_transform_to_cpu(pos, orn, visual_instance_id); + } + } + } + } + } + } + visualizer.render(); + //std::this_thread::sleep_for(std::chrono::duration(frameskip_gfx_sync* contact_sim.dt)); + } + + + } + } + + model.forward_zero.deallocate(); + + for (const auto& thread : outputs) { + for (const Scalar& t : thread) { + std::cout << t << " "; + } + std::cout << std::endl; + } + + return EXIT_SUCCESS; +} diff --git a/examples/laikago_opengl_example.cpp b/examples/laikago_opengl_example.cpp index 92b72710..801eef85 100644 --- a/examples/laikago_opengl_example.cpp +++ b/examples/laikago_opengl_example.cpp @@ -60,8 +60,11 @@ typedef typename MyAlgebra::MatrixX MatrixX; - +#ifdef _DEBUG +int frameskip_gfx_sync = 1; // don't skip, we are debugging +#else int frameskip_gfx_sync = 10; // only sync every 10 frames (sim at 1000 Hz, gfx at ~60hz) +#endif bool do_sim = true; @@ -214,14 +217,11 @@ struct LaikagoSimulation { int main(int argc, char* argv[]) { int sync_counter = 0; int frame = 0; - World world; + UrdfParser parser; // create graphics OpenGLUrdfVisualizer visualizer; - - - visualizer.delete_all(); LaikagoSimulation contact_sim; @@ -231,8 +231,10 @@ int main(int argc, char* argv[]) { std::vector prep_outputs; prep_inputs.resize(input_dim); + //quaternion 'w' = 1 prep_inputs[3] = 1; - prep_inputs[6] = 1; + //start height at 0.7 + prep_inputs[6] = 0.7; //int sphere_shape = visualizer.m_opengl_app.register_graphics_unit_sphere_shape(SPHERE_LOD_LOW); @@ -264,7 +266,7 @@ int main(int argc, char* argv[]) { visualizer.convert_visuals(urdf_structures, texture_path); - int num_total_threads = 16; + int num_total_threads = 64; std::vector visual_instances; std::vector num_instances; int num_base_instances = 0; @@ -325,8 +327,10 @@ int main(int argc, char* argv[]) { for (int i = 0; i < num_total_threads; ++i) { parallel_inputs[i] = std::vector(contact_sim.input_dim(), MyScalar(0)); + //quaternion 'w' = 1 parallel_inputs[i][3] = 1; - parallel_inputs[i][6] = 1; + //start height at 0.7 + parallel_inputs[i][6] = 0.7; } diff --git a/examples/opengl_urdf_visualizer.h b/examples/opengl_urdf_visualizer.h index 7f29bcdd..a8b1f6cd 100644 --- a/examples/opengl_urdf_visualizer.h +++ b/examples/opengl_urdf_visualizer.h @@ -169,7 +169,9 @@ struct OpenGLUrdfVisualizer { if (::tds::FileUtils::find_file(org_obj_filename, obj_filename)) { ::TINY::TinyVector3f pos(0, 0, 0); - ::TINY::TinyVector3f scaling(v.geometry.mesh.scale[0], v.geometry.mesh.scale[1], v.geometry.mesh.scale[2]); + ::TINY::TinyVector3f scaling(Algebra::to_double(v.geometry.mesh.scale[0]), + Algebra::to_double(v.geometry.mesh.scale[1]), + Algebra::to_double(v.geometry.mesh.scale[2])); ::TINY::TinyQuaternionf orn(0, 0, 0, 1); load_obj_shapes(obj_filename, b2v.visual_shape_uids, b2v.shape_colors); } diff --git a/src/dynamics/forward_dynamics.hpp b/src/dynamics/forward_dynamics.hpp index 93770222..6c45d03d 100644 --- a/src/dynamics/forward_dynamics.hpp +++ b/src/dynamics/forward_dynamics.hpp @@ -164,9 +164,9 @@ void forward_dynamics(MultiBody &mb, // NEURAL_ASSIGN(base_bias_force[5], "base_bias_force_5"); // #endif - // mb.base_acceleration() = -mb.base_abi().inv_mul(mb.base_bias_force()); - mb.base_acceleration() = -MotionVector( - Algebra::inverse(mb.base_abi().matrix()) * mb.base_bias_force()); + mb.base_acceleration() = -mb.base_abi().inv_mul(mb.base_bias_force()); + //mb.base_acceleration() = -MotionVector( + // Algebra::inverse(mb.base_abi().matrix()) * mb.base_bias_force()); } else { mb.base_acceleration() = -spatial_gravity; diff --git a/src/math/eigen_algebra.hpp b/src/math/eigen_algebra.hpp index 10c5dbd8..3d4cd7bb 100644 --- a/src/math/eigen_algebra.hpp +++ b/src/math/eigen_algebra.hpp @@ -220,7 +220,14 @@ struct EigenAlgebraT { template EIGEN_ALWAYS_INLINE static auto normalize(T &v) { - v.normalize(); + Scalar z = v.squaredNorm(); + //don't call Eigen .normalize, since it has a comparison > 0, which fails CppADCodegen + //assert(z > Scalar(0)); + Scalar invZ = Scalar(1) / sqrt(z); + v.x() *= invZ; + v.y() *= invZ; + v.z() *= invZ; + v.w() *= invZ; return v; } diff --git a/src/visualizer/opengl/tiny_win32_opengl_window.cpp b/src/visualizer/opengl/tiny_win32_opengl_window.cpp index e62657a7..9033defa 100644 --- a/src/visualizer/opengl/tiny_win32_opengl_window.cpp +++ b/src/visualizer/opengl/tiny_win32_opengl_window.cpp @@ -29,7 +29,112 @@ misrepresented as being the original software. #include #include "tiny_win32_internal_window_data.h" +typedef HGLRC WINAPI wglCreateContextAttribsARB_type(HDC hdc, HGLRC hShareContext, + const int* attribList); +wglCreateContextAttribsARB_type* wglCreateContextAttribsARB = 0; + +// See https://www.opengl.org/registry/specs/ARB/wgl_create_context.txt for all values +#define WGL_CONTEXT_MAJOR_VERSION_ARB 0x2091 +#define WGL_CONTEXT_MINOR_VERSION_ARB 0x2092 +#define WGL_CONTEXT_PROFILE_MASK_ARB 0x9126 +#define WGL_CONTEXT_CORE_PROFILE_BIT_ARB 0x00000001 + +typedef BOOL WINAPI wglChoosePixelFormatARB_type(HDC hdc, const int* piAttribIList, + const FLOAT* pfAttribFList, UINT nMaxFormats, int* piFormats, UINT* nNumFormats); +wglChoosePixelFormatARB_type* wglChoosePixelFormatARB = 0; + +void fatal_error(const char* msg) +{ + printf("Error:%s\n", msg); + exit(0); +} + +static void +init_opengl_extensions(void) +{ + // Before we can load extensions, we need a dummy OpenGL context, created using a dummy window. + // We use a dummy window because you can only set the pixel format for a window once. For the + // real window, we want to use wglChoosePixelFormatARB (so we can potentially specify options + // that aren't available in PIXELFORMATDESCRIPTOR), but we can't load and use that before we + // have a context. + WNDCLASSA window_class; + ZeroMemory(&window_class, sizeof(WNDCLASSA)); + + window_class.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC; + window_class.lpfnWndProc = DefWindowProcA; + window_class.hInstance = GetModuleHandle(0); + window_class.lpszClassName = "Dummy_WGL_djuasiodwa"; + + if (!RegisterClassA(&window_class)) { + fatal_error("Failed to register dummy OpenGL window."); + } + + HWND dummy_window = CreateWindowExA( + 0, + window_class.lpszClassName, + "Dummy OpenGL Window", + 0, + CW_USEDEFAULT, + CW_USEDEFAULT, + CW_USEDEFAULT, + CW_USEDEFAULT, + 0, + 0, + window_class.hInstance, + 0); + + if (!dummy_window) { + fatal_error("Failed to create dummy OpenGL window."); + } + + HDC dummy_dc = GetDC(dummy_window); + + PIXELFORMATDESCRIPTOR pfd; + ZeroMemory(&pfd, sizeof(PIXELFORMATDESCRIPTOR)); + + pfd.nSize = sizeof(pfd); + pfd.nVersion = 1; + pfd.iPixelType = PFD_TYPE_RGBA; + pfd.dwFlags = PFD_DRAW_TO_WINDOW | PFD_SUPPORT_OPENGL | PFD_DOUBLEBUFFER; + pfd.cColorBits = 32; + pfd.cAlphaBits = 8; + pfd.iLayerType = PFD_MAIN_PLANE; + pfd.cDepthBits = 24; + pfd.cStencilBits = 8; + + + int pixel_format = ChoosePixelFormat(dummy_dc, &pfd); + if (!pixel_format) { + fatal_error("Failed to find a suitable pixel format."); + } + if (!SetPixelFormat(dummy_dc, pixel_format, &pfd)) { + fatal_error("Failed to set the pixel format."); + } + + HGLRC dummy_context = wglCreateContext(dummy_dc); + if (!dummy_context) { + fatal_error("Failed to create a dummy OpenGL rendering context."); + } + + if (!wglMakeCurrent(dummy_dc, dummy_context)) { + fatal_error("Failed to activate dummy OpenGL rendering context."); + } + + wglCreateContextAttribsARB = (wglCreateContextAttribsARB_type*)wglGetProcAddress( + "wglCreateContextAttribsARB"); + wglChoosePixelFormatARB = (wglChoosePixelFormatARB_type*)wglGetProcAddress( + "wglChoosePixelFormatARB"); + + wglMakeCurrent(dummy_dc, 0); + wglDeleteContext(dummy_context); + ReleaseDC(dummy_window, dummy_dc); + DestroyWindow(dummy_window); +} + void TinyWin32OpenGLWindow::enableOpenGL() { + + init_opengl_extensions(); + PIXELFORMATDESCRIPTOR pfd; int format; @@ -54,8 +159,30 @@ void TinyWin32OpenGLWindow::enableOpenGL() { format = ChoosePixelFormat(m_data->m_hDC, &pfd); SetPixelFormat(m_data->m_hDC, format, &pfd); + + // Specify that we want to create an OpenGL 3.3 core profile context + int gl33_attribs[] = { + WGL_CONTEXT_MAJOR_VERSION_ARB, 3, + WGL_CONTEXT_MINOR_VERSION_ARB, 3, + WGL_CONTEXT_PROFILE_MASK_ARB, WGL_CONTEXT_CORE_PROFILE_BIT_ARB, + 0, + }; + + + if (wglCreateContextAttribsARB) + { + m_data->m_hRC = wglCreateContextAttribsARB(m_data->m_hDC, 0, gl33_attribs); + if (!m_data->m_hRC) { + printf("Failed to create OpenGL 3.3 context."); + exit(0); + } + } + else + { + m_data->m_hRC = wglCreateContext(m_data->m_hDC); + } + // create and enable the render context (RC) - m_data->m_hRC = wglCreateContext(m_data->m_hDC); wglMakeCurrent(m_data->m_hDC, m_data->m_hRC); // printGLString("Extensions", GL_EXTENSIONS); From ad9f7d7e6e1915def360f7677f861f2a67f796b8 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 17 Dec 2020 20:41:18 -0800 Subject: [PATCH 09/13] add DEBUG_MODEL for debugging laikago_cuda_codegen --- examples/cuda_laikago_codegen.cpp | 48 ++++++++++++++++++------------- 1 file changed, 28 insertions(+), 20 deletions(-) diff --git a/examples/cuda_laikago_codegen.cpp b/examples/cuda_laikago_codegen.cpp index 392acfaf..238d4559 100644 --- a/examples/cuda_laikago_codegen.cpp +++ b/examples/cuda_laikago_codegen.cpp @@ -1,3 +1,4 @@ +#define DEBUG_MODEL // clang-format off #include "utils/differentiation.hpp" #include "utils/cuda_codegen.hpp" @@ -191,24 +192,15 @@ int main(int argc, char* argv[]) { p.nvcc_path() = nvcc_path; p.generate_code(); - //comment-out to re-use previously build CUDA shared library - p.create_library(); - - // CppAD::cg::ModelLibraryCSourceGen libcgen(cgen); - // libcgen.setVerbose(true); - // CppAD::cg::DynamicModelLibraryProcessor p(libcgen); - // auto compiler = std::make_unique>(); - // compiler->setSourcesFolder("cgen_srcs"); - // compiler->setSaveToDiskFirst(true); - // compiler->addCompileFlag("-O" + std::to_string(1)); - // p.setLibraryName(model_name); - // p.createDynamicLibrary(*compiler, false); - // create model to load shared library +#ifndef DEBUG_MODEL + //comment-out to re-use previously build CUDA shared library + //p.create_library(); tds::CudaModel model(model_name); +#endif //DEBUG_MODEL // how many threads to run on the GPU - int num_total_threads = 1024; + int num_total_threads = 1; std::vector> outputs( num_total_threads, std::vector(simulation.output_dim())); @@ -306,8 +298,9 @@ int main(int argc, char* argv[]) { } } - +#ifndef DEBUG_MODEL model.forward_zero.allocate(num_total_threads); +#endif //DEBUG_MODEL std::vector< TinyVector3f> positions; std::vector indices; @@ -334,19 +327,33 @@ int main(int argc, char* argv[]) { timer.start(); // call GPU kernel +#ifndef DEBUG_MODEL model.forward_zero(&outputs, inputs, 64); +#endif //DEBUG_MODEL + timer.stop(); std::cout << "Kernel execution took " << timer.elapsed() << " seconds.\n"; for (int i = 0; i < num_total_threads; ++i) { - for (int j = 0; j < simulation.input_dim(); ++j) { - inputs[i][j] = outputs[i][j]; - } +#ifdef DEBUG_MODEL + for (int xx = 0; xx < simulation.input_dim(); xx++) + { + ax[xx] = inputs[i][xx]; + } + ay = simulation(ax); + for (int yy = 0; yy < simulation.output_dim(); yy++) + { + outputs[i][yy] = DiffAlgebra::to_double(ay[yy]); + } +#endif //DEBUG_MODEL + for (int j = 0; j < simulation.input_dim(); ++j) { + inputs[i][j] = outputs[i][j]; + } } sync_counter++; - if (sync_counter > frameskip_gfx_sync) { + if (sync_counter >= frameskip_gfx_sync) { sync_counter = 0; if (1) { bool manual_sync = false; @@ -417,8 +424,9 @@ int main(int argc, char* argv[]) { } } - +#ifndef DEBUG_MODEL model.forward_zero.deallocate(); +#endif //DEBUG_MODEL for (const auto& thread : outputs) { for (const Scalar& t : thread) { From 5440bc9523c363f87e57b62265cbe1e4f32731f7 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 18 Dec 2020 08:48:50 -0800 Subject: [PATCH 10/13] use TDS_DEBUG instead of _DEBUG (it causes issues on MSVC) --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2f48d72a..e48027e1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) endif() endif() -set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG") +set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -DTDS_DEBUG") if(COMMAND cmake_policy) cmake_policy(SET CMP0003 NEW) From 3e44ec81bdc61b8cde3c4b8674713362c89ed627 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 18 Dec 2020 09:34:33 -0800 Subject: [PATCH 11/13] add patch and apply using build_libs_windows.bat --- build_libs_windows.bat | 9 +++++++-- third_party/patches/CppADCodeGen.diff | 13 +++++++++++++ third_party/patches/CppADCodeGen.txt | 4 ++++ 3 files changed, 24 insertions(+), 2 deletions(-) create mode 100644 third_party/patches/CppADCodeGen.diff create mode 100644 third_party/patches/CppADCodeGen.txt diff --git a/build_libs_windows.bat b/build_libs_windows.bat index 30252351..8005bd8f 100644 --- a/build_libs_windows.bat +++ b/build_libs_windows.bat @@ -20,6 +20,11 @@ popd cd %ROOT% +pushd third_party\cppadcodegen +git apply ..\patches\CppADCodeGen.diff +popd +cd %ROOT% + pushd third_party\gflags mkdir build_cmake cd build_cmake @@ -88,5 +93,5 @@ cd build_cmake cmake -DCMAKE_CXX_FLAGS="/MP" -DUSE_CERES=ON -DUSE_MSVC_RUNTIME_LIBRARY_DLL=ON -DBullet_DIR=%ROOT%\third_party\bullet3\build_cmake -Dgflags_DIR=%ROOT%\third_party\gflags\build_cmake -Dglog_DIR=%ROOT%\third_party\glog\build_cmake -DEigen3_DIR=%ROOT%\third_party\eigen3\build_cmake -DCeres_DIR=%ROOT%\third_party\ceres-solver\build_cmake\local_install\cmake -DUSE_CPPAD=ON .. -cmake --build . --target INSTALL --config Release -start DIFF_PHYSICS.sln +rem cmake --build . --target INSTALL --config Release +rem start DIFF_PHYSICS.sln diff --git a/third_party/patches/CppADCodeGen.diff b/third_party/patches/CppADCodeGen.diff new file mode 100644 index 00000000..447acf66 --- /dev/null +++ b/third_party/patches/CppADCodeGen.diff @@ -0,0 +1,13 @@ +diff --git a/include/cppad/cg/nan.hpp b/include/cppad/cg/nan.hpp +index 600d320..d486291 100644 +--- a/include/cppad/cg/nan.hpp ++++ b/include/cppad/cg/nan.hpp +@@ -26,7 +26,7 @@ inline bool isnan(const cg::CG &s) { + } else { + // a parameter + const Base& v = s.getValue(); +- return (v != v); ++ return ::std::isnan(v); + } + } + diff --git a/third_party/patches/CppADCodeGen.txt b/third_party/patches/CppADCodeGen.txt new file mode 100644 index 00000000..9165b116 --- /dev/null +++ b/third_party/patches/CppADCodeGen.txt @@ -0,0 +1,4 @@ +In Visual Studio, when using /fp:fast the check for nan s!=s doesn't work, we need to use std::isnan +This patch requires C++17, CppAD and CppADCodegen is still at C++11/14 +See https://github.com/joaoleal/CppADCodeGen/pull/65 +git apply ..\patches\CppADCodeGen.diff From c044c71fc36ea5b79c74dc5aa46dd020891d4d82 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 18 Dec 2020 11:38:55 -0800 Subject: [PATCH 12/13] remove debug cmake setting, it messes up the build in MSVC --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e48027e1..5a09732d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) endif() endif() -set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -DTDS_DEBUG") + if(COMMAND cmake_policy) cmake_policy(SET CMP0003 NEW) From 697a8d4cebd581e8a99b92a06267d7fb35d2127f Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 18 Dec 2020 15:04:37 -0800 Subject: [PATCH 13/13] enable cuda_laikago_codegen, runs smooth for 2048 Laikagos simulating in real-time! fix matrix inverse used for Eigen/CppAD/CppADCodeGen, removing conditional (our matrices are positive semi-definite) --- examples/cuda_laikago_codegen.cpp | 24 +++-- src/math/eigen_algebra.hpp | 154 +++++++++++++++++++----------- src/mb_constraint_solver.hpp | 5 +- 3 files changed, 115 insertions(+), 68 deletions(-) diff --git a/examples/cuda_laikago_codegen.cpp b/examples/cuda_laikago_codegen.cpp index 238d4559..7ca28724 100644 --- a/examples/cuda_laikago_codegen.cpp +++ b/examples/cuda_laikago_codegen.cpp @@ -1,4 +1,4 @@ -#define DEBUG_MODEL +//#define DEBUG_MODEL // clang-format off #include "utils/differentiation.hpp" #include "utils/cuda_codegen.hpp" @@ -47,6 +47,7 @@ struct LaikagoSimulation { int output_dim() const { return num_timesteps * state_dim(); } LaikagoSimulation() { + //world.set_gravity(Algebra::zero3()); std::string plane_filename; tds::FileUtils::find_file("plane_implicit.urdf", plane_filename); cache.construct(plane_filename, world, false, false); @@ -67,8 +68,6 @@ struct LaikagoSimulation { } std::vector result(output_dim()); for (int t = 0; t < num_timesteps; ++t) { - - // pd control if (1) { // use PD controller to compute tau @@ -141,7 +140,7 @@ struct LaikagoSimulation { result[j++] = orn.z(); result[j++] = orn.w(); } - + } } return result; @@ -150,6 +149,7 @@ struct LaikagoSimulation { + int main(int argc, char* argv[]) { using Scalar = double; using CGScalar = typename CppAD::cg::CG; @@ -200,7 +200,13 @@ int main(int argc, char* argv[]) { #endif //DEBUG_MODEL // how many threads to run on the GPU - int num_total_threads = 1; + int num_total_threads = 2048; + + for (int i = 1; i < argc; i++) { + if (!strcmp(argv[i], "-n") && (i + 1 < argc) && argv[i + 1][0] != '-') + num_total_threads = atoi(argv[++i]); + } + std::vector> outputs( num_total_threads, std::vector(simulation.output_dim())); @@ -251,7 +257,7 @@ int main(int argc, char* argv[]) { std::vector num_instances; int num_base_instances = 0; int sync_counter = 0; - int frameskip_gfx_sync = 1; + int frameskip_gfx_sync = 10; for (int t = 0; t < num_total_threads; t++) { @@ -312,13 +318,13 @@ int main(int argc, char* argv[]) { const int square_id = (int)std::sqrt((double)num_total_threads); //sim_spacing is the visual distance between independent parallel simulations const float sim_spacing = 5.f; - for (int run = 0; run < 40; ++run) { + while (!visualizer.m_opengl_app.m_window->requested_exit()) { for (int i = 0; i < num_total_threads; ++i) { inputs[i] = std::vector(simulation.input_dim(), Scalar(0)); //quaternion 'w' = 1 inputs[i][3] = 1; //height of Laikago at 0.7 meter - inputs[i][6] = 0.7; + inputs[i][6] = 0.7 + std::rand() * 0.5 / RAND_MAX; } for (int t = 0; t < 1000; ++t) { @@ -428,12 +434,14 @@ int main(int argc, char* argv[]) { model.forward_zero.deallocate(); #endif //DEBUG_MODEL +#if 0 for (const auto& thread : outputs) { for (const Scalar& t : thread) { std::cout << t << " "; } std::cout << std::endl; } +#endif return EXIT_SUCCESS; } diff --git a/src/math/eigen_algebra.hpp b/src/math/eigen_algebra.hpp index 3d4cd7bb..0ace8e50 100644 --- a/src/math/eigen_algebra.hpp +++ b/src/math/eigen_algebra.hpp @@ -112,73 +112,113 @@ struct EigenAlgebraT { } /** - * CppAD-friendly matrix inverse operation that assumes the input matrix is - * positive-definite. - */ - static void plain_symmetric_inverse(const MatrixX &mat, MatrixX &mat_inv) { - assert(mat.rows() == mat.cols()); - VectorX diagonal = mat.diagonal(); - mat_inv = mat; - const int n = mat.rows(); - int i, j, k; - Scalar sum; - for (i = 0; i < n; i++) { - mat_inv(i, i) = one() / diagonal[i]; - for (j = i + 1; j < n; j++) { - sum = zero(); - for (k = i; k < j; k++) { - sum -= mat_inv(j, k) * mat_inv(k, i); - } - mat_inv(j, i) = sum / diagonal[j]; - } - } - for (i = 0; i < n; i++) { - for (j = i + 1; j < n; j++) { - mat_inv(i, j) = zero(); + * main method for Cholesky decomposition. + * input/output a Symmetric positive def. matrix + * output diagonal vector of resulting diag of a + * inspired by public domain https://math.nist.gov/javanumerics/jama + */ + + static bool cholesky_decomposition( + MatrixX& a, + VectorX& diagonal) { + int i, j, k; + Scalar sum; + int n = a.cols(); + bool is_positive_definite = true; + for (i = 0; i < n; i++) { + for (j = i; j < n; j++) { + sum = a(i,j); + for (k = i - 1; k >= 0; k--) { + sum -= a(i,k) * a(j,k); + } + if (i == j) { + //if (sum <= zero()) { + // is_positive_definite = false; + // break; + //} + diagonal(i) = sqrt(sum); + } + else { + a(j,i) = sum / diagonal[i]; + } + } } - } - for (i = 0; i < n; i++) { - mat_inv(i, i) = mat_inv(i, i) * mat_inv(i, i); - for (k = i + 1; k < n; k++) { - mat_inv(i, i) += mat_inv(k, i) * mat_inv(k, i); - } - for (j = i + 1; j < n; j++) { - for (k = j; k < n; k++) { - mat_inv(i, j) += mat_inv(k, i) * mat_inv(k, j); - } + return is_positive_definite; + } + + + static bool inverse_cholesky_decomposition( const MatrixX& A, + MatrixX& a) { + int i, j, k; + int n = A.rows(); + Scalar sum; + VectorX diagonal(A.rows()); + for (i = 0; i < n; i++) + for (j = 0; j < n; j++) a(i,j) = A(i,j); + bool is_positive_definite = cholesky_decomposition(a, diagonal); + if (is_positive_definite) { + for (i = 0; i < n; i++) { + a(i,i) = one() / diagonal[i]; + for (j = i + 1; j < n; j++) { + sum = zero(); + for (k = i; k < j; k++) { + sum -= a(j,k) * a(k,i); + } + a(j,i) = sum / diagonal[j]; + } + } } - } - for (i = 0; i < n; i++) { - for (j = 0; j < i; j++) { - mat_inv(i, j) = mat_inv(j, i); + else + { + printf("no!\n"); } - } + return is_positive_definite; } /** - * Returns true if the matrix `mat` is positive-definite, and assigns - * `mat_inv` to the inverse of mat. - * `mat` must be a symmetric matrix. + * Inverse of a matrix, using Cholesky decomposition. + * + * input A Symmetric positive def. matrix + * input a storage for the result + * output boolean is_positive_definite if operation succeeded */ - static bool symmetric_inverse(const MatrixX &mat, MatrixX &mat_inv) { - if constexpr (!is_cppad_scalar::value) { - Eigen::LLT llt(mat); - if (llt.info() == Eigen::NumericalIssue) { - return false; + static bool inverse(const MatrixX& A, MatrixX& a) { + assert(a.cols() == A.cols()); + assert(a.rows() == A.rows()); + + bool is_positive_definite = inverse_cholesky_decomposition(A, a); + if (is_positive_definite) { + int n = A.cols(); + int i, j, k; + + for (i = 0; i < n; i++) { + for (j = i + 1; j < n; j++) { + a(i,j) = zero(); + } + } + + for (i = 0; i < n; i++) { + a(i,i) = a(i,i) * a(i,i); + for (k = i + 1; k < n; k++) { + a(i,i) += a(k,i) * a(k,i); + } + for (j = i + 1; j < n; j++) { + for (k = j; k < n; k++) { + a(i,j) += a(k,i) * a(k,j); + } + } + } + for (i = 0; i < n; i++) { + for (j = 0; j < i; j++) { + a(i,j) = a(j,i); + } + } } - mat_inv = mat.inverse(); - } else { - plain_symmetric_inverse(mat, mat_inv); - // // FIXME the atomic op needs to remain in memory but it will fail when - // the - // // dimensions of the input matrix are not always the same - // using InnerScalar = typename Scalar::value_type; - // static atomic_eigen_mat_inv mat_inv_op; - // mat_inv = mat_inv_op.op(mat); - } - return true; + return is_positive_definite; } + + /** * V = mv(w, v) * F = mv(n, f) diff --git a/src/mb_constraint_solver.hpp b/src/mb_constraint_solver.hpp index 5fd724ab..59070b8c 100644 --- a/src/mb_constraint_solver.hpp +++ b/src/mb_constraint_solver.hpp @@ -191,7 +191,7 @@ class MultiBodyConstraintSolver { // } else { submit_profile_timing("inverse_mass_matrix_a"); is_positive_definite_a = - Algebra::symmetric_inverse(mass_matrix_a, mass_matrix_a_inv); + Algebra::inverse(mass_matrix_a, mass_matrix_a_inv); submit_profile_timing(""); // } } @@ -209,8 +209,7 @@ class MultiBodyConstraintSolver { // is_positive_definite_b = true; // } else { submit_profile_timing("inverse_mass_matrix_b"); - is_positive_definite_b = - Algebra::symmetric_inverse(mass_matrix_b, mass_matrix_b_inv); + is_positive_definite_b = Algebra::inverse(mass_matrix_b, mass_matrix_b_inv); submit_profile_timing(""); // } }