Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add ManipulabilityRatioSigmoidMoveIt #19

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Add ManipulabilityRatioSigmoidMoveIt #19

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 14 additions & 0 deletions include/reach_ros/evaluation/manipulability_moveit.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,20 @@ struct ManipulabilityMoveItFactory : public reach::EvaluatorFactory
reach::Evaluator::ConstPtr create(const YAML::Node& config) const override;
};

class ManipulabilityRatioSigmoid : public ManipulabilityMoveIt
{
public:
using ManipulabilityMoveIt::ManipulabilityMoveIt;
virtual double calculateScore(const Eigen::MatrixXd& jacobian_singular_values) const override;
};

struct ManipulabilityRatioSigmoidFactory : public reach::EvaluatorFactory
{
using reach::EvaluatorFactory::EvaluatorFactory;

reach::Evaluator::ConstPtr create(const YAML::Node& config) const override;
};

/** @brief Computes the manipulability of a robot pose divided by the characteristic length of the robot */
class ManipulabilityScaled : public ManipulabilityMoveIt
{
Expand Down
29 changes: 29 additions & 0 deletions src/evaluation/manipulability_moveit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include <reach/plugin_utils.h>
#include <reach/utils.h>
#include <yaml-cpp/yaml.h>
#include <cmath>

static std::vector<Eigen::Index> getJacobianRowSubset(const YAML::Node& config, const std::string& key = "jacobian_row_"
"subset")
Expand Down Expand Up @@ -179,6 +180,34 @@ reach::Evaluator::ConstPtr ManipulabilityRatioFactory::create(const YAML::Node&
return std::make_shared<ManipulabilityRatio>(model, planning_group, jacobian_row_subset);
}

double ManipulabilityRatioSigmoid::calculateScore(const Eigen::MatrixXd& jacobian_singular_values) const
{
double manipulability = jacobian_singular_values.array().prod();
double manipulability_ratio = jacobian_singular_values.minCoeff() / jacobian_singular_values.maxCoeff();
double manipulability_scaled = std::sqrt(manipulability);
double manipulability_measure = std::sqrt(manipulability_ratio);
Comment on lines +185 to +188
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove unused variables

Suggested change
double manipulability = jacobian_singular_values.array().prod();
double manipulability_ratio = jacobian_singular_values.minCoeff() / jacobian_singular_values.maxCoeff();
double manipulability_scaled = std::sqrt(manipulability);
double manipulability_measure = std::sqrt(manipulability_ratio);
double manipulability_ratio = jacobian_singular_values.minCoeff() / jacobian_singular_values.maxCoeff();
double manipulability_measure = std::sqrt(manipulability_ratio);


double score = manipulability_measure;

double shifted_score = 50.0 * (score - 0.15);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why is score the square root of the manipulability ratio? Also what is the significance of the hard-coded numbers 50 and 0.15? It seems like these might need to be parameters to give the user some control over the "width" of the sigmoid

double sigmoid = 1.0 / (1.0 + std::exp(-shifted_score));

return sigmoid;
}

reach::Evaluator::ConstPtr ManipulabilityRatioSigmoidFactory::create(const YAML::Node& config) const
{
auto planning_group = reach::get<std::string>(config, "planning_group");
std::vector<Eigen::Index> jacobian_row_subset = getJacobianRowSubset(config);

utils::initROS();
moveit::core::RobotModelConstPtr model = moveit::planning_interface::getSharedRobotModel("robot_description");
if (!model)
throw std::runtime_error("Failed to initialize robot model pointer");

return std::make_shared<ManipulabilityRatioSigmoid>(model, planning_group, jacobian_row_subset);
}

ManipulabilityScaled::ManipulabilityScaled(moveit::core::RobotModelConstPtr model, const std::string& planning_group,
std::vector<Eigen::Index> jacobian_row_subset,
std::vector<std::string> excluded_links)
Expand Down
1 change: 1 addition & 0 deletions src/plugins.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ EXPORT_EVALUATOR_PLUGIN(reach_ros::evaluation::JointPenaltyMoveItFactory, JointP
EXPORT_EVALUATOR_PLUGIN(reach_ros::evaluation::ManipulabilityMoveItFactory, ManipulabilityMoveIt)
EXPORT_EVALUATOR_PLUGIN(reach_ros::evaluation::ManipulabilityScaledFactory, ManipulabilityScaledMoveIt)
EXPORT_EVALUATOR_PLUGIN(reach_ros::evaluation::ManipulabilityRatioFactory, ManipulabilityRatioMoveIt)
EXPORT_EVALUATOR_PLUGIN(reach_ros::evaluation::ManipulabilityRatioSigmoidFactory, ManipulabilityRatioSigmoidMoveIt)
EXPORT_IK_SOLVER_PLUGIN(reach_ros::ik::MoveItIKSolverFactory, MoveItIKSolver)
EXPORT_IK_SOLVER_PLUGIN(reach_ros::ik::DiscretizedMoveItIKSolverFactory, DiscretizedMoveItIKSolverFactory)
EXPORT_TARGET_POSE_GENERATOR_PLUGIN(reach_ros::TransformedPointCloudTargetPoseGeneratorFactory,
Expand Down
Loading