// Return list of options in a section. (not implemented yet)
//vector<string> options(string section);
vector<string> listSections(void)
{
vector<string> out( config.size() );
int i=0;
for(config_t::iterator it=config.begin(); it!=config.end(); it++)
out[i++] = it->first;
return out;
}
bool unigripper_motoman_plant_t::IK_steering( const config_t& start_config, const config_t& goal_config, sim::plan_t& result_plan, bool set_grasping )
{
//Begin by determining a distance between the start and goal configurations
double distance = start_config.get_position().distance( goal_config.get_position() );
//Set the number of interpolation steps
double steps = std::ceil(distance / MAX_IK_STEP);
//Clear out the plan we were given, we will be filling it in ourselves
result_plan.clear();
//Keep around an intermediate plan for systematically building up the solution
plan_t intermediate_plan = result_plan;
//Store the arm's current state as the previous state
state_space->copy_to_point(prev_st);
//So first, let's do the IK for the start configuration
if( !IK_solver( start_config, prev_st, set_grasping, prev_st, true ) )
{
PRX_DEBUG_COLOR("IK failed in Steering for the start config.", PRX_TEXT_RED);
return false;
}
//Storage for the interpolated configuration
config_t interpolated_config;
//Then, for each step
for( double i=1; i<=steps; ++i )
{
//Compute the next interpolated configuration
double t = PRX_MINIMUM( (i/steps), 1.0 );
interpolated_config.interpolate( start_config, goal_config, t );
//Compute Minimum IK for interpolated configuration (Pose, seed state, soln)
if( IK_solver( interpolated_config, inter_st, set_grasping, prev_st, true ) )
{
//Create a plan by steering between states and add it to the resulting plan
intermediate_plan.clear();
steering_function(prev_st, inter_st, intermediate_plan);
result_plan += intermediate_plan;
//And make sure we store this state as our prior state
state_space->copy_point(prev_st, inter_st);
}
//If the MinIK fails, then the steering is a failure, abort
else
{
//Clear out whatever plan we had started making
PRX_DEBUG_COLOR("IK failed in Steering", PRX_TEXT_BROWN);
result_plan.clear();
return false;
}
}
//Report our Success
return true;
}
void config_t::global_to_relative( const config_t& root_config )
{
//Have an identity temporary orientaiton
util::quaternion_t tmp_orient;
tmp_orient.zero();
//Then, set our position to be the relative position
this->set_position( this->get_position() - root_config.get_position() );
tmp_orient /= root_config.get_orientation();
this->set_position(vector_t(tmp_orient.qv_rotation(this->get_position())));
tmp_orient *= this->get_orientation();
tmp_orient.normalize();
PRX_ASSERT(tmp_orient.is_valid());
this->set_orientation(tmp_orient);
}
///
/// \brief YoloDarknetDetector::Init
/// \return
///
bool YoloDarknetDetector::Init(const config_t& config)
{
auto modelConfiguration = config.find("modelConfiguration");
auto modelBinary = config.find("modelBinary");
if (modelConfiguration != config.end() && modelBinary != config.end())
{
m_detector = std::make_unique<Detector>(modelConfiguration->second, modelBinary->second);
m_detector->nms = 0.2f;
}
auto classNames = config.find("classNames");
if (classNames != config.end())
{
std::ifstream classNamesFile(classNames->second);
if (classNamesFile.is_open())
{
m_classNames.clear();
std::string className;
for (; std::getline(classNamesFile, className); )
{
m_classNames.push_back(className);
}
}
}
auto confidenceThreshold = config.find("confidenceThreshold");
if (confidenceThreshold != config.end())
{
m_confidenceThreshold = std::stof(confidenceThreshold->second);
}
auto maxCropRatio = config.find("maxCropRatio");
if (maxCropRatio != config.end())
{
m_maxCropRatio = std::stof(maxCropRatio->second);
if (m_maxCropRatio < 1.f)
{
m_maxCropRatio = 1.f;
}
}
bool correct = m_detector.get() != nullptr;
return correct;
}
void unigripper_motoman_plant_t::get_end_effector_configuration(config_t& effector_config)
{
double qw,qx,qy,qz;
bool update = true;
VectorNd tmpVec = VectorNd::Zero(3);
VectorNd pos;
Matrix3d orient;
Quaternion quat;
std::string ee = "vacuum_volume";
unsigned int id = model.GetBodyId(ee.c_str());
pos = CalcBodyToBaseCoordinates(model,Q,id,tmpVec,update);
update = false;
orient = CalcBodyWorldOrientation(model,Q,id,update);
quat = Quaternion::fromMatrix(orient);
effector_config.set_position(pos[0], pos[1], pos[2]);
effector_config.set_orientation(quat[0],quat[1],quat[2],quat[3]);
if(std::isnan(quat.w()) || std::isnan(quat.x()) || std::isnan(quat.y()) || std::isnan(quat.z()) )
{
uni_convert_matrix_to_quaternion(orient,qx,qy,qz,qw);
effector_config.set_orientation(qx,qy,qz,qw);
}
}
port_mapping_t::port_mapping_t(const config_t& config):
m_pinned(config.network().ports().pinned())
{
port_t minimum, maximum;
std::tie(minimum, maximum) = config.network().ports().shared();
std::vector<port_t> seed;
if((minimum == 0 && maximum == 0) || maximum <= minimum) {
seed.resize(65535);
std::fill(seed.begin(), seed.end(), 0);
} else {
seed.resize(maximum - minimum);
std::iota(seed.begin(), seed.end(), minimum);
}
std::random_device device;
std::shuffle(seed.begin(), seed.end(), std::default_random_engine(device()));
// Populate the shared port queue.
m_shared = std::deque<port_t>(seed.begin(), seed.end());
}
void movable_body_plant_t::get_configuration( config_t& body_config ) const
{
body_config.set_position(_x, _y, _z);
body_config.set_xyzw_orientation(_qx, _qy, _qz, _qw);
}
bool unigripper_motoman_plant_t::IK_solver(const config_t& effector_config, space_point_t* computed_state, bool set_grasping, const space_point_t* seed_state, bool do_min)
{
double qx,qy,qz,qw;
double x,y,z;
effector_config.get_orientation().get(qx,qy,qz,qw);
effector_config.get_position(x,y,z);
KDL::Chain chain1;
my_tree->getChain("base_link","vacuum_volume",chain1);
KDL::JntArray q(chain1.getNrOfJoints());
KDL::JntArray q_out(chain1.getNrOfJoints());
KDL::JntArray q_min(chain1.getNrOfJoints());
KDL::JntArray q_max(chain1.getNrOfJoints());
std::vector< double > state_var;
if( seed_state != NULL )
state_space->copy_point_to_vector( seed_state, state_var );
else
state_space->copy_to_vector( state_var );
q(0) = state_var[0];
q(1) = state_var[1];
q(2) = state_var[2];
q(3) = state_var[3];
q(4) = state_var[4];
q(5) = state_var[5];
q(6) = state_var[6];
q(7) = state_var[7];
q(8) = state_var[15];
q_min(0) = state_space->get_bounds()[0]->get_lower_bound();
q_min(1) = state_space->get_bounds()[1]->get_lower_bound();
q_min(2) = state_space->get_bounds()[2]->get_lower_bound();
q_min(3) = state_space->get_bounds()[3]->get_lower_bound();
q_min(4) = state_space->get_bounds()[4]->get_lower_bound();
q_min(5) = state_space->get_bounds()[5]->get_lower_bound();
q_min(6) = state_space->get_bounds()[6]->get_lower_bound();
q_min(7) = state_space->get_bounds()[7]->get_lower_bound();
q_min(8) = state_space->get_bounds()[15]->get_lower_bound();
q_max(0) = state_space->get_bounds()[0]->get_upper_bound();
q_max(1) = state_space->get_bounds()[1]->get_upper_bound();
q_max(2) = state_space->get_bounds()[2]->get_upper_bound();
q_max(3) = state_space->get_bounds()[3]->get_upper_bound();
q_max(4) = state_space->get_bounds()[4]->get_upper_bound();
q_max(5) = state_space->get_bounds()[5]->get_upper_bound();
q_max(6) = state_space->get_bounds()[6]->get_upper_bound();
q_max(7) = state_space->get_bounds()[7]->get_upper_bound();
q_max(8) = state_space->get_bounds()[15]->get_upper_bound();
KDL::ChainFkSolverPos_recursive fk_solver(chain1);
KDL::ChainIkSolverVel_pinv ik_solver_vel(chain1);
KDL::ChainIkSolverPos_NR_JL ik_solver(chain1,q_min,q_max,fk_solver,ik_solver_vel,1000,1e-6);
KDL::Frame F(KDL::Rotation::Quaternion(qx,qy,qz,qw),KDL::Vector(x,y,z));
bool ik_res = (ik_solver.CartToJnt(q,F,q_out)>=0);
if(ik_res)
{
std::vector<double> state_vec;
state_vec = state_var;
state_vec[0] = q_out(0);
state_vec[1] = q_out(1);
state_vec[2] = q_out(2);
state_vec[3] = q_out(3);
state_vec[4] = q_out(4);
state_vec[5] = q_out(5);
state_vec[6] = q_out(6);
state_vec[7] = q_out(7);
state_vec[15] = q_out(8);
state_vec[16] = set_grasping ? (double)GRIPPER_CLOSED : GRIPPER_OPEN;
state_space->copy_vector_to_point( state_vec, computed_state );
}
return ik_res;
}
bool hasSection(string section)
{
return config.find(section) != config.end();
}
void config_t::relative_to_global(const config_t& root_config)
{
set_position(vector_t(root_config.get_orientation().qv_rotation(position)));
*this = root_config + *this;
}
void config_t::interpolate(const config_t& start, const config_t& target, double t)
{
position.interpolate(start.get_position(), target.get_position(), t);
orientation.interpolate(start.get_orientation(), target.get_orientation(), t);
}
bool kv_array_t::open ( config_t & config )
{
try
{
int count = (( hustdb_t * ) G_APPTOOL->get_hustdb ())->get_worker_count () + 1;
m_get_buffers.resize ( count );
for ( int i = 0; i < count; ++ i )
{
std::string & s = m_get_buffers[ i ].value;
s.resize ( RESERVE_BYTES_FOR_RSP_BUFER );
memset ( & s[ 0 ], 0, RESERVE_BYTES_FOR_RSP_BUFER );
s.resize ( 0 );
}
}
catch ( ... )
{
LOG_ERROR ( "[kv_array]bad_alloc" );
return false;
}
int count = config.get_max_file_count ();
try
{
m_files.resize ( count, NULL );
}
catch ( ... )
{
LOG_ERROR ( "[kv_array]bad_alloc" );
return false;
}
int i;
for ( i = 0; i < count; ++ i )
{
const char * path = config.get_file_path ( i );
if ( NULL == path )
{
LOG_ERROR ( "[kv_array]config.get_file_path( %d ) failed", i );
return false;
}
if ( '\0' == * path )
{
continue;
}
i_kv_t * o = NULL;
try
{
o = create_file ();
}
catch ( ... )
{
LOG_ERROR ( "[kv_array]bad_alloc" );
return false;
}
if ( NULL == o )
{
LOG_ERROR ( "[kv_array]create_file() return NULL" );
return false;
}
const kv_config_t & kv_cfg = config.get_kv_config ();
if ( ! o->open ( path, kv_cfg, i ) )
{
LOG_ERROR ( "[kv_array]o->open( %s ) failed", path );
o->kill_me ();
return false;
}
LOG_DEBUG ( "[kv_array][i=%u][p=%p][file_id=%u] file opened[path=%s]", i, o, o->get_id (), o->get_path () );
m_files[ i ] = o;
}
m_ok = true;
return true;
}
