diff --git a/src/fkcc_gen.cc b/src/fkcc_gen.cc index c17055e..08bf092 100644 --- a/src/fkcc_gen.cc +++ b/src/fkcc_gen.cc @@ -1,20 +1,3 @@ -#include "pinocchio_cppadcg.hh" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - #include #include #include @@ -25,393 +8,10 @@ #include #include -#include "lang_cpp.hh" -#include "lang_rust.hh" - -using namespace pinocchio; -using namespace CppAD; -using namespace CppAD::cg; - -// Typedef for AD types -using CGD = CG; -using ADCG = AD; - -using ADModel = ModelTpl; -using ADData = DataTpl; -using ADVectorXs = Eigen::Matrix; - -struct SphereInfo -{ - std::size_t geom_index; - float radius; - std::size_t parent_joint; - std::size_t parent_frame; - SE3 relative; -}; - -auto min_sphere_of_spheres(const std::vector &info) -> std::array -{ - using K = CGAL::Exact_predicates_inexact_constructions_kernel; - using Traits = CGAL::Min_sphere_of_spheres_d_traits_3; - using Sphere = Traits::Sphere; - using Point = K::Point_3; - using MinSphere = CGAL::Min_sphere_of_spheres_d; - - std::vector cgal_spheres; - cgal_spheres.reserve(info.size()); - - for (const auto &sphere : info) - { - auto pos = sphere.relative.translation(); - cgal_spheres.emplace_back(Point(pos[0], pos[1], pos[2]), sphere.radius); - } - - MinSphere ms(cgal_spheres.begin(), cgal_spheres.end()); - std::array sphere; - std::copy(ms.center_cartesian_begin(), ms.center_cartesian_end(), sphere.begin()); - sphere[3] = ms.radius(); - return sphere; -} - -struct RobotInfo -{ - RobotInfo( - const std::filesystem::path &urdf_file, - const std::optional &srdf_file, - const std::optional &end_effector) - { - if (not std::filesystem::exists(urdf_file)) - { - throw std::runtime_error(fmt::format("URDF file {} does not exist!", urdf_file.string())); - } - - pinocchio::urdf::buildModel(urdf_file, model); - pinocchio::urdf::buildGeom(model, urdf_file, COLLISION, collision_model); - - if (srdf_file and not std::filesystem::exists(*srdf_file)) - { - throw std::runtime_error(fmt::format("SRDF file () does not exist!", srdf_file->string())); - } - else if (not srdf_file) - { - fmt::print("No SRDF file provided, guessing collisions!\n"); - guess_self_collisions(); - } - else - { - collision_model.addAllCollisionPairs(); - pinocchio::srdf::removeCollisionPairs(model, collision_model, *srdf_file); - extract_collision_data(); - } - - extract_spheres(); - - if (not end_effector) - { - end_effector_name = model.frames[model.nframes - 1].name; - fmt::print("No EE provided, using distal link `{}`.\n", end_effector_name); - } - else if (not model.existFrame(*end_effector)) - { - throw std::runtime_error(fmt::format("Invalid EE name {}", *end_effector)); - } - else - { - end_effector_name = *end_effector; - } - - end_effector_index = model.getFrameId(end_effector_name); - } - - auto json() -> nlohmann::json - { - const Eigen::VectorXd lower_bound = model.lowerPositionLimit; - const Eigen::VectorXd upper_bound = model.upperPositionLimit; - const Eigen::VectorXd bound_range = upper_bound - lower_bound; - const Eigen::VectorXd bound_descale = bound_range.cwiseInverse(); - - nlohmann::json json; - json["n_q"] = model.nq; - json["n_spheres"] = spheres.size(); - json["bound_lower"] = std::vector(lower_bound.data(), lower_bound.data() + model.nq); - json["bound_range"] = std::vector(bound_range.data(), bound_range.data() + model.nq); - json["bound_descale"] = std::vector(bound_descale.data(), bound_descale.data() + model.nq); - json["measure"] = bound_range.prod(); - json["end_effector"] = end_effector_name; - json["end_effector_index"] = end_effector_index; - json["min_radius"] = min_radius; - json["max_radius"] = max_radius; - json["joint_names"] = dof_to_joint_names(); - json["allowed_link_pairs"] = allowed_link_pairs; - json["per_link_spheres"] = per_link_spheres; - json["links_with_geometry"] = links_with_geometry; - json["bounding_sphere_index"] = bounding_sphere_index; - json["end_effector_collisions"] = get_frames_colliding_end_effector(); - - std::vector link_names; - for (auto i = 0U; i < model.frames.size(); ++i) - { - link_names.emplace_back(model.frames[i].name); - } - json["link_names"] = link_names; - - return json; - } - - auto dof_to_joint_names() -> std::vector - { - std::vector dof_to_joint_id(model.nq); - for (auto joint_id = 1U; joint_id < model.joints.size(); ++joint_id) - { - const auto &joint = model.joints[joint_id]; - auto start_idx = joint.idx_q(); - auto nq = joint.nq(); - - for (auto i = 0U; i < nq; ++i) - { - dof_to_joint_id[start_idx + i] = joint_id; - } - } - - std::vector dof_to_joint_name(model.nq); - for (auto i = 0U; i < model.nq; ++i) - { - dof_to_joint_name[i] = model.names[dof_to_joint_id[i]]; - } - - return dof_to_joint_name; - } - - auto get_frames_colliding_end_effector() -> std::vector - { - std::size_t end_effector_joint = model.frames[end_effector_index].parentJoint; - - std::vector frames; - for (auto i = 0U; i < model.frames.size(); ++i) - { - if (model.frames[i].parentJoint == end_effector_joint) - { - if (bounding_spheres.find(i) != bounding_spheres.end()) - { - frames.emplace_back(i); - } - } - } - - std::set end_effector_allowed_collisions; - for (const auto &[first, second] : allowed_link_pairs) - { - if (std::find(frames.begin(), frames.end(), first) != frames.end()) - { - end_effector_allowed_collisions.emplace(second); - } +#include "robot_info.hh" +#include "housekeeping.hh" +#include "tracer_utils.hh" - if (std::find(frames.begin(), frames.end(), second) != frames.end()) - { - end_effector_allowed_collisions.emplace(first); - } - } - - return std::vector( - end_effector_allowed_collisions.begin(), end_effector_allowed_collisions.end()); - } - - auto extract_spheres() -> void - { - for (auto i = 0U; i < collision_model.ngeoms; ++i) - { - const auto &geom_obj = collision_model.geometryObjects[i]; - const auto &sphere_ptr = std::dynamic_pointer_cast(geom_obj.geometry); - - if (sphere_ptr) - { - SphereInfo info; - info.geom_index = i; - info.radius = sphere_ptr->radius; - info.parent_joint = geom_obj.parentJoint; - info.parent_frame = geom_obj.parentFrame; - info.relative = geom_obj.placement; - - spheres.emplace_back(info); - - min_radius = std::min(min_radius, info.radius); - max_radius = std::max(max_radius, info.radius); - } - else - { - throw std::runtime_error( - fmt::format("Invalid non-sphere geometry in URDF {}", geom_obj.name)); - } - } - - std::size_t bs = 0; - for (auto i = 0U; i < model.frames.size(); ++i) - { - std::vector link_info; - std::vector sphere_indices; - for (const auto &info : spheres) - { - if (info.parent_frame == i) - { - link_info.emplace_back(info); - sphere_indices.emplace_back(info.geom_index); - } - } - - per_link_spheres.emplace_back(sphere_indices); - - if (not link_info.empty()) - { - auto sphere = min_sphere_of_spheres(link_info); - - SphereInfo info; - info.geom_index = bs; - info.radius = sphere[3]; - info.parent_joint = link_info[0].parent_joint; - info.relative = SE3::Identity(); - info.relative.translation()[0] = sphere[0]; - info.relative.translation()[1] = sphere[1]; - info.relative.translation()[2] = sphere[2]; - - bounding_spheres.emplace(i, info); - bounding_sphere_index.emplace_back(bs); - links_with_geometry.emplace_back(i); - bs++; - } - else - { - bounding_sphere_index.emplace_back(0); - } - } - } - - auto collision_pair_to_frame_pair(const CollisionPair &cp) -> std::pair - { - const auto &geom1 = collision_model.geometryObjects[cp.first]; - const auto &geom2 = collision_model.geometryObjects[cp.second]; - - std::size_t link1_idx = geom1.parentFrame; - std::size_t link2_idx = geom2.parentFrame; - - return std::make_pair(std::min(link1_idx, link2_idx), std::max(link1_idx, link2_idx)); - } - - auto extract_collision_data() -> void - { - for (const auto &cp : collision_model.collisionPairs) - { - allowed_link_pairs.insert(collision_pair_to_frame_pair(cp)); - } - } - - auto get_adjacent_frames() -> std::set> - { - const auto nf = model.frames.size(); - const auto nj = model.joints.size(); - - std::set> adjacents; - - for (auto i = 0U; i < nf; ++i) - { - for (auto j = i + 1; j < nf; ++j) - { - const auto &frame_i = model.frames[i]; - const auto &frame_j = model.frames[j]; - - if (frame_i.parentJoint < nj and frame_j.parentJoint < nj) - { - const auto &joint_i = model.joints[frame_i.parentJoint]; - const auto &joint_j = model.joints[frame_j.parentJoint]; - - // Check if joints are parent-child related - if (model.parents[frame_i.parentJoint] == frame_j.parentJoint or - model.parents[frame_j.parentJoint] == frame_i.parentJoint) - { - adjacents.insert({i, j}); - } - } - } - } - - return adjacents; - } - - auto guess_self_collisions(std::size_t n = 1000000U) -> void - { - collision_model.addAllCollisionPairs(); - - Data data(model); - GeometryData collision_data(collision_model); - - std::set> always_pairs; - - for (auto j = 0U; j < collision_model.collisionPairs.size(); ++j) - { - always_pairs.emplace(collision_pair_to_frame_pair(collision_model.collisionPairs[j])); - } - - allowed_link_pairs.clear(); - - for (auto i = 0U; i < n; ++i) - { - auto q = randomConfiguration(model); - computeCollisions(model, data, collision_model, collision_data, q); - - for (auto j = 0U; j < collision_model.collisionPairs.size(); ++j) - { - const auto &cr = collision_data.collisionResults[j]; - auto pair = collision_pair_to_frame_pair(collision_model.collisionPairs[j]); - - if (cr.isCollision()) - { - allowed_link_pairs.insert(pair); - } - else - { - auto it = always_pairs.find(pair); - if (it != always_pairs.end()) - { - always_pairs.erase(it); - } - } - } - } - - // Remove all adjacent frames - auto adjacents = get_adjacent_frames(); - for (const auto &pair : adjacents) - { - allowed_link_pairs.erase(pair); - } - - // Remove all pairs that never collided - for (const auto &pair : always_pairs) - { - allowed_link_pairs.erase(pair); - } - - // Add remaining potential collisions - collision_model.removeAllCollisionPairs(); - for (const auto &pair : allowed_link_pairs) - { - collision_model.addCollisionPair(CollisionPair(pair.first, pair.second)); - } - } - - Model model; - GeometryModel collision_model; - std::string end_effector_name; - std::size_t end_effector_index; - - float min_radius{std::numeric_limits::max()}; - float max_radius{std::numeric_limits::min()}; - std::vector spheres; - std::map bounding_spheres; - std::vector links_with_geometry; - std::vector> per_link_spheres; - std::set> allowed_link_pairs; - std::vector bounding_sphere_index; -}; auto trace_sphere(const SphereInfo &sphere, const ADData &ad_data, ADVectorXs &data, std::size_t index) { @@ -453,13 +53,6 @@ auto trace_frame(std::size_t ee_index, const ADData &ad_data, ADVectorXs &data, data[index + 11] = R(2, 2); } -struct Traced -{ - std::string code; - std::size_t temp_variables; - std::size_t outputs; -}; - auto trace_sphere_cc_fk( const RobotInfo &info, const std::string &language, diff --git a/src/housekeeping.hh b/src/housekeeping.hh new file mode 100644 index 0000000..69d50b7 --- /dev/null +++ b/src/housekeeping.hh @@ -0,0 +1,17 @@ +#include "pinocchio_cppadcg.hh" +#include "lang_cpp.hh" +#include "lang_rust.hh" + + +using namespace pinocchio; +using namespace CppAD; +using namespace CppAD::cg; + +// Typedef for AD types +using CGD = CG; +using ADCG = AD; + +using ADModel = ModelTpl; +using ADData = DataTpl; +using ADVectorXs = Eigen::Matrix; +using ADMatrixXs = Eigen::Matrix; \ No newline at end of file diff --git a/src/robot_info.hh b/src/robot_info.hh new file mode 100644 index 0000000..d84c57f --- /dev/null +++ b/src/robot_info.hh @@ -0,0 +1,405 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + + +// #include "pinocchio/multibody/model.hpp" +// #include "pinocchio/multibody/data.hpp" +// #include "pinocchio/algorithm/compute-all-terms.hpp" +#include "pinocchio/algorithm/center-of-mass.hpp" + + +#include + +#include +#include +#include + +#include +#include +#include +#include + +using namespace pinocchio; + + +struct SphereInfo +{ + std::size_t geom_index; + float radius; + std::size_t parent_joint; + std::size_t parent_frame; + SE3 relative; +}; + +auto min_sphere_of_spheres(const std::vector &info) -> std::array +{ + using K = CGAL::Exact_predicates_inexact_constructions_kernel; + using Traits = CGAL::Min_sphere_of_spheres_d_traits_3; + using Sphere = Traits::Sphere; + using Point = K::Point_3; + using MinSphere = CGAL::Min_sphere_of_spheres_d; + + std::vector cgal_spheres; + cgal_spheres.reserve(info.size()); + + for (const auto &sphere : info) + { + auto pos = sphere.relative.translation(); + cgal_spheres.emplace_back(Point(pos[0], pos[1], pos[2]), sphere.radius); + } + + MinSphere ms(cgal_spheres.begin(), cgal_spheres.end()); + std::array sphere; + std::copy(ms.center_cartesian_begin(), ms.center_cartesian_end(), sphere.begin()); + sphere[3] = ms.radius(); + return sphere; +} + +struct RobotInfo +{ + RobotInfo( + const std::filesystem::path &urdf_file, + const std::optional &srdf_file, + const std::optional &end_effector) + { + if (not std::filesystem::exists(urdf_file)) + { + throw std::runtime_error(fmt::format("URDF file {} does not exist!", urdf_file.string())); + } + + pinocchio::urdf::buildModel(urdf_file, model); + pinocchio::urdf::buildGeom(model, urdf_file, COLLISION, collision_model); + + if (srdf_file and not std::filesystem::exists(*srdf_file)) + { + throw std::runtime_error(fmt::format("SRDF file () does not exist!", srdf_file->string())); + } + else if (not srdf_file) + { + fmt::print("No SRDF file provided, guessing collisions!\n"); + guess_self_collisions(); + } + else + { + collision_model.addAllCollisionPairs(); + pinocchio::srdf::removeCollisionPairs(model, collision_model, *srdf_file); + extract_collision_data(); + } + + extract_spheres(); + + if (not end_effector) + { + end_effector_name = model.frames[model.nframes - 1].name; + fmt::print("No EE provided, using distal link `{}`.\n", end_effector_name); + } + else if (not model.existFrame(*end_effector)) + { + throw std::runtime_error(fmt::format("Invalid EE name {}", *end_effector)); + } + else + { + end_effector_name = *end_effector; + } + + end_effector_index = model.getFrameId(end_effector_name); + } + + auto json() -> nlohmann::json + { + const Eigen::VectorXd lower_bound = model.lowerPositionLimit; + const Eigen::VectorXd upper_bound = model.upperPositionLimit; + const Eigen::VectorXd bound_range = upper_bound - lower_bound; + const Eigen::VectorXd bound_descale = bound_range.cwiseInverse(); + + nlohmann::json json; + json["n_q"] = model.nq; + json["n_spheres"] = spheres.size(); + json["bound_lower"] = std::vector(lower_bound.data(), lower_bound.data() + model.nq); + json["bound_range"] = std::vector(bound_range.data(), bound_range.data() + model.nq); + json["bound_descale"] = std::vector(bound_descale.data(), bound_descale.data() + model.nq); + json["measure"] = bound_range.prod(); + json["end_effector"] = end_effector_name; + json["end_effector_index"] = end_effector_index; + json["min_radius"] = min_radius; + json["max_radius"] = max_radius; + json["joint_names"] = dof_to_joint_names(); + json["allowed_link_pairs"] = allowed_link_pairs; + json["per_link_spheres"] = per_link_spheres; + json["links_with_geometry"] = links_with_geometry; + json["bounding_sphere_index"] = bounding_sphere_index; + json["end_effector_collisions"] = get_frames_colliding_end_effector(); + + std::vector link_names; + for (auto i = 0U; i < model.frames.size(); ++i) + { + link_names.emplace_back(model.frames[i].name); + } + json["link_names"] = link_names; + + return json; + } + + auto dof_to_joint_names() -> std::vector + { + std::vector dof_to_joint_id(model.nq); + for (auto joint_id = 1U; joint_id < model.joints.size(); ++joint_id) + { + const auto &joint = model.joints[joint_id]; + auto start_idx = joint.idx_q(); + auto nq = joint.nq(); + + for (auto i = 0U; i < nq; ++i) + { + dof_to_joint_id[start_idx + i] = joint_id; + } + } + + std::vector dof_to_joint_name(model.nq); + for (auto i = 0U; i < model.nq; ++i) + { + dof_to_joint_name[i] = model.names[dof_to_joint_id[i]]; + } + + return dof_to_joint_name; + } + + auto get_frames_colliding_end_effector() -> std::vector + { + std::size_t end_effector_joint = model.frames[end_effector_index].parentJoint; + + std::vector frames; + for (auto i = 0U; i < model.frames.size(); ++i) + { + if (model.frames[i].parentJoint == end_effector_joint) + { + if (bounding_spheres.find(i) != bounding_spheres.end()) + { + frames.emplace_back(i); + } + } + } + + std::set end_effector_allowed_collisions; + for (const auto &[first, second] : allowed_link_pairs) + { + if (std::find(frames.begin(), frames.end(), first) != frames.end()) + { + end_effector_allowed_collisions.emplace(second); + } + + if (std::find(frames.begin(), frames.end(), second) != frames.end()) + { + end_effector_allowed_collisions.emplace(first); + } + } + + return std::vector( + end_effector_allowed_collisions.begin(), end_effector_allowed_collisions.end()); + } + + auto extract_spheres() -> void + { + for (auto i = 0U; i < collision_model.ngeoms; ++i) + { + const auto &geom_obj = collision_model.geometryObjects[i]; + const auto &sphere_ptr = std::dynamic_pointer_cast(geom_obj.geometry); + + if (sphere_ptr) + { + SphereInfo info; + info.geom_index = i; + info.radius = sphere_ptr->radius; + info.parent_joint = geom_obj.parentJoint; + info.parent_frame = geom_obj.parentFrame; + info.relative = geom_obj.placement; + + spheres.emplace_back(info); + + min_radius = std::min(min_radius, info.radius); + max_radius = std::max(max_radius, info.radius); + } + else + { + throw std::runtime_error( + fmt::format("Invalid non-sphere geometry in URDF {}", geom_obj.name)); + } + } + + std::size_t bs = 0; + for (auto i = 0U; i < model.frames.size(); ++i) + { + std::vector link_info; + std::vector sphere_indices; + for (const auto &info : spheres) + { + if (info.parent_frame == i) + { + link_info.emplace_back(info); + sphere_indices.emplace_back(info.geom_index); + } + } + + per_link_spheres.emplace_back(sphere_indices); + + if (not link_info.empty()) + { + auto sphere = min_sphere_of_spheres(link_info); + + SphereInfo info; + info.geom_index = bs; + info.radius = sphere[3]; + info.parent_joint = link_info[0].parent_joint; + info.relative = SE3::Identity(); + info.relative.translation()[0] = sphere[0]; + info.relative.translation()[1] = sphere[1]; + info.relative.translation()[2] = sphere[2]; + + bounding_spheres.emplace(i, info); + bounding_sphere_index.emplace_back(bs); + links_with_geometry.emplace_back(i); + bs++; + } + else + { + bounding_sphere_index.emplace_back(0); + } + } + } + + auto collision_pair_to_frame_pair(const CollisionPair &cp) -> std::pair + { + const auto &geom1 = collision_model.geometryObjects[cp.first]; + const auto &geom2 = collision_model.geometryObjects[cp.second]; + + std::size_t link1_idx = geom1.parentFrame; + std::size_t link2_idx = geom2.parentFrame; + + return std::make_pair(std::min(link1_idx, link2_idx), std::max(link1_idx, link2_idx)); + } + + auto extract_collision_data() -> void + { + for (const auto &cp : collision_model.collisionPairs) + { + allowed_link_pairs.insert(collision_pair_to_frame_pair(cp)); + } + } + + auto get_adjacent_frames() -> std::set> + { + const auto nf = model.frames.size(); + const auto nj = model.joints.size(); + + std::set> adjacents; + + for (auto i = 0U; i < nf; ++i) + { + for (auto j = i + 1; j < nf; ++j) + { + const auto &frame_i = model.frames[i]; + const auto &frame_j = model.frames[j]; + + if (frame_i.parentJoint < nj and frame_j.parentJoint < nj) + { + const auto &joint_i = model.joints[frame_i.parentJoint]; + const auto &joint_j = model.joints[frame_j.parentJoint]; + + // Check if joints are parent-child related + if (model.parents[frame_i.parentJoint] == frame_j.parentJoint or + model.parents[frame_j.parentJoint] == frame_i.parentJoint) + { + adjacents.insert({i, j}); + } + } + } + } + + return adjacents; + } + + auto guess_self_collisions(std::size_t n = 1000000U) -> void + { + collision_model.addAllCollisionPairs(); + + Data data(model); + GeometryData collision_data(collision_model); + + std::set> always_pairs; + + for (auto j = 0U; j < collision_model.collisionPairs.size(); ++j) + { + always_pairs.emplace(collision_pair_to_frame_pair(collision_model.collisionPairs[j])); + } + + allowed_link_pairs.clear(); + + for (auto i = 0U; i < n; ++i) + { + auto q = randomConfiguration(model); + computeCollisions(model, data, collision_model, collision_data, q); + + for (auto j = 0U; j < collision_model.collisionPairs.size(); ++j) + { + const auto &cr = collision_data.collisionResults[j]; + auto pair = collision_pair_to_frame_pair(collision_model.collisionPairs[j]); + + if (cr.isCollision()) + { + allowed_link_pairs.insert(pair); + } + else + { + auto it = always_pairs.find(pair); + if (it != always_pairs.end()) + { + always_pairs.erase(it); + } + } + } + } + + // Remove all adjacent frames + auto adjacents = get_adjacent_frames(); + for (const auto &pair : adjacents) + { + allowed_link_pairs.erase(pair); + } + + // Remove all pairs that never collided + for (const auto &pair : always_pairs) + { + allowed_link_pairs.erase(pair); + } + + // Add remaining potential collisions + collision_model.removeAllCollisionPairs(); + for (const auto &pair : allowed_link_pairs) + { + collision_model.addCollisionPair(CollisionPair(pair.first, pair.second)); + } + } + + Model model; + GeometryModel collision_model; + std::string end_effector_name; + std::size_t end_effector_index; + + float min_radius{std::numeric_limits::max()}; + float max_radius{std::numeric_limits::min()}; + std::vector spheres; + std::map bounding_spheres; + std::vector links_with_geometry; + std::vector> per_link_spheres; + std::set> allowed_link_pairs; + std::vector bounding_sphere_index; +}; + diff --git a/src/tracer_utils.hh b/src/tracer_utils.hh new file mode 100644 index 0000000..5dc0a5a --- /dev/null +++ b/src/tracer_utils.hh @@ -0,0 +1,19 @@ +#pragma once + +#include +#include + +struct Traced +{ + std::string code; + std::size_t temp_variables; + std::size_t outputs; +}; + +void add_to_trace(Traced tcode, std::string name, nlohmann::json &data) +{ + data[name] = tcode.code; + data[fmt::format("{}_vars", name)] = tcode.temp_variables; + data[fmt::format("{}_output", name)] = tcode.outputs; +} +