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;
}
}
}
// 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;
}
bool CentroidPartitioner::sort_z (const std::pair<Point, Elem*>& lhs,
const std::pair<Point, Elem*>& rhs)
{
return (lhs.first(2) < rhs.first(2));
}