示例#1
0
void moveit::core::JointModelGroup::setSolverAllocators(const std::pair<SolverAllocatorFn, SolverAllocatorMapFn> &solvers)
{
  if (solvers.first)
  {
    group_kinematics_.first.allocator_ = solvers.first;
    group_kinematics_.first.solver_instance_ = solvers.first(this);
    if (group_kinematics_.first.solver_instance_)
    {
      group_kinematics_.first.solver_instance_->setDefaultTimeout(group_kinematics_.first.default_ik_timeout_);
      group_kinematics_.first.solver_instance_const_ = group_kinematics_.first.solver_instance_;
      if (!computeIKIndexBijection(group_kinematics_.first.solver_instance_->getJointNames(),
                                   group_kinematics_.first.bijection_))
        group_kinematics_.first.reset();
    }
  }
  else
    // we now compute a joint bijection only if we have a solver map
    for (SolverAllocatorMapFn::const_iterator it = solvers.second.begin() ; it != solvers.second.end() ; ++it)
      if (it->first->getSolverInstance())
      {
        KinematicsSolver &ks = group_kinematics_.second[it->first];
        ks.allocator_ = it->second;
        ks.solver_instance_ = const_cast<JointModelGroup*>(it->first)->getSolverInstance();
        ks.solver_instance_const_ = ks.solver_instance_;
        ks.default_ik_timeout_ = group_kinematics_.first.default_ik_timeout_;
        ks.default_ik_attempts_ = group_kinematics_.first.default_ik_attempts_;
        if (!computeIKIndexBijection(ks.solver_instance_->getJointNames(), ks.bijection_))
        {
          group_kinematics_.second.clear();
          break;
        }
      }
}
示例#2
0
// This function is the derivative of the residual() function with respect to the parameters.
parameter_vector testClass::residual_derivative (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    parameter_vector der;

    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);
    const double p3 = params(3);

    const double i0 = data.first(0);
    const double i1 = data.first(1);

    // const double temp = p0*i0 + p1*i1 + p2;
    const double temp = exp(- p1 * i0 * i0 - p2 * i0 * i1 - p3 * i1 * i1);

	der(0) = temp;
	der(1) = - p0 * temp * ( i0*i0);
	der(2) = - p0 * temp * ( i1*i0);
	der(3) = - p0 * temp * ( i1*i1);


    // der(0) = i0*2*temp;
    // der(1) = i1*2*temp;
    // der(2) = 2*temp;

    return der;
}
// This function is the derivative of the residual() function with respect to the parameters.
parameter_vector residual_derivative (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    parameter_vector der;

    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);

    const double i0 = data.first(0);
    const double i1 = data.first(1);

    const double temp = p0*i0 + p1*i1 + p2;

    der(0) = i0*2*temp;
    der(1) = i1*2*temp;
    der(2) = 2*temp;

    return der;
}
示例#4
0
bool CentroidPartitioner::sort_z (const std::pair<Point, Elem*>& lhs,
                                  const std::pair<Point, Elem*>& rhs)
{
  return (lhs.first(2) < rhs.first(2));
}