void GraspPlannerEvaluatorWindow::pertubateStepObject()
{
Eigen::Vector3f rotPertub;
rotPertub.setRandom(3).normalize();
Eigen::Vector3f translPertub;
translPertub.setRandom(3).normalize();
Eigen::Matrix4f deltaPose;
deltaPose.setIdentity();
translPertub(0) = UI.doubleSpinBoxPertDistanceX->value();
translPertub(1) = UI.doubleSpinBoxPertDistanceY->value();
translPertub(2) = UI.doubleSpinBoxPertDistanceZ->value();
// rotPertub(0) = UI.doubleSpinBoxPertAngleX->value();
// rotPertub(1) = UI.doubleSpinBoxPertAngleY->value();
/* rotPertub(2) = UI.doubleSpinBoxPertAngleZ->value(); */
//deltaPose.block(0,0,3,3) = rodriguesFormula(rotPertub, UI.doubleSpinBoxPertAngle->value());
// deltaPose.block(0,0,3,3) = rotPertub;
deltaPose.block(0,3,3,1) = translPertub;
std::cout << "DeltaPose:\n" << deltaPose << std::endl;
std::cout << "Pose:\n" << object->getGlobalPose() << std::endl;
std::cout << "FinalPose:\n" << (deltaPose*object->getGlobalPose()) << std::endl;
object->setGlobalPose(deltaPose*object->getGlobalPose());
}
void GraspPlannerEvaluatorWindow::perturbatedGrasp()
{
openEEF();
resetPose();
graspNumber = UI.spinBoxGraspNum->value();
if (graspNumber <= 0 || grasps->getSize() < graspNumber) {
graspNumber = grasps->getSize();
UI.spinBoxGraspNum->setValue(graspNumber);
std::cout << "Invalid grasp number selection! Settting to last planned grasp." << std::endl;
}
// set to last valid grasp
if (graspNumber > 0 && eefCloned && eefCloned->getEndEffector(eefName))
{
Eigen::Matrix4f mGrasp = grasps->getGrasp(graspNumber - 1)->getTcpPoseGlobal(object->getGlobalPose());
eefCloned->setGlobalPoseForRobotNode(eefCloned->getEndEffector(eefName)->getTcp(), mGrasp);
if (!UI.doubleSpinBoxStep->value() && !UI.doubleSpinBoxVDelay->value() ){
perturbateObject();
//Grasp Center Point Robot Node
VirtualRobot::RobotNodePtr graspNode = eefCloned->getEndEffector(eefName)->getGCP();
//Grasp Pose
Eigen::Matrix4f pose = graspNode->getGlobalPose();
//Approach Direction (-z vector)
Eigen::Vector3f approachDir = -pose.block(0,0,3,1);
//Move eff away
moveEEFAway(approachDir, 3.0f);
closeEEF();
}
else
{
Eigen::Vector3f rotPertub;
rotPertub.setRandom(3).normalize();
Eigen::Vector3f translPertub;
translPertub.setRandom(3).normalize();
Eigen::Matrix4f deltaPose;
deltaPose.setIdentity();
std::cout<<object->getGlobalPose()<<std::endl;
float step=UI.doubleSpinBoxStep->value();
float delay =UI.doubleSpinBoxVDelay->value(); //Delay in secs
//float min_error=0;
//check id step is valid
//
Eigen::Matrix4f temp;
temp=object->getGlobalPose();
VirtualRobot::RobotNodePtr graspNode = eefCloned->getEndEffector(eefName)->getGCP();
for (step;step<UI.doubleSpinBoxPertDistanceX->value();step++){
translPertub(0) = step;
translPertub(1) = 0;
translPertub(2) = 0;
//std::cout<<step<<std::endl;
//std::cout<<UI.doubleSpinBoxPertDistanceX->value()<<std::endl;
deltaPose.block(0,3,3,1) = translPertub;
/* std::cout<< "DeltaPose:\n" << deltaPose << std::endl; */
// std::cout << "Pose:\n" << object->getGlobalPose() << std::endl;
/* std::cout << "FinalPose:\n" << (deltaPose*temp) << std::endl; */
object->setGlobalPose(deltaPose*temp);
// VirtualRobot::RobotNodePtr graspNode = eefCloned->getEndEffector(eefName)->getGCP();
//Grasp Pose
//Eigen::Matrix4f pose = graspNode->getGlobalPose();
//Approach Direction (-z vector)
//Eigen::Vector3f approachDir = -pose.block(0,0,3,1);
//Move eff away
//moveEEFAway(approachDir, 3.0f);
closeEEF();
contacts.clear();
contacts = eefCloned->getEndEffector(eefName)->closeActors(object);
qualityMeasure->setContactPoints(contacts);
float qual = qualityMeasure->getGraspQuality();
bool isFC = qualityMeasure->isGraspForceClosure();
//if (qual<=UI.doubleSpinBoxError->value()) break;
std::cout << "Grasp Nr " << graspNumber << "\nQuality (wrench space): " << qual << "\nForce closure: ";
if (isFC){
std::cout << "yes"<<std::endl;
}
else{
std::cout << "no"<<std::endl;
}
viewer->render();
sleep(delay/1000);
}
}
}
}
