void RenderModel::
resetCameraSettings()
{
auto camera = this->camera.write ();
camera->q = vectord(0,0,.5f);
camera->p = vectord(0,0,-10.f);
camera->r = vectord(91,180,0);
camera->xscale = -camera->p[2]*0.1f;
camera->zscale = -camera->p[2]*0.75f;
float L = tfr_mapping ().read ()->length();
if (L)
{
camera->xscale = 10/L;
camera->q[0] = 0.5*L;
}
#ifdef TARGET_hast
camera->p[2] = -6;
xscale = 0.1f;
if (L)
camera->xscale = 14/L;
#endif
}
void RenderModel::
setTestCamera()
{
auto c = camera.write();
render_settings.y_scale = 0.01f;
c->q = vectord(63.4565,0,0.37);
c->p = vectord(0,0,-10);
c->r = vectord(46.2, 253.186, 0);
c->orthoview.reset( c->r[0] >= 90 );
}
void GraphicsScene::
updateMousePosInWorldCoordinates()
{
Tools::Support::RenderViewInfo r(renderview_->model);
Heightmap::Position cursorPos = r.getPlanePos( renderview_->glwidget->mapFromGlobal(QCursor::pos()) );
renderview_->model->render_settings.cursor = vectord(cursorPos.time, 0, cursorPos.scale);
}
/**************************************************************************************************
* Procecure *
* *
* Description: chooseActiveVariables *
* Class : iCubOptimizable *
**************************************************************************************************/
void iCubOptimizable::chooseActiveVariables(vectord& query)
{
if (dim != query.size())
{
cout << endl << "[ERROR] Query size is not equal to mask active components. From: iCubOptimizable::chooseActiveVariables.";
exit(-1);
}
uint variables_updated = 0;
vectord result = vectord(_original_dim, 0.0);
for (uint index = 0; index < _original_dim; index += 1)
{
if (_active_variables_mask[index] == true)
{
result[index] = query[variables_updated];
variables_updated += 1;
}
else
{
result[index] = _icubparams.default_query[index];
}
}
// Return new query
query = result;
}
/**************************************************************************************************
* Procecure *
* *
* Description: setExplorationBoundingBox *
* Class : iCubOptimizable *
**************************************************************************************************/
void iCubOptimizable::setExplorationBoundingBox(void)
{
lower_bound = vectord(_original_dim, 0);
upper_bound = vectord(_original_dim, 1);
// Set dx, dy, dz, droll, dpitch, dyaw bounds
for(uint index = 0; index < 6; index += 1)
{
lower_bound(index) = 0;
upper_bound(index) = 1000;
}
// Grasp Auxiliary Components
for(uint index = 6; index < _original_dim; index += 1)
{
lower_bound(index) = -M_PI;
upper_bound(index) = M_PI;
}
}
/**************************************************************************************************
* Procecure *
* *
* Description: applyQueryToHand *
* Class : iCubOptimizable *
**************************************************************************************************/
vector<GraspResult> iCubOptimizable::applyQueryToHand(const vectord& query, std::vector<double>& position, std::vector<double>& orientation)
{
// Variables
vector<GraspResult> qualities;
vectord noise(_original_dim);
unsigned rand_seed = std::chrono::system_clock::now().time_since_epoch().count();
default_random_engine generator(rand_seed);
vector<uint> index_dim;
for(uint trial = 0; trial < _icubparams.n_grasp_trials; trial += 1)
{
// Reset number of contacts
_number_of_contacts = 0;
noise = query;
if (_icubparams.n_grasp_trials > 1)
{
// Add noise to query
for(uint index = 0; index < _original_dim; index += 1)
{
if ( (noise(index) >= lower_bound(index)) &&
(noise(index) <= upper_bound(index)) )
{
noise(index) = query(index) + (_noise_distribution[index])(generator);
}
}
}
std::vector<double> dummy1, dummy2;
// Reset Grasp and Hand configs
applyQueryToHandMinimal(vectord(_original_dim, 0), dummy1, dummy2);
// Apply inteded query
GraspQualityPtr quality = applyQueryToHandMinimal(noise, position, orientation);
if (!quality) return qualities;
// Return quality
if (_number_of_contacts > 1) { qualities.push_back(GraspResult(quality));}
else { return qualities;}
if (quality -> getGraspQuality() < _icubparams.grasp_threshold) return qualities;
}
// Return qualities
return qualities;
}
/**************************************************************************************************
* Procecure *
* *
* Description: getBoundingBox *
* Class : iCubOptimizable *
**************************************************************************************************/
void iCubOptimizable::getBoundingBox(vectord& lower, vectord& upper)
{
uint active_counter = 0;
lower = vectord(dim, 0.0);
upper = vectord(dim, 0.0);
for (uint index = 0; index < _original_dim; index += 1)
{
if (_active_variables_mask[index] == true)
{
lower[active_counter] = lower_bound[index];
upper[active_counter] = upper_bound[index];
active_counter += 1;
}
}
if (active_counter != dim)
{
cout << endl << "[ERROR] Bounds size are not equal to mask active components. From: iCubOptimizable::getBoundingBox.";
exit(-1);
}
};
/**************************************************************************************************
* Procecure *
* *
* Description: Constructor *
* Class : iCubOptimizable *
**************************************************************************************************/
iCubOptimizable::iCubOptimizable(iCubRobotPtr robot, SceneObjectPtr object, iCubOptParameters& params) :
_icubparams(params)
{
this -> dim = robot -> calculateNumberOfDimensionsForLearning(params.grasp);
this -> name = "iCubOptimizable";
this -> ymin = 0;
this -> ymax = 1;
_robot = robot;
_col_checker = CollisionChecker::getGlobalCollisionChecker();
_object_set.reset(new SceneObjectSet());
_object_set -> addSceneObject(object);
_object = _object_set -> getSceneObject(0);
_original_dim = dim;
_active_variables_mask.resize(_original_dim);
if ( (_icubparams.default_query.size() == 0) || (_icubparams.active_variables.size() == 0) )
{
_icubparams.default_query = vectord(_original_dim, 0.0);
for (uint index = 0; index < _original_dim; index += 1)
{
_icubparams.active_variables.push_back(index);
}
}
if (_original_dim != _icubparams.default_query.size())
{
cout << endl << "[ERROR] Default Query size doesn't match iCubOptimizable Dim. From: iCubOptimizable::iCubOptimizable.";
exit(-1);
}
for (uint index = 0; index < _original_dim; index += 1)
{
_active_variables_mask[index] = true;
}
setExplorationBoundingBox();
initializeRandomDist(_icubparams.trial_stddev);
selectOptVariables();
}
