Skip to content

Commit

Permalink
fixed issues with invalid error messages that resulted from processin…
Browse files Browse the repository at this point in the history 
…g the request constraints
  • Loading branch information
@jrgnicho
jrgnicho committed Sep 14, 2018
1 parent 5567aac commit e935166
Show file tree
Hide file tree
Showing 7 changed files with 100 additions and 70 deletions.
4 changes: 2 additions & 2 deletions stomp_moveit/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ find_package(catkin REQUIRED COMPONENTS
kdl_parser
trac_ik_lib
)
find_package(Eigen REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED COMPONENTS system)

add_definitions("-std=c++11")
Expand All @@ -23,7 +23,7 @@ catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS moveit_ros_planning moveit_core stomp_core cmake_modules pluginlib roscpp kdl_parser trac_ik_lib
DEPENDS Eigen
DEPENDS EIGEN3
)

###########
Expand Down
14 changes: 10 additions & 4 deletions stomp_moveit/include/stomp_moveit/utils/kinematics.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
#include <moveit_msgs/PositionConstraint.h>
#include <moveit_msgs/OrientationConstraint.h>
#include <trac_ik/trac_ik.hpp>
#include <boost/optional.hpp>


namespace stomp_moveit
Expand Down Expand Up @@ -128,18 +129,23 @@ namespace kinematics

};


bool validateCartesianConstraints(const moveit_msgs::Constraints& c);
/**
* @brief Checks if the constraint structured contains valid data from which a proper cartesian constraint can
* be produced.
* @param c The constraint object. It may define position, orientation or both constraint types.
* @return True when it is Cartesian, false otherwise.
*/
bool isCartesianConstraints(const moveit_msgs::Constraints& c);

/**
* @brief Populates the missing parts of a Cartesian constraints in order to provide a constraint that can be used by the Ik solver.
* @param c The constraint object. It may define position, orientation or both constraint types.
* @param ref_pose If no orientation or position constraints are given then this pose will be use to fill the missing information.
* @param default_pos_tol Used when no position tolerance is specified.
* @param default_rot_tol Used when no rotation tolerance is specified
* @return
* @return A constraint object
*/
moveit_msgs::Constraints constructCartesianConstraints(const moveit_msgs::Constraints& c,const Eigen::Affine3d& ref_pose,
boost::optional<moveit_msgs::Constraints> curateCartesianConstraints(const moveit_msgs::Constraints& c,const Eigen::Affine3d& ref_pose,
double default_pos_tol = 0.0005, double default_rot_tol = M_PI);

/**
Expand Down
37 changes: 23 additions & 14 deletions stomp_moveit/src/stomp_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -398,24 +398,29 @@ bool StompPlanner::getSeedParameters(Eigen::MatrixXd& parameters) const
}

// now check Cartesian constraint

// first construct proper Cartesian tool constraints
state.updateLinkTransforms();
Eigen::Affine3d start_tool_pose = state.getGlobalLinkTransform(tool_link);
moveit_msgs::Constraints tool_constraints = constructCartesianConstraints(gc,start_tool_pose);
boost::optional<moveit_msgs::Constraints> tool_constraints = curateCartesianConstraints(gc,start_tool_pose);
if(!tool_constraints.is_initialized())
{
ROS_WARN("Cartesian Goal could not be created from provided constraints");
found_goal = true;
break;
}

Eigen::VectorXd solution;
Eigen::VectorXd seed = start;
ik_solver_->setKinematicState(state);
if(ik_solver_->solve(seed,tool_constraints,solution))
if(ik_solver_->solve(seed,tool_constraints.get(),solution))
{
goal = solution;
found_goal = true;
break;
}
else
{
ROS_DEBUG_STREAM_NAMED(DEBUG_NS,"IK failed with goal constraint \n"<<tool_constraints);
ROS_ERROR("A valid ik solution for the given Cartesian constraints was not found ");
ROS_DEBUG_STREAM_NAMED(DEBUG_NS,"IK failed with goal constraint \n"<<tool_constraints.get());
ROS_DEBUG_STREAM_NAMED(DEBUG_NS,"Reference Tool pose used was: \n"<<start_tool_pose.matrix());
}
}
Expand All @@ -429,13 +434,13 @@ bool StompPlanner::getSeedParameters(Eigen::MatrixXd& parameters) const
}
else
{
ROS_ERROR("%s Goal in seed to far away from Goal requested",getName().c_str());
ROS_ERROR("%s Goal in seed is too far away from requested goal constraints",getName().c_str());
return false;
}
}
else
{
ROS_ERROR("%s Goal in seed to far away from Goal requested",getName().c_str());
ROS_ERROR("%s requested goal constraint was invalid or unreachable, comparison with goal in seed isn't possible",getName().c_str());
return false;
}

Expand Down Expand Up @@ -657,28 +662,32 @@ bool StompPlanner::getStartAndGoal(Eigen::VectorXd& start, Eigen::VectorXd& goal
}

// now check cartesian constraint

// first construct proper cartesian tool constraints
state->updateLinkTransforms();
Eigen::Affine3d start_tool_pose = state->getGlobalLinkTransform(tool_link);
moveit_msgs::Constraints tool_constraints = constructCartesianConstraints(gc,start_tool_pose);
boost::optional<moveit_msgs::Constraints> tool_constraints = curateCartesianConstraints(gc,start_tool_pose);
if(!tool_constraints.is_initialized())
{
ROS_WARN("Cartesian Goal could not be created from provided constraints");
found_goal = true;
break;
}

// now solve ik
Eigen::VectorXd solution;
Eigen::VectorXd seed = start;
ik_solver_->setKinematicState(*state);
if(ik_solver_->solve(seed,tool_constraints,solution))
if(ik_solver_->solve(seed,tool_constraints.get(),solution))
{
goal = solution;
found_goal = true;
break;
}
else
{
ROS_DEBUG_STREAM_NAMED(DEBUG_NS,"IK failed with goal constraint \n"<<tool_constraints);
ROS_ERROR("A valid ik solution for the given Cartesian constraints was not found ");
ROS_DEBUG_STREAM_NAMED(DEBUG_NS,"IK failed with goal constraint \n"<<tool_constraints.get());
ROS_DEBUG_STREAM_NAMED(DEBUG_NS,"Reference Tool pose used was: \n"<<start_tool_pose.matrix());
}

}

ROS_ERROR_COND(!found_goal,"%s was unable to retrieve the goal from the MotionPlanRequest",getName().c_str());
Expand Down Expand Up @@ -713,7 +722,7 @@ bool StompPlanner::canServiceRequest(const moveit_msgs::MotionPlanRequest &req)
// check that we have joint or cartesian constraints at the goal
const auto& gc = req.goal_constraints[0];
if ((gc.joint_constraints.size() == 0) &&
!utils::kinematics::validateCartesianConstraints(gc))
!utils::kinematics::isCartesianConstraints(gc))
{
ROS_ERROR("STOMP couldn't find either a joint or cartesian goal.");
return false;
Expand Down
79 changes: 42 additions & 37 deletions stomp_moveit/src/utils/kinematics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -204,42 +204,47 @@ bool IKSolver::solve(const Eigen::VectorXd& seed, const moveit_msgs::Constraints
return solve(seed,tool_pose,solution,tolerance_eigen);
}

bool validateCartesianConstraints(const moveit_msgs::Constraints& c)
bool isCartesianConstraints(const moveit_msgs::Constraints& c)
{
std::string frame_id;
std::string pos_frame_id, orient_frame_id;
bool found_cart = false;
if(c.position_constraints.empty() && c.orientation_constraints.empty())
{
ROS_ERROR("Constraint is empty");
ROS_DEBUG("No cartesian constraints were found, failed validation");
return false;
}

if(!c.position_constraints.empty())
{
frame_id = c.position_constraints.front().header.frame_id;
pos_frame_id = c.position_constraints.front().header.frame_id;
found_cart = !pos_frame_id.empty();
}

if(!c.orientation_constraints.empty())
{
std::string o_frame_id = c.orientation_constraints.front().header.frame_id;
if(frame_id != o_frame_id)
{
ROS_ERROR("Position and orientation frame_id are different");
return false;
}
orient_frame_id = c.orientation_constraints.front().header.frame_id;
found_cart = !orient_frame_id.empty();
}

return true;
if(!pos_frame_id.empty() && !orient_frame_id.empty() && (pos_frame_id != orient_frame_id))
{
ROS_ERROR("Position frame id '%s' differs from orientation frame id '%s', invalid Cartesian constraint",
pos_frame_id.c_str(), orient_frame_id.c_str());
return false;
}

return found_cart;
}

moveit_msgs::Constraints constructCartesianConstraints(const moveit_msgs::Constraints& c,const Eigen::Affine3d& ref_pose,
boost::optional<moveit_msgs::Constraints> curateCartesianConstraints(const moveit_msgs::Constraints& c,const Eigen::Affine3d& ref_pose,
double default_pos_tol , double default_rot_tol)
{
using namespace moveit_msgs;

moveit_msgs::Constraints cc;
if(!validateCartesianConstraints(c))
moveit_msgs::Constraints full_constraint;
if(!isCartesianConstraints(c))
{
return cc;
return boost::none;
}

// obtaining defaults
Expand All @@ -250,54 +255,54 @@ moveit_msgs::Constraints constructCartesianConstraints(const moveit_msgs::Constr


// creating default position constraint
PositionConstraint pc;
pc.header.frame_id = frame_id;
pc.constraint_region.primitive_poses.resize(1);
tf::poseEigenToMsg(Eigen::Affine3d::Identity(), pc.constraint_region.primitive_poses[0]);
pc.constraint_region.primitive_poses[0].position.x = ref_pose.translation().x();
pc.constraint_region.primitive_poses[0].position.y = ref_pose.translation().y();
pc.constraint_region.primitive_poses[0].position.z = ref_pose.translation().z();
PositionConstraint default_pos_constraint;
default_pos_constraint.header.frame_id = frame_id;
default_pos_constraint.constraint_region.primitive_poses.resize(1);
tf::poseEigenToMsg(Eigen::Affine3d::Identity(), default_pos_constraint.constraint_region.primitive_poses[0]);
default_pos_constraint.constraint_region.primitive_poses[0].position.x = ref_pose.translation().x();
default_pos_constraint.constraint_region.primitive_poses[0].position.y = ref_pose.translation().y();
default_pos_constraint.constraint_region.primitive_poses[0].position.z = ref_pose.translation().z();
shape_msgs::SolidPrimitive shape;
shape.type = shape.SPHERE;
shape.dimensions.push_back(default_pos_tol);
pc.constraint_region.primitives.push_back(shape);
default_pos_constraint.constraint_region.primitives.push_back(shape);

// creating default orientation constraint
OrientationConstraint oc;
oc.header.frame_id = frame_id;
oc.absolute_x_axis_tolerance = default_rot_tol;
oc.absolute_y_axis_tolerance = default_rot_tol;
oc.absolute_z_axis_tolerance = default_rot_tol;
oc.orientation.x = oc.orientation.y = oc.orientation.z = 0;
oc.orientation.w = 1;
OrientationConstraint default_orient_constraint;
default_orient_constraint.header.frame_id = frame_id;
default_orient_constraint.absolute_x_axis_tolerance = default_rot_tol;
default_orient_constraint.absolute_y_axis_tolerance = default_rot_tol;
default_orient_constraint.absolute_z_axis_tolerance = default_rot_tol;
default_orient_constraint.orientation.x = default_orient_constraint.orientation.y = default_orient_constraint.orientation.z = 0;
default_orient_constraint.orientation.w = 1;


cc.position_constraints.resize(num_entries);
cc.orientation_constraints.resize(num_entries);
full_constraint.position_constraints.resize(num_entries);
full_constraint.orientation_constraints.resize(num_entries);
for(int i =0; i < num_entries; i++)
{
// populating position constraints
if(c.position_constraints.size() >= num_entries)
{
cc.position_constraints[i] = c.position_constraints[i];
full_constraint.position_constraints[i] = c.position_constraints[i];
}
else
{
cc.position_constraints[i] = pc;
full_constraint.position_constraints[i] = default_pos_constraint;
}

// populating orientation constraints
if(c.orientation_constraints.size() >= num_entries)
{
cc.orientation_constraints[i] = c.orientation_constraints[i];
full_constraint.orientation_constraints[i] = c.orientation_constraints[i];
}
else
{
cc.orientation_constraints[i] = oc;
full_constraint.orientation_constraints[i] = default_orient_constraint;
}
}

return cc;
return full_constraint;
}

bool decodeCartesianConstraint(moveit::core::RobotModelConstPtr model, const moveit_msgs::Constraints& constraints, Eigen::Affine3d& tool_pose,
Expand Down
13 changes: 8 additions & 5 deletions stomp_plugins/src/cost_functions/tool_goal_pose.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,15 +114,18 @@ bool ToolGoalPose::setMotionPlanRequest(const planning_scene::PlanningSceneConst
for(const auto& g: goals)
{

if(utils::kinematics::validateCartesianConstraints(g))
if(utils::kinematics::isCartesianConstraints(g))
{
// tool cartesian goal data
state_->updateLinkTransforms();
Eigen::Affine3d start_tool_pose = state_->getGlobalLinkTransform(tool_link_);
moveit_msgs::Constraints cartesian_constraints = utils::kinematics::constructCartesianConstraints(g,start_tool_pose);
found_goal = utils::kinematics::decodeCartesianConstraint(robot_model_,cartesian_constraints,tool_goal_pose_,
tool_goal_tolerance_,robot_model_->getRootLinkName());
ROS_DEBUG_STREAM("ToolGoalTolerance cost function will use tolerance: "<<tool_goal_tolerance_.transpose());
boost::optional<moveit_msgs::Constraints> cartesian_constraints = utils::kinematics::curateCartesianConstraints(g,start_tool_pose);
if(cartesian_constraints.is_initialized())
{
found_goal = utils::kinematics::decodeCartesianConstraint(robot_model_,cartesian_constraints.get(),tool_goal_pose_,
tool_goal_tolerance_,robot_model_->getRootLinkName());
ROS_DEBUG_STREAM("ToolGoalTolerance cost function will use tolerance: "<<tool_goal_tolerance_.transpose());
}
break;
}

Expand Down
12 changes: 8 additions & 4 deletions stomp_plugins/src/noise_generators/goal_guided_multivariate_gaussian.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,14 +208,18 @@ bool GoalGuidedMultivariateGaussian::setupGoalConstraints(const planning_scene::
for(const auto& g: goals)
{

if(utils::kinematics::validateCartesianConstraints(g))
if(utils::kinematics::isCartesianConstraints(g))
{
// decoding goal
state_->updateLinkTransforms();
Eigen::Affine3d start_tool_pose = state_->getGlobalLinkTransform(tool_link_);
moveit_msgs::Constraints cartesian_constraints = utils::kinematics::constructCartesianConstraints(g,start_tool_pose);
Eigen::Affine3d tool_goal_pose;
found_valid = utils::kinematics::decodeCartesianConstraint(robot_model_,cartesian_constraints,tool_goal_pose,tool_goal_tolerance_,robot_model_->getRootLinkName());
boost::optional<moveit_msgs::Constraints> cartesian_constraints = utils::kinematics::curateCartesianConstraints(g,start_tool_pose);
if(cartesian_constraints.is_initialized())
{
Eigen::Affine3d tool_goal_pose;
found_valid = utils::kinematics::decodeCartesianConstraint(robot_model_,cartesian_constraints.get(),
tool_goal_pose,tool_goal_tolerance_,robot_model_->getRootLinkName());
}
}


Expand Down
11 changes: 7 additions & 4 deletions stomp_plugins/src/update_filters/constrained_cartesian_goal.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,14 +103,17 @@ bool ConstrainedCartesianGoal::setMotionPlanRequest(const planning_scene::Planni
for(const auto& g: goals)
{

if(utils::kinematics::validateCartesianConstraints(g))
if(utils::kinematics::isCartesianConstraints(g))
{
// tool cartesian goal data
state_->updateLinkTransforms();
Eigen::Affine3d start_tool_pose = state_->getGlobalLinkTransform(tool_link_);
moveit_msgs::Constraints cartesian_constraints = utils::kinematics::constructCartesianConstraints(g,start_tool_pose);
found_goal = utils::kinematics::decodeCartesianConstraint(robot_model_,cartesian_constraints,tool_goal_pose_,tool_goal_tolerance_,
robot_model_->getRootLinkName());
boost::optional<moveit_msgs::Constraints> cartesian_constraints = utils::kinematics::curateCartesianConstraints(g,start_tool_pose);
if(cartesian_constraints.is_initialized())
{
found_goal = utils::kinematics::decodeCartesianConstraint(robot_model_,cartesian_constraints.get(),tool_goal_pose_,tool_goal_tolerance_,
robot_model_->getRootLinkName());
}
break;
}

Expand Down

0 comments on commit e935166

Please sign in to comment.