constrained_ik: solver_state.cpp Source File

Go to the documentation of this file.
00001 
00026 #include "constrained_ik/solver_state.h"
00027 #include <limits>
00028 #include <ros/ros.h>
00029 
00030 using namespace Eigen;
00031 
00032 namespace constrained_ik
00033 {
00034 
00035 SolverState::SolverState(const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed)
00036 {
00037   reset(goal, joint_seed);
00038 }
00039 
00040 void SolverState::reset(const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed)
00041 {
00042   this->goal = goal;
00043   this->joint_seed = joint_seed;
00044   this->iter = 0;
00045   this->joints = VectorXd::Constant(joint_seed.size(), std::numeric_limits<double>::max());
00046   this->joints_delta = VectorXd::Zero(joint_seed.size());
00047   this->primary_sum = 0.0;
00048   this->auxiliary_sum = 0.0;
00049   this->auxiliary_at_limit = false;
00050   this->pose_estimate = Affine3d::Identity();
00051   this->condition = initialization_state::NothingInitialized;
00052 }
00053 
00054 } // namespace constrained_ik
00055