/*!
This populates the quality measure list with the currently defined quality
measures for this grasp. Then it populates the QM comboBox with the
all the possible quality measure types. Next, it creates an empty
settings area widget to hold the settings for the individual types of
quality measures, and adds it to the layout.
*/
void QMDlg::init()
{
std::list<QualityMeasure *>::iterator qp;
Grasp *g = graspitCore->getWorld()->getCurrentHand()->getGrasp();
int i;
qmListBox->insertItem("New quality measure");
for (qp=g->qmList.begin(),i=1;qp!=g->qmList.end();qp++,i++)
qmListBox->insertItem((*qp)->getName());
for (i=0;QualityMeasure::TYPE_LIST[i];i++)
qmTypeComboBox->insertItem(QString(QualityMeasure::TYPE_LIST[i]));
qmSettingsBox->setColumnLayout(0, Qt::Vertical );
QHBoxLayout *settingsBoxLayout = new QHBoxLayout(qmSettingsBox->layout());
settingsBoxLayout->setAlignment( Qt::AlignTop );
qmDlgData.settingsArea = new QWidget(qmSettingsBox);
settingsBoxLayout->addWidget(qmDlgData.settingsArea);
qmDlgData.grasp = g;
qmDlgData.qmTypeComboBox = qmTypeComboBox;
qmDlgData.qmName = qmName;
gravityBox->setChecked( g->isGravitySet() );
qmListBox->setCurrentItem(0);
}
/*!
If "New Quality Measure" is selected, the first item is the QM type box
is selected, and the delete button is disabled. Otherwise, it updates
the name in the text area, and sets the QM type in the comboBox to the
type of thecurrently selected QM, and finally calls the updateSettingsBox
to update the settings area.
*/
void QMDlg::selectQM( int which)
{
if (which==0) { // "New quality measure" selected
qmDlgData.currQM = NULL;
qmDlgData.qmType = QualityMeasure::TYPE_LIST[0];
qmTypeComboBox->setCurrentItem(0);
DeleteButton->setEnabled(false);
qmName->setText(QString("New Quality Measure"));
}
else {
DeleteButton->setEnabled(true);
Grasp *g = graspitCore->getWorld()->getCurrentHand()->getGrasp();
qmDlgData.currQM = g->getQM(which-1);
for (int i=0;QualityMeasure::TYPE_LIST[i];i++) {
if (!strcmp(QualityMeasure::TYPE_LIST[i],qmDlgData.currQM->getType())) {
qmTypeComboBox->setCurrentItem(i);
qmDlgData.qmType = QualityMeasure::TYPE_LIST[i];
break;
}
}
qmName->setText(qmListBox->text(which));
}
updateSettingsBox();
}
void GraspSet::labelHypothesis(const PointList& point_list, const FingerHand& finger_hand, Grasp& hand)
const
{
Antipodal antipodal;
int label = antipodal.evaluateGrasp(point_list, 0.003, finger_hand.getLateralAxis(), finger_hand.getForwardAxis(),
rotation_axis_);
hand.setHalfAntipodal(label == Antipodal::HALF_GRASP || label == Antipodal::FULL_GRASP);
hand.setFullAntipodal(label == Antipodal::FULL_GRASP);
}
/*!
When the user clicks the new button, a quality measure dialog box
( QMDlg ) is created. If on the return from that, there is at least one
QM defined, the generate button is enabled, and the QM comboBox is populated
with the currently defined quality measures.
*/
void PlannerDlg::newQM()
{
Grasp *grasp = graspitCore->getWorld()->getCurrentHand()->getGrasp();
graspitCore->getMainWindow()->graspQualityMeasures();
if (grasp->getNumQM() > 0) { GenerateButton->setEnabled(true); }
qmComboBox->clear();
for (int i = 0; i < grasp->getNumQM(); i++) {
qmComboBox->insertItem(grasp->getQM(i)->getName());
}
}
void QMDlg::gravityBox_clicked()
{
Grasp *g = graspitCore->getWorld()->getCurrentHand()->getGrasp();
g->setGravity( gravityBox->isChecked() );
if ( gravityBox->isChecked() ) {
fprintf(stderr,"Gravity on\n");
} else {
fprintf(stderr,"Gravity off\n");
}
// g->updateWrenchSpaces();
g->update();
}
/*!
First this creates a new grasp_manager and gets the default planning and
testing parameters. Then it sets up number validators for the text entry
boxes. Finally it populates the quality measure comboBox with the names
of the currently defined quality measures for this grasp. If there are
no quality measures defined, the generate button is disabled, but the user
can add QM's by pressing the new button in this dialog box.
*/
void PlannerDlg::init()
{
Grasp *grasp;
int a, b, c, d, e;
int density;
double f;
int i;
myGraspManager = new grasp_manager;
myGraspManager->get_graspPlanner()->get_planningParameters(a, b, c, d);
myGraspManager->get_graspTester()->get_testingParameters(e, f);
density = myGraspManager->get_graspPlanner()->get_parameterMode();
densityFactorLine->setText(QString::number(density));
densityFactorLine->setValidator(new QIntValidator(1, 100, this));
num360stepsLine->setText(QString::number(a));
num360stepsLine->setValidator(new QIntValidator(1, 999, this));
numParPlanesLine->setText(QString::number(b));
numParPlanesLine->setValidator(new QIntValidator(1, 999, this));
num180graspsLine->setText(QString::number(c));
num180graspsLine->setValidator(new QIntValidator(1, 999, this));
numGraspRotsLine->setText(QString::number(d));
numGraspRotsLine->setValidator(new QIntValidator(1, 999, this));
maxNumStepsLine->setText(QString::number(e));
maxNumStepsLine->setValidator(new QIntValidator(1, 999, this));
backstepSizeLine->setText(QString::number(f));
backstepSizeLine->setValidator(new QDoubleValidator(0, 1000, 6, this));
qmComboBox->clear();
grasp = graspitCore->getWorld()->getCurrentHand()->getGrasp();
if (grasp->getNumQM() == 0) { GenerateButton->setEnabled(false); }
else {
for (i = 0; i < grasp->getNumQM(); i++) {
qmComboBox->insertItem(grasp->getQM(i)->getName());
}
}
}
/*!
Creates a new quality measure of the selected type with the given name from
the name text area. If "New Quality Measure" is selected in the list, it
adds this QM to the grasp and to the quality measure list. Otherwise it
replaces the currently selected QM with the new one.
*/
void QMDlg::addEditQM()
{
Grasp *g = graspitCore->getWorld()->getCurrentHand()->getGrasp();
QualityMeasure *newQM;
int selectedQM;
newQM = QualityMeasure::createInstance(&qmDlgData);
selectedQM = qmListBox->currentItem();
if (selectedQM == 0) { // create a new quality measure
g->addQM(newQM);
qmListBox->insertItem(qmName->text());
}
else { // replace an old quality measure with a new one
g->replaceQM(selectedQM-1,newQM);
qmListBox->changeItem(qmName->text(),selectedQM);
}
qmListBox->setCurrentItem(0);
qmListBox->update();
qmListBox->show();
}
//------------------------------------------------------------------
VectorXui generateRandomGrasp(TargetObjectPtr object,const FParamList f_parameters, WrenchSpacePtr const tws)
{
assert(object->getNumCp() >= f_parameters.size());
struct timeval c_time;
uint n_fingers=f_parameters.size();
VectorXui centerpoint_ids(n_fingers);
while(1)
{
std::list<uint> l;
for(uint i=0; i<n_fingers;i++)
{
while (1)
{
gettimeofday(&c_time,0);
srand(c_time.tv_usec);
bool is_unique=true;
uint rand_val=(rand() % (object->getNumCp()));
for (uint j=0; j<i;j++)
if (centerpoint_ids(j)==rand_val)
is_unique=false;
if (is_unique)
{
centerpoint_ids(i)=rand_val;
break;
}
}
}
Grasp grasp;
grasp.init(f_parameters,object,centerpoint_ids);
if (tws->getWrenchSpaceType()==Spherical)
{
if(grasp.getGWS()->getOcInsphereRadius() >= dynamic_cast<const SphericalWrenchSpace*>(tws.get())->getRadius())
break;
}
else if (tws->getWrenchSpaceType()==Discrete)
{
//Should ascertain that the TWS contains the origin ...
uint nTW = dynamic_cast<const DiscreteTaskWrenchSpace*>(tws.get())->getNumWrenches();
Eigen::MatrixXd TWS(6,nTW);
doubleArrayToEigenMatrix(dynamic_cast<const DiscreteTaskWrenchSpace*>(tws.get())->getWrenches().get(),TWS);
bool contained =true;
Eigen::MatrixXd nh(6,1); Eigen::MatrixXd eh(1,1);
facetT* curr_f= grasp.getGWS()->getConvexHull()->beginFacet().getFacetT();
for (uint h=0; h< grasp.getGWS()->getConvexHull()->facetCount();h++)
{
doubleArrayToEigenMatrix(curr_f->normal,nh);
eh(0,0)=-curr_f->offset;
nh=-nh;
if((TWS.transpose()*nh+eh.replicate(nTW,1)).minCoeff() <= 0)
{
contained =false;
break;
}
curr_f=curr_f->next;
}
if (contained)
break;
}
else
{
std::cout<<"Error in generateRandomGrasp(TargetObjectPtr object,const FParamList f_parameters,const WrenchSpace* const tws) - Invalid task wrench space type, cannot compute a valid random grasp. Exiting ..."<<std::endl;
exit(0);
}
}
return centerpoint_ids;
}
int main(int argc, char** argv)
{
//Settings:
//From regression algorithm
double mass(3); //1kg
double sigmaMass(0.2); //uncertainty: 0.2kg
Eigen::Vector3d com(0.0,0,0); //center of mass in which frame??
Eigen::Vector3d sigmaCom(0.05,0.05,0.05); //uncertainty center of mass in meters
Eigen::Matrix3d camera_to_pca_rot=Eigen::Matrix3d::Identity();
//Gravity Direction (s) TODO: Later add set of Rotations of body
Eigen::Vector3d gravity_normal(0,0,-1); //in the frame of the camera, but here cam to pca is identity for testing
//settings hand and environment;
double maxForceHand=50; //maximum contact force in each contact is 50 Newtons
double noiseForce=1; //1 Newton noise in force
double noiseTorque=noiseForce*0.05; // Noise in Torques, actually could be computed from the maximum bounding sphere of sigmaCom
//Transforms:
Transform camera_to_world, world_to_hand;
camera_to_world.translation << 0,0,0;
camera_to_world.rotation = Eigen::Matrix3d::Identity();
//gripper frame is 2 cm below object with 3 fingers pointing upwards to
//check if non force closure grasps works together with gravity, translation along z should not play any role for the grasp quality measure!
world_to_hand.translation << 0,0,-0.02;
world_to_hand.rotation = Eigen::Matrix3d::Identity();
//defining all contacts for the grasp!
Grasp grasp;
Vector6d contact1, contact2, contact3, contact4;
//3 Finger Grasp, hard finger contact with friction, in hand frame pointing upwards at positions building a triangle, it might make the hull degenerate?
contact1 << 0, 0.03 ,0.01, 0,0,1;
contact2 << -0.03*0.5,-0.03*0.87 ,0.01, 0,0,1;
contact3 << 0.03*0.5,-0.03*0.87 ,0.01, 0,0,1;
//contact4 << 0,0,0.03, 0,0,-1; //add grasp from above!
grasp.addContact(contact1);
grasp.addContact(contact2);
grasp.addContact(contact3);
//grasp.addContact(contact4);
grasp.setFriction(0.3);
//starting clock, computing Task Wrench Ellipsoids
clock_t begin, end;
double time_spent;
begin = clock();
cout << "Starting Timer ..." << endl;
taskwrenchEllipsoid tws_ell;
tws_ell.setInput(mass,sigmaMass, com, sigmaCom, camera_to_pca_rot);
tws_ell.setNoise(noiseForce,noiseTorque);
tws_ell.setCameraCalib(camera_to_world);
tws_ell.setGripperPose(world_to_hand);
tws_ell.setGravityNormal(gravity_normal);
tws_ell.sampleComEllipsoid();
tws_ell.computeTorqueEllipsoid();
end = clock();
time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
cout<<"Computation Time for Ellipsoid: "<< time_spent << endl;
tws_ell.writeSampledPts("/home/alexander/tmp/2dGraspWrenchSpace/sampledPts.txt");
tws_ell.writeTaskEllipse("/home/alexander/tmp/2dGraspWrenchSpace/taskEll.txt");
Transform hand_to_ellipse;
Eigen::Vector3d semiAxesTorque;
Eigen::Vector3d semiAxesForces;
Eigen::Vector3d forceOffset; //in pca frame!
tws_ell.getLinearTransform(hand_to_ellipse, semiAxesTorque, semiAxesForces,forceOffset);
//writing the semiaxes + rotation also to a file!
//After get Linear Transform!!!!!!!!
tws_ell.writeTransformedPts("/home/alexander/tmp/2dGraspWrenchSpace/sampledPtsTrans.txt");//torque points transformed to gripper frame
tws_ell.writeForceTorqueEllipse("/home/alexander/tmp/2dGraspWrenchSpace/ellpsoidTrans.txt");
cout << "Transformation for the wrenches:" << endl;
cout << "Hand to Ellipse:" << endl;
cout << hand_to_ellipse.hom << endl;
cout << "SemiAxis Torque:" << endl;
cout << semiAxesTorque.transpose() << endl;
cout << "SemiAxis Force:" << endl;
cout << semiAxesForces.transpose() << endl;
cout << "Computing primitive wrenches from cones contacts " << endl;
WrenchConesAll wrenchCones(grasp);
wrenchCones.setMaxForce(maxForceHand);
wrenchCones.computeAllWrenchCones(8);
wrenchCones.addZeroToWrenches(); //adding zero force grasp
vector<double> allWrenches=wrenchCones.getAllWrenches();
cout << "Nr wrenches " << wrenchCones.getNrWrenches() << endl;
//for (int i=0; i < allWrenches.size(); i++){
// cout << allWrenches[i] << " " << endl;
//}
double *wrenchArray = allWrenches.data();
SharedDoublePtr wrenchPtr(wrenchArray);
cout << "Transforming wrenches with transformations!" << endl;
std::vector<Vector6d> wrenchVec=wrenchCones.get6dEigen();
cout << "Wrenched Eigen 6D:" << endl;
//for(int i=0; i < wrenchVec.size(); i++){
// cout << wrenchVec.at(i).transpose() << endl;
//}
twsGraspQuality gwsTransform;
gwsTransform.setParams(wrenchVec,
hand_to_ellipse,
semiAxesTorque,
semiAxesForces,
forceOffset);
gwsTransform.computeTransform();
SharedDoublePtr wrenchPtrTrans;
uint nrWrenches;
gwsTransform.getOutput(wrenchPtrTrans,nrWrenches);
vector<double> wrenchTransformed=gwsTransform.getVector();
SharedDoublePtr transfWrench(wrenchTransformed.data());
nrWrenches=wrenchTransformed.size()/6;
vector<double> taskWrench;
taskWrench.push_back(0);
taskWrench.push_back(0);
taskWrench.push_back(0);
taskWrench.push_back(0);
taskWrench.push_back(0);
taskWrench.push_back(0);
cout << "Writing wrenches to file for visualization!" << endl;
writeWrenchesToFile(wrenchPtr,wrenchCones.getNrWrenches(),"/home/alexander/tmp/2dGraspWrenchSpace/wrenches_orig.txt");
writeWrenchesToFile(transfWrench,nrWrenches,"/home/alexander/tmp/2dGraspWrenchSpace/wrenches_trans.txt");
//cout << "Wrenches Size" <<
cout << "Computing convex Hull on original wrenches:" << endl;
DiscreteWrenchSpace discreteWrenchspace(6,wrenchPtr,wrenchCones.getNrWrenches());
discreteWrenchspace.computeConvexHull();
cout << discreteWrenchspace << endl;
SharedDoublePtr taskW(taskWrench.data());
//cout << "minDist: = " << discreteWrenchspace.computeDistToHull(taskW) << std::endl;
cout << "Computing convex Hull on transformed wrenches:" << endl;
DiscreteWrenchSpace discreteWrenchspaceTrans(6,transfWrench,nrWrenches);
discreteWrenchspaceTrans.computeConvexHull();
cout << discreteWrenchspaceTrans << endl;
// cout << "minDist: = " << discreteWrenchspaceTrans.computeDistToHull(taskW) << std::endl;
//compute second convex hull from 3d points of forces:
vector<double> allForces=wrenchCones.getAllForces();
double *forceArray = allForces.data();
SharedDoublePtr forcePtr(forceArray);
DiscreteWrenchSpace forceWrenchspace(3,forcePtr,wrenchCones.getNrWrenches());
forceWrenchspace.computeConvexHull();
cout << forceWrenchspace << endl;
forceWrenchspace.writeToOffFile("/home/alexander/tmp/offtest/forcespace.off"); //To compare to the Mathematica visualization!
vector<double> forceTrans=getForce(wrenchTransformed);
vector<double> torqueTrans=getTorque(wrenchTransformed);
SharedDoublePtr forceTransPtr(forceTrans.data());
SharedDoublePtr torqueTransPtr(torqueTrans.data());
uint forceNr=forceTrans.size()/3;
uint torqueNr=torqueTrans.size()/3;
//cout << "Force and Torque Getter:"<< endl;
//for(int i=0; i < forceTrans.size(); i++){
// cout << forceTrans.at(i) << " " << endl;
//}
//for(int i=0; i < torqueTrans.size(); i++){
// cout << torqueTrans.at(i) << " " << endl;
//}
DiscreteWrenchSpace forceTransWrenchspace(3,forceTransPtr,forceNr);
forceTransWrenchspace.computeConvexHull();
cout << forceTransWrenchspace << endl;
forceTransWrenchspace.writeToOffFile("/home/alexander/tmp/offtest/forcespace_trans.off"); //To compare to the Mathematica visualization!
DiscreteWrenchSpace torqueTransWrenchspace(3,torqueTransPtr,torqueNr);
torqueTransWrenchspace.computeConvexHull();
cout << torqueTransWrenchspace << endl;
torqueTransWrenchspace.writeToOffFile("/home/alexander/tmp/offtest/torque_trans.off"); //To compare to the Mathematica visualization!
cout << "Endresult Grasp Quality Measure:" << endl;
cout << discreteWrenchspaceTrans.getOcInsphereRadius() << endl;
/*
cout << "Testing Grasp:" << endl;
double torqueArm=grasp.getMaxTorqueArm();
cout << "Max Torque Arm: " << torqueArm << endl;
//compute CONEWS for contact1:
WrenchCone wrenchcone1;
wrenchcone1.setFriction(0.5);
wrenchcone1.setNrFactes(16);
wrenchcone1.setContact(contact1);
wrenchcone1.setTorqueArm(torqueArm);
wrenchcone1.computePrimitiveWrenches();
std::vector<Vector6d> primWrenches = wrenchcone1.getPrimitiveWrenches();
cout << "Computed Primitive Wrenches:" << endl;
for(int i = 0; i < primWrenches.size(); i++)
{
cout << primWrenches[i].transpose() << endl;
}
//computeAll
WrenchConesAll wrenchCones(grasp);
wrenchCones.computeAllWrenchCones(8);
vector<double> allWrenches=wrenchCones.getAllWrenches();
cout << "Nr wrenches " << wrenchCones.getNrWrenches() << endl;
for (int i=0; i < allWrenches.size(); i++){
cout << allWrenches[i] << " " << endl;
}
double *wrenchArray = allWrenches.data();
SharedDoublePtr wrenchPtr(wrenchArray);
DiscreteWrenchSpace discreteWrenchspace(6,wrenchPtr,wrenchCones.getNrWrenches());
discreteWrenchspace.computeConvexHull();
vector<double> taskWrench;
taskWrench.push_back(0);
taskWrench.push_back(0);
taskWrench.push_back(0);
taskWrench.push_back(0);
taskWrench.push_back(0);
taskWrench.push_back(0);
SharedDoublePtr taskW(taskWrench.data());
cout << "minDist: = " << discreteWrenchspace.computeDistToHull(taskW) << std::endl;
//compute second convex hull from 3d points of forces:
vector<double> allForces=wrenchCones.getAllForces();
cout << "Nr forces " << allForces.size()/3 << endl;
for (int i=0; i < allForces.size(); i++){
cout << allForces[i] << " " << endl;
}
double *forceArray = allForces.data();
SharedDoublePtr forcePtr(forceArray);
DiscreteWrenchSpace forceWrenchspace(3,forcePtr,wrenchCones.getNrWrenches());
forceWrenchspace.computeConvexHull();
cout << discreteWrenchspace << endl;
cout << forceWrenchspace << endl;
vector<double> taskForce;
taskForce.push_back(0);
taskForce.push_back(0);
taskForce.push_back(0);
SharedDoublePtr taskF(taskForce.data());
cout << "minDist: = " << forceWrenchspace.computeDistToHull(taskF) << std::endl;
forceWrenchspace.writeToOffFile("/home/alexander/tmp/offtest/forcespace.off");
///visualizing pure torquespace:
vector<double> allTorques=wrenchCones.getAllTorques();
SharedDoublePtr torquePtr(allTorques.data());
DiscreteWrenchSpace torqueWrenchspace(3,torquePtr,wrenchCones.getNrWrenches());
torqueWrenchspace.computeConvexHull();
cout << torqueWrenchspace << endl;
torqueWrenchspace.writeToOffFile("/home/alexander/tmp/offtest/torquespace.off");
*/
return 0;
}
bool RosDatabaseManager::GetGrasps(const Model &model, const string &hand_name,
vector<Grasp *> *grasp_list) const
{
std::vector< boost::shared_ptr<household_objects_database::DatabaseGrasp> > db_grasp_list;
if (!database_->getGrasps(model.ModelId(), hand_name, db_grasp_list)) { return false; }
std::cerr << "Loading grasps for hand " << hand_name << " on model " << model.ModelId() << "\n";
for (size_t i = 0; i < db_grasp_list.size(); i++)
{
Grasp *grasp = grasp_allocator_->Get();
const household_objects_database::DatabaseGrasp &db_grasp = *(db_grasp_list[i]);
grasp->SetSourceModel(model);
grasp->SetHandName(hand_name);
grasp->SetGraspId(db_grasp.id_.get());
grasp->SetEnergy(db_grasp.quality_.get());
grasp->SetEpsilonQuality(0.0);
grasp->SetVolumeQuality(0.0);
grasp->SetClearance(db_grasp.pre_grasp_clearance_.get());
grasp->SetClusterRep(db_grasp.cluster_rep_.get());
grasp->SetHandName(db_grasp.hand_name_.get());
grasp->SetTableClearance(db_grasp.table_clearance_.get());
grasp->SetCompliantCopy(db_grasp.compliant_copy_.get());
grasp->SetCompliantOriginalId(db_grasp.compliant_original_id_.get());
std::stringstream str;
std::vector<double> pregrasp_joints;
str << db_grasp.pre_grasp_posture_.get();
str >> pregrasp_joints;
if (str.fail()) { return false; }
std::vector<double> pregrasp_position;
str << db_grasp.pre_grasp_pose_.get();
str >> pregrasp_position;
if (str.fail()) { return false; }
std::vector<double> grasp_joints;
str << db_grasp.final_grasp_posture_.get();
str >> grasp_joints;
if (str.fail()) { return false; }
std::vector<double> grasp_position;
str << db_grasp.final_grasp_pose_.get();
str >> grasp_position;
if (str.fail()) { return false; }
//convert from ROS units to Graspit units
pregrasp_position[0] *= 1000;
pregrasp_position[1] *= 1000;
pregrasp_position[2] *= 1000;
grasp_position[0] *= 1000;
grasp_position[1] *= 1000;
grasp_position[2] *= 1000;
grasp->SetGraspParameters(pregrasp_joints,
pregrasp_position,
grasp_joints,
grasp_position);
grasp->SetPregraspJoints(pregrasp_joints);
grasp->SetPregraspPosition(pregrasp_position);
grasp->SetFinalgraspJoints(grasp_joints);
grasp->SetFinalgraspPosition(grasp_position);
grasp_list->push_back(grasp);
}
std::cerr << "Loaded " << grasp_list->size() << " grasps\n";
return true;
}
