Skip to content

Commit

Permalink
setFromIK(): remove 'attempts' argument
Browse files Browse the repository at this point in the history 
randomized seeding happens in the kinematics solvers and shouldn't repeated in setFromIK()
  • Loading branch information
@rhaschke
rhaschke committed Jan 23, 2019
1 parent aa42e1e commit eebfe9c
Show file tree
Hide file tree
Showing 2 changed files with 28 additions and 104 deletions.
27 changes: 8 additions & 19 deletions moveit_core/robot_state/include/moveit/robot_state/robot_state.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -947,10 +947,9 @@ as the new values that correspond to the group */
set by computing inverse kinematics.
The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
@param pose The pose the last link in the chain needs to achieve
@param attempts The number of times IK is attempted
@param timeout The timeout passed to the kinematics solver on each attempt
@param constraint A state validity constraint to be required for IK solutions */
bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int attempts = 0,
bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose,
double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());

Expand All @@ -959,11 +958,9 @@ as the new values that correspond to the group */
The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
@param pose The pose the \e tip link in the chain needs to achieve
@param tip The name of the link the pose is specified for
@param attempts The number of times IK is attempted
@param timeout The timeout passed to the kinematics solver on each attempt
@param constraint A state validity constraint to be required for IK solutions */
bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
unsigned int attempts = 0, double timeout = 0.0,
bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip, double timeout = 0.0,
const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());

Expand All @@ -972,21 +969,18 @@ as the new values that correspond to the group */
The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
@param pose The pose the last link in the chain needs to achieve
@param tip The name of the link the pose is specified for
@param attempts The number of times IK is attempted
@param timeout The timeout passed to the kinematics solver on each attempt */
bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int attempts = 0,
bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose,
double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());

/** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be
set by computing inverse kinematics.
The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
@param pose The pose the last link in the chain needs to achieve
@param attempts The number of times IK is attempted
@param timeout The timeout passed to the kinematics solver on each attempt
@param constraint A state validity constraint to be required for IK solutions */
bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
unsigned int attempts = 0, double timeout = 0.0,
bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, double timeout = 0.0,
const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());

Expand All @@ -996,11 +990,10 @@ as the new values that correspond to the group */
@param pose The pose the last link in the chain needs to achieve
@param tip The name of the frame for which IK is attempted.
@param consistency_limits This specifies the desired distance between the solution and the seed state
@param attempts The number of times IK is attempted
@param timeout The timeout passed to the kinematics solver on each attempt
@param constraint A state validity constraint to be required for IK solutions */
bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
const std::vector<double>& consistency_limits, unsigned int attempts = 0, double timeout = 0.0,
const std::vector<double>& consistency_limits, double timeout = 0.0,
const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());

Expand All @@ -1011,11 +1004,10 @@ as the new values that correspond to the group */
to be in the same order as the order of the sub-groups in this group. Returns true on success.
@param poses The poses the last link in each chain needs to achieve
@param tips The names of the frames for which IK is attempted.
@param attempts The number of times IK is attempted
@param timeout The timeout passed to the kinematics solver on each attempt
@param constraint A state validity constraint to be required for IK solutions */
bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
const std::vector<std::string>& tips, unsigned int attempts = 0, double timeout = 0.0,
const std::vector<std::string>& tips, double timeout = 0.0,
const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());

Expand All @@ -1027,12 +1019,10 @@ as the new values that correspond to the group */
@param poses The poses the last link in each chain needs to achieve
@param tips The names of the frames for which IK is attempted.
@param consistency_limits This specifies the desired distance between the solution and the seed state
@param attempts The number of times IK is attempted
@param timeout The timeout passed to the kinematics solver on each attempt
@param constraint A state validity constraint to be required for IK solutions */
bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
unsigned int attempts = 0, double timeout = 0.0,
const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits, double timeout = 0.0,
const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());

Expand All @@ -1042,12 +1032,11 @@ as the new values that correspond to the group */
@param poses The poses the last link in each chain needs to achieve
@param tips The names of the frames for which IK is attempted.
@param consistency_limits This specifies the desired distance between the solution and the seed state
@param attempts The number of times IK is attempted
@param timeout The timeout passed to the kinematics solver on each attempt
@param constraint A state validity constraint to be required for IK solutions */
bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
const std::vector<std::string>& tips,
const std::vector<std::vector<double> >& consistency_limits, unsigned int attempts = 0,
const std::vector<std::vector<double> >& consistency_limits,
double timeout = 0.0,
const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
Expand Down
105 changes: 20 additions & 85 deletions moveit_core/robot_state/src/robot_state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1298,7 +1298,7 @@ bool RobotState::integrateVariableVelocity(const JointModelGroup* jmg, const Eig
return true;
}

bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose, unsigned int attempts,
bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose,
double timeout, const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options)
{
Expand All @@ -1308,20 +1308,20 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose
ROS_ERROR_NAMED(LOGNAME, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
return false;
}
return setFromIK(jmg, pose, solver->getTipFrame(), attempts, timeout, constraint, options);
return setFromIK(jmg, pose, solver->getTipFrame(), timeout, constraint, options);
}

bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose, const std::string& tip,
unsigned int attempts, double timeout, const GroupStateValidityCallbackFn& constraint,
double timeout, const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options)
{
Eigen::Isometry3d mat;
tf2::fromMsg(pose, mat);
static std::vector<double> consistency_limits;
return setFromIK(jmg, mat, tip, consistency_limits, attempts, timeout, constraint, options);
return setFromIK(jmg, mat, tip, consistency_limits, timeout, constraint, options);
}

bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose, unsigned int attempts,
bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose,
double timeout, const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options)
{
Expand All @@ -1332,15 +1332,15 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d&
return false;
}
static std::vector<double> consistency_limits;
return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, attempts, timeout, constraint, options);
return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, timeout, constraint, options);
}

bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose_in, const std::string& tip_in,
unsigned int attempts, double timeout, const GroupStateValidityCallbackFn& constraint,
double timeout, const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options)
{
static std::vector<double> consistency_limits;
return setFromIK(jmg, pose_in, tip_in, consistency_limits, attempts, timeout, constraint, options);
return setFromIK(jmg, pose_in, tip_in, consistency_limits, timeout, constraint, options);
}

namespace
Expand Down Expand Up @@ -1383,7 +1383,7 @@ bool RobotState::setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string&
}

bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose_in, const std::string& tip_in,
const std::vector<double>& consistency_limits_in, unsigned int attempts, double timeout,
const std::vector<double>& consistency_limits_in, double timeout,
const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options)
{
Expand All @@ -1397,21 +1397,21 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d&
std::vector<std::vector<double> > consistency_limits;
consistency_limits.push_back(consistency_limits_in);

return setFromIK(jmg, poses, tips, consistency_limits, attempts, timeout, constraint, options);
return setFromIK(jmg, poses, tips, consistency_limits, timeout, constraint, options);
}

bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Isometry3d& poses_in,
const std::vector<std::string>& tips_in, unsigned int attempts, double timeout,
const std::vector<std::string>& tips_in, double timeout,
const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options)
{
static const std::vector<std::vector<double> > CONSISTENCY_LIMITS;
return setFromIK(jmg, poses_in, tips_in, CONSISTENCY_LIMITS, attempts, timeout, constraint, options);
const std::vector<std::vector<double> > consistency_limits;
return setFromIK(jmg, poses_in, tips_in, consistency_limits, timeout, constraint, options);
}

bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Isometry3d& poses_in,
const std::vector<std::string>& tips_in,
const std::vector<std::vector<double> >& consistency_limit_sets, unsigned int attempts,
const std::vector<std::vector<double> >& consistency_limit_sets,
double timeout, const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options)
{
Expand Down Expand Up @@ -1615,9 +1615,6 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
if (timeout < std::numeric_limits<double>::epsilon())
timeout = jmg->getDefaultIKTimeout();

if (attempts == 0)
attempts = jmg->getDefaultIKAttempts();

// set callback function
kinematics::KinematicsBase::IKCallbackFn ik_callback_fn;
if (constraint)
Expand All @@ -1626,61 +1623,30 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
// Bijection
const std::vector<unsigned int>& bij = jmg->getKinematicsSolverJointBijection();

bool first_seed = true;
std::vector<double> initial_values;
for (unsigned int st = 0; st < attempts; ++st)
{
std::vector<double> seed(bij.size());

// the first seed is the current robot state joint values
if (first_seed)
{
first_seed = false;
copyJointGroupPositions(jmg, initial_values);
std::vector<double> seed(bij.size());
for (std::size_t i = 0; i < bij.size(); ++i)
seed[i] = initial_values[bij[i]];
}
else
{
ROS_DEBUG_NAMED(LOGNAME, "Rerunning IK solver with random joint positions");

// sample a random seed
random_numbers::RandomNumberGenerator& rng = getRandomNumberGenerator();
std::vector<double> random_values;
jmg->getVariableRandomPositions(rng, random_values);
for (std::size_t i = 0; i < bij.size(); ++i)
seed[i] = random_values[bij[i]];

if (options.lock_redundant_joints)
{
std::vector<unsigned int> red_joints;
solver->getRedundantJoints(red_joints);
copyJointGroupPositions(jmg, initial_values);
for (std::size_t i = 0; i < red_joints.size(); ++i)
seed[red_joints[i]] = initial_values[bij[red_joints[i]]];
}
}

// compute the IK solution
std::vector<double> ik_sol;
moveit_msgs::MoveItErrorCodes error;

if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, error, options,
this))
if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, error, options, this))
{
std::vector<double> solution(bij.size());
for (std::size_t i = 0; i < bij.size(); ++i)
solution[bij[i]] = ik_sol[i];
setJointGroupPositions(jmg, solution);
return true;
}
}
return false;
}

bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL::vector_Isometry3d& poses_in,
const std::vector<std::string>& tips_in,
const std::vector<std::vector<double> >& consistency_limits, unsigned int attempts,
const std::vector<std::vector<double> >& consistency_limits,
double timeout, const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options)
{
Expand Down Expand Up @@ -1813,69 +1779,38 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL::
ik_queries[i].orientation.w = quat.w();
}

if (attempts == 0)
attempts = jmg->getDefaultIKAttempts();

// if no timeout has been specified, use the default one
if (timeout < std::numeric_limits<double>::epsilon())
timeout = jmg->getDefaultIKTimeout();

bool first_seed = true;
for (unsigned int st = 0; st < attempts; ++st)
{
bool found_solution = true;
for (std::size_t sg = 0; sg < sub_groups.size(); ++sg)
{
const std::vector<unsigned int>& bij = sub_groups[sg]->getKinematicsSolverJointBijection();
std::vector<double> seed(bij.size());
// the first seed is the initial state
if (first_seed)
{
std::vector<double> initial_values;
copyJointGroupPositions(sub_groups[sg], initial_values);
for (std::size_t i = 0; i < bij.size(); ++i)
seed[i] = initial_values[bij[i]];
}
else
{
// sample a random seed
random_numbers::RandomNumberGenerator& rng = getRandomNumberGenerator();
std::vector<double> random_values;
sub_groups[sg]->getVariableRandomPositions(rng, random_values);
for (std::size_t i = 0; i < bij.size(); ++i)
seed[i] = random_values[bij[i]];
}

// compute the IK solution
std::vector<double> ik_sol;
moveit_msgs::MoveItErrorCodes error;
const std::vector<double>& climits = consistency_limits.empty() ? std::vector<double>() : consistency_limits[sg];
if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, timeout, climits, ik_sol, error))
{
if (!solvers[sg]->searchPositionIK(ik_queries[sg], seed, timeout, climits, ik_sol, error))
return false;

std::vector<double> solution(bij.size());
for (std::size_t i = 0; i < bij.size(); ++i)
solution[bij[i]] = ik_sol[i];
setJointGroupPositions(sub_groups[sg], solution);
}
else
{
found_solution = false;
break;
}
ROS_DEBUG_NAMED(LOGNAME, "IK attempt: %d of %d", st, attempts);
}
if (found_solution)
{
std::vector<double> full_solution;
copyJointGroupPositions(jmg, full_solution);
if (constraint ? constraint(this, jmg, &full_solution[0]) : true)
{
ROS_DEBUG_NAMED(LOGNAME, "Found IK solution");
return true;
}
}
}
return false;
}

double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
Expand Down

0 comments on commit eebfe9c

Please sign in to comment.