Skip to content

Commit

Permalink
wip
Browse files Browse the repository at this point in the history
  • Loading branch information
@domire8
domire8 committed Apr 9, 2024
1 parent 8d0a14b commit 47ff4f4
Show file tree
Hide file tree
Showing 5 changed files with 88 additions and 262 deletions.
4 changes: 2 additions & 2 deletions python/source/robot_model/bind_model.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,8 +62,8 @@ void model(py::module_& m) {
// c.def("get_pinocchio_model", &Model::get_pinocchio_model, "Getter of the pinocchio model.");


c.def("check_collision", py::overload_cast<const JointPositions&>(&Model::check_collision), "Check if the robot is in collision at a given joint state.", "joint_positions"_a);
c.def("compute_minimum_collision_distances", py::overload_cast<const JointPositions&>(&Model::compute_minimum_collision_distances), "Compute the minimum distances between the robot links.", "joint_positions"_a);
// c.def("check_collision", py::overload_cast<const JointPositions&>(&Model::check_collision), "Check if the robot is in collision at a given joint state.", "joint_positions"_a);
// c.def("compute_minimum_collision_distances", py::overload_cast<const JointPositions&>(&Model::compute_minimum_collision_distances), "Compute the minimum distances between the robot links.", "joint_positions"_a);
c.def("get_number_of_collision_pairs", &Model::get_number_of_collision_pairs, "Get the number of collision pairs in the model.");
c.def("is_geometry_model_initialized", &Model::is_geometry_model_initialized, "Check if the geometry model is initialized.");
c.def(
Expand Down
4 changes: 2 additions & 2 deletions source/robot_model/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ find_package(pinocchio ${PINOCCHIO_VERSION} REQUIRED)
find_package(hpp-fcl ${HPP_FCL_VERSION} REQUIRED)
find_package(OsqpEigen ${OSQP_EIGEN_VERSION} REQUIRED)

add_definitions(-DPINOCCHIO_WITH_HPP_FCL)
# add_definitions(-DPINOCCHIO_WITH_HPP_FCL)

set(CORE_SOURCES
src/Model.cpp
Expand All @@ -29,7 +29,7 @@ target_link_libraries(${LIBRARY_NAME}
${hpp-fcl_LIBRARIES}
OsqpEigen::OsqpEigen
)
target_compile_definitions(${LIBRARY_NAME} PUBLIC -DPINOCCHIO_WITH_HPP_FCL)
# target_compile_definitions(${LIBRARY_NAME} PUBLIC -DPINOCCHIO_WITH_HPP_FCL)

# install the target and create export-set
install(TARGETS ${LIBRARY_NAME}
Expand Down
34 changes: 17 additions & 17 deletions source/robot_model/include/robot_model/Model.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ class Model {
/**
* @brief Initialize the pinocchio geometry model from the URDF and the package paths
*/
void init_geom_model(std::string urdf);
// void init_geom_model(std::string urdf);

/**
* @brief initialize the constraints for the QP solver
Expand Down Expand Up @@ -264,22 +264,22 @@ class Model {
*/
static bool create_urdf_from_string(const std::string& urdf_string, const std::string& desired_path);

/**
* @brief Compute the minimum distances between the robot links
* @details The distances are computed for each collision pair, resulting in a square matrix with
* the same size as the number of joints. The diagonal entries are always zero.
* @param joint_positions state_representation object containing the joint positions of the robot
* @return the matrix containing the minimum distance between the robot links
*/
Eigen::MatrixXd compute_minimum_collision_distances(const state_representation::JointPositions& joint_positions);

/**
* @brief Check if the links of the robot are in collision
* @param joint_positions containing the joint positions of the robot
* @throws robot_model::exceptions::CollisionGeometryException if collision geometry is not initialized
* @return true if the robot is in collision, false otherwise
*/
bool check_collision(const state_representation::JointPositions& joint_positions);
// /**
// * @brief Compute the minimum distances between the robot links
// * @details The distances are computed for each collision pair, resulting in a square matrix with
// * the same size as the number of joints. The diagonal entries are always zero.
// * @param joint_positions state_representation object containing the joint positions of the robot
// * @return the matrix containing the minimum distance between the robot links
// */
// Eigen::MatrixXd compute_minimum_collision_distances(const state_representation::JointPositions& joint_positions);

// /**
// * @brief Check if the links of the robot are in collision
// * @param joint_positions containing the joint positions of the robot
// * @throws robot_model::exceptions::CollisionGeometryException if collision geometry is not initialized
// * @return true if the robot is in collision, false otherwise
// */
// bool check_collision(const state_representation::JointPositions& joint_positions);

/**
* @brief Getter of the number of collision pairs in the model
Expand Down
134 changes: 67 additions & 67 deletions source/robot_model/src/Model.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,9 +97,9 @@ void Model::init_model() {
pinocchio::urdf::buildModelFromXML(urdf, this->robot_model_);
this->robot_data_ = pinocchio::Data(this->robot_model_);

if (this->load_collision_geometries_) {
this->init_geom_model(urdf);
}
// if (this->load_collision_geometries_) {
// this->init_geom_model(urdf);
// }

// get the frames
std::vector<std::string> frames;
Expand All @@ -111,26 +111,26 @@ void Model::init_model() {
this->init_qp_solver();
}

void Model::init_geom_model(std::string urdf) {
try {
auto package_paths = this->resolve_package_paths_in_urdf(urdf);
pinocchio::urdf::buildGeom(
this->robot_model_, std::istringstream(urdf), pinocchio::COLLISION, this->geom_model_, package_paths);
this->geom_model_.addAllCollisionPairs();
// void Model::init_geom_model(std::string urdf) {
// try {
// auto package_paths = this->resolve_package_paths_in_urdf(urdf);
// pinocchio::urdf::buildGeom(
// this->robot_model_, std::istringstream(urdf), pinocchio::COLLISION, this->geom_model_, package_paths);
// this->geom_model_.addAllCollisionPairs();

std::vector<pinocchio::CollisionPair> excluded_pairs = this->generate_joint_exclusion_list();
// std::vector<pinocchio::CollisionPair> excluded_pairs = this->generate_joint_exclusion_list();

// remove collision pairs for linked joints (i.e. parent-child joints)
for (const auto& pair : excluded_pairs) {
this->geom_model_.removeCollisionPair(pair);
}
// // remove collision pairs for linked joints (i.e. parent-child joints)
// for (const auto& pair : excluded_pairs) {
// this->geom_model_.removeCollisionPair(pair);
// }

this->geom_data_ = pinocchio::GeometryData(this->geom_model_);
} catch (const std::exception& ex) {
throw robot_model::exceptions::CollisionGeometryException(
"Failed to initialize geometry model for " + this->get_robot_name() + ": " + ex.what());
}
}
// this->geom_data_ = pinocchio::GeometryData(this->geom_model_);
// } catch (const std::exception& ex) {
// throw robot_model::exceptions::CollisionGeometryException(
// "Failed to initialize geometry model for " + this->get_robot_name() + ": " + ex.what());
// }
// }

std::vector<pinocchio::CollisionPair> Model::generate_joint_exclusion_list() {
std::vector<pinocchio::CollisionPair> excluded_pairs;
Expand Down Expand Up @@ -164,53 +164,53 @@ bool Model::is_geometry_model_initialized() {
return !this->geom_model_.collisionPairs.empty();
}

bool Model::check_collision(const state_representation::JointPositions& joint_positions) {
if (!this->is_geometry_model_initialized()) {
throw robot_model::exceptions::CollisionGeometryException(
"Geometry model not loaded for " + this->get_robot_name());
}

Eigen::VectorXd configuration = joint_positions.get_positions();

pinocchio::computeCollisions(
this->robot_model_, this->robot_data_, this->geom_model_, this->geom_data_, configuration, true);

for (size_t pair_index = 0; pair_index < this->geom_model_.collisionPairs.size(); ++pair_index) {
const auto& collision_result = this->geom_data_.collisionResults[pair_index];
if (collision_result.isCollision()) {
return true;
}
}
return false;
}

Eigen::MatrixXd Model::compute_minimum_collision_distances(const state_representation::JointPositions& joint_positions) {
if (!this->is_geometry_model_initialized()) {
throw robot_model::exceptions::CollisionGeometryException(
"Geometry model not loaded for " + this->get_robot_name());
}
Eigen::VectorXd configuration = joint_positions.get_positions();
pinocchio::computeDistances(
this->robot_model_, this->robot_data_, this->geom_model_, this->geom_data_, configuration);

// nb_joints is the number of joints in the robot model
unsigned int nb_joints = this->get_number_of_joints();

// create a square matrix to store the distances and initialize to zero
Eigen::MatrixXd distances = Eigen::MatrixXd::Zero(nb_joints, nb_joints);

// iterate over the collision pairs and extract the distances
unsigned int pair_index = 0;
for (unsigned int row_index = 0; row_index < nb_joints; ++row_index) {
for (unsigned int column_index = row_index + 1; column_index < nb_joints; ++column_index) {
distances(row_index, column_index) = this->geom_data_.distanceResults[pair_index].min_distance;
distances(column_index, row_index) = distances(row_index, column_index);
pair_index++;
}
}

return distances;
}
// bool Model::check_collision(const state_representation::JointPositions& joint_positions) {
// if (!this->is_geometry_model_initialized()) {
// throw robot_model::exceptions::CollisionGeometryException(
// "Geometry model not loaded for " + this->get_robot_name());
// }

// Eigen::VectorXd configuration = joint_positions.get_positions();

// pinocchio::computeCollisions(
// this->robot_model_, this->robot_data_, this->geom_model_, this->geom_data_, configuration, true);

// for (size_t pair_index = 0; pair_index < this->geom_model_.collisionPairs.size(); ++pair_index) {
// const auto& collision_result = this->geom_data_.collisionResults[pair_index];
// if (collision_result.isCollision()) {
// return true;
// }
// }
// return false;
// }

// Eigen::MatrixXd Model::compute_minimum_collision_distances(const state_representation::JointPositions& joint_positions) {
// if (!this->is_geometry_model_initialized()) {
// throw robot_model::exceptions::CollisionGeometryException(
// "Geometry model not loaded for " + this->get_robot_name());
// }
// Eigen::VectorXd configuration = joint_positions.get_positions();
// pinocchio::computeDistances(
// this->robot_model_, this->robot_data_, this->geom_model_, this->geom_data_, configuration);

// // nb_joints is the number of joints in the robot model
// unsigned int nb_joints = this->get_number_of_joints();

// // create a square matrix to store the distances and initialize to zero
// Eigen::MatrixXd distances = Eigen::MatrixXd::Zero(nb_joints, nb_joints);

// // iterate over the collision pairs and extract the distances
// unsigned int pair_index = 0;
// for (unsigned int row_index = 0; row_index < nb_joints; ++row_index) {
// for (unsigned int column_index = row_index + 1; column_index < nb_joints; ++column_index) {
// distances(row_index, column_index) = this->geom_data_.distanceResults[pair_index].min_distance;
// distances(column_index, row_index) = distances(row_index, column_index);
// pair_index++;
// }
// }

// return distances;
// }

bool Model::init_qp_solver() {
// clear the solver
Expand Down
Loading

0 comments on commit 47ff4f4

Please sign in to comment.