Skip to content
Snippets Groups Projects
Commit dc8d969e authored by Torben Hildebrand's avatar Torben Hildebrand
Browse files

woking on histogram table masking

parent e1468a92
No related branches found
No related tags found
1 merge request!55Feature/tof grasping live
Hide whitespace changes
Inline Side-by-side
source/armarx/manipulation/skills/ToFBasedGrasping.cpp
+ 55
− 16
View file @ dc8d969e
......@@ -2,6 +2,7 @@
// Created by hyseni on 08.07.24.
//
#include <cmath>
#include <fstream>
#include <armarx/control/skills/aron/ShapeHandParams.aron.generated.h>
#include <armarx/control/skills/aron/MoveJointsToPositionParams.aron.generated.h>
......@@ -37,7 +38,7 @@ namespace armarx::manipulation::skills{
{
ParamType defaultParams;
defaultParams.hand = core::arondto::Hand::Left;
defaultParams.graspName = "Side Grasp 1";
defaultParams.graspName = "Side_Grasp_1";
toAron(defaultParams.object, armarx::ObjectID{"Maintenance", "spraybottle", ""});
return armarx::skills::SkillDescription{
.skillId = armarx::skills::SkillID{.skillName = constants::ToFBasedGrasping},
......@@ -94,12 +95,15 @@ namespace armarx::manipulation::skills{
ARMARX_CHECK(ctx.robot.synchronize(Clock::Now()));
ARMARX_INFO << "initialize";
initialize(in.parameters);
auto lpose = ctx.arviz.layer("ObjectPose");
auto lpose = ctx.arviz.layer("InitialObjectPose");
tof_based_grasping::ToFObjectPoseOptimization poseOptimization = tof_based_grasping::ToFObjectPoseOptimization(ctx,param);
poseOptimization.loadObject(objectId);
objpose::ObjectPose objectPose = poseOptimization.getObjectPose(objectId);
lpose.add(armarx::viz::Sphere("ObjectPose").pose(objectPose.objectPoseGlobal).scale(5).color(simox::Color::kit_red()));
//lpose.add(armarx::viz::Sphere("ObjectPose").pose(objectPose.objectPoseGlobal).scale(5).color(simox::Color::kit_red()));
auto filestring = "/home/armar-user/code/h2t/PriorKnowledgeData/data/PriorKnowledgeData/objects/" + objectPose.objectID.str() + "/" +objectPose.objectID.className() + "/" + objectPose.objectID.className() + ".xml";
lpose.add(armarx::viz::Object("InitialObjectPose").file(filestring).pose(objectPose.objectPoseGlobal));
ctx.arviz.commit(lpose);
ARMARX_INFO << VAROUT(objectPose.objectID);
ARMARX_INFO << VAROUT(objectPose.objectPoseGlobal);
......@@ -107,6 +111,7 @@ namespace armarx::manipulation::skills{
std::shared_ptr<VirtualRobot::GraspSet> graspSet;
auto graspSets=manipulationObject->getAllGraspSets();
std::string joint;
std::string handSide;
for(const auto& set:graspSets)
{
ARMARX_INFO<<VAROUT(set->getName());
......@@ -116,12 +121,14 @@ namespace armarx::manipulation::skills{
if(param.hand == Hand::Left)
{
ARMARX_INFO << "Left Hand";
handSide<<"Left_Hand";
ARMARX_INFO << handSide;
graspSet = manipulationObject->getGraspSet("ArmarDE","Hand_L_EEF");
joint="ArmL6_Elb2";
}else if(param.hand == Hand::Right)
{
ARMARX_INFO << "Right Hand";
handSide<<"Right_Hand";
ARMARX_INFO << handSide;
graspSet = manipulationObject->getGraspSet("ArmarDE","Hand_R_EEF");
joint="ArmR6_Elb2";
}
......@@ -150,40 +157,72 @@ namespace armarx::manipulation::skills{
auto shapeHandParams = shapeHandSkill.getRootProfileParameters();
shapeHandParams.hand=in.parameters.hand;
shapeHandParams.jointAngles=jointValues;
this->callSubskill(shapeHandSkill,shapeHandParams.toAron());
ARMARX_INFO<<"Rotate Thumb";
std::string filename;
filename<<in.parameters.object.dataset<<"_"<<in.parameters.object.className<<"_"<<handSide<<"_"<<graspName<<"_"<<armarx::Clock::Now()<<".csv";
std::string absoluteFilename;
absoluteFilename<< "/home/armar-user/code/armarx/skills/manipulation/data/armarx_manipulation/tof_losses"<<"/"<<filename;
std::ofstream myfile;
myfile.open(absoluteFilename);
Metronome metronome(Frequency::Hertz(15));
Eigen::Matrix4f graspPose= grasp->getTcpPoseRobotRoot(objectPose.objectPoseGlobal,ctx.robot.robot());
Eigen::Matrix4f graspPoseGlobal= grasp->getTcpPoseGlobal(objectPose.objectPoseGlobal);
auto l = ctx.arviz.layer("target");
l.add(armarx::viz::Sphere("targetPoseLocal").pose(graspPose).scale(5).color(simox::Color::kit_blue()));
l.add(armarx::viz::Sphere("targetPose").pose(graspPoseGlobal).scale(5).color(simox::Color::kit_brown()));
auto l = ctx.arviz.layer("targetPoseCtrl");
//l.add(armarx::viz::Sphere("targetPoseLocal").pose(graspPose).scale(5).color(simox::Color::kit_blue()));
l.add(armarx::viz::Sphere("targetPose").pose(graspPoseGlobal).scale(4).color(simox::Color::kit_brown()));
ctx.arviz.commit(l);
ARMARX_INFO << VAROUT(graspPose);
ARMARX_INFO << "setting target";
positionController->thresholdPositionReached = 5; // [mm]
positionController->thresholdOrientationReached = VirtualRobot::MathTools::deg2rad(3);
if (graspName.find("Top")!=std::string::npos){
ARMARX_INFO<<"Doing Top Grasp";
Eigen::Matrix4f graspPoseTop=graspPose;
graspPoseTop.block(0,3,3,1)=graspPose.block(0,3,3,1)+Eigen::Vector3f::UnitZ()*150;
ARMARX_INFO<<VAROUT(graspPoseTop);
ARMARX_INFO << "Waiting until target is reached";
positionController->setTarget(graspPoseTop);
positionController->update();
// run controller
while (not positionController->isFinalTargetReached())
{
ARMARX_CHECK(ctx.robot.synchronize(Clock::Now()));
positionController->setTarget(graspPoseTop);
positionController->update();
metronome.waitForNextTick();
}
ARMARX_INFO << "Target is reached";
}else{
ARMARX_INFO<<"Doing Side Grasp";
}
positionController->setTarget(graspPose);
positionController->update();
ARMARX_INFO << VAROUT(positionController->getCurrentTargetPosition());
ARMARX_INFO << VAROUT(positionController->getCurrentTarget());
// parameterize position controller (thresholds)
positionController->thresholdPositionReached = 5; // [mm]
positionController->thresholdOrientationReached = VirtualRobot::MathTools::deg2rad(3);
//positionController->thresholdPositionReached = 5; // [mm]
//positionController->thresholdOrientationReached = VirtualRobot::MathTools::deg2rad(3);
ARMARX_INFO << "Waiting until target is reached";
Metronome metronome(Frequency::Hertz(15));
// run controller
while (not positionController->isFinalTargetReached())
{
ARMARX_CHECK(ctx.robot.synchronize(Clock::Now()));
objectPose = poseOptimization.improveObjectPose(objectId);
myfile<<poseOptimization.getLoss()<<"\n";
ARMARX_INFO << VAROUT(objectPose.objectPoseGlobal);
graspPose= grasp->getTcpPoseRobotRoot(objectPose.objectPoseGlobal,ctx.robot.robot());
graspPoseGlobal = grasp->getTcpPoseGlobal(objectPose.objectPoseGlobal);
ARMARX_INFO << VAROUT(graspPose);
l.add(armarx::viz::Sphere("targetPoseLocal").pose(graspPose).scale(5).color(simox::Color::kit_blue()));
l.add(armarx::viz::Sphere("targetPoseGlobal").pose(graspPoseGlobal).scale(5).color(simox::Color::kit_yellow()));
//auto filestring = "/home/armar-user/code/h2t/PriorKnowledgeData/data/PriorKnowledgeData/objects/" + objectPose.objectID.str() + "/" +objectPose.objectID.className() + "/" + objectPose.objectID.className() + ".xml";
//l.add(armarx::viz::Object("UpdatedObjectPose").file(filestring).pose(objectPose.objectPoseGlobal));
//l.add(armarx::viz::Sphere("targetPoseLocal").pose(graspPose).scale(3).color(simox::Color::kit_blue()));
l.add(armarx::viz::Sphere("targetPoseGlobal").pose(graspPoseGlobal).scale(4).color(simox::Color::kit_yellow()));
ctx.arviz.commit(l);
positionController->setTarget(graspPose);
positionController->update();
......@@ -193,7 +232,7 @@ namespace armarx::manipulation::skills{
ARMARX_INFO << "Goal reached";
stop();
myfile.close();
ARMARX_INFO << "Close Hand";
......
source/armarx/manipulation/tof_based_grasping/ToFObjectPoseOptimization.cpp
+ 81
− 37
View file @ dc8d969e
......@@ -42,6 +42,7 @@
#include <ArmarXCore/core/time/ClockType.h>
#include <ArmarXCore/core/time/StopWatch.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/variant/Variant.h>
namespace armarx::manipulation::tof_based_grasping
......@@ -58,12 +59,7 @@ namespace armarx::manipulation::tof_based_grasping
ARMARX_INFO<<"No Object Found!";
}
objpose::ObjectPose currPose = objectPoses_.at(index);
//ARMARX_INFO<<VAROUT(currPose.objectPoseGlobal);
tofRenderer_.createOrUpdateObjectSet(objectPoses_, currPose, index);
// auto tableIndex = findObject(ObjectID("Interior/hund-table-180x80/0"));
// objpose::ObjectPose tablePose = objectPoses_.at(index);
// maskRenderer_.createOrUpdateObjectSet(objectPoses_, tablePose, tableIndex);
}
objpose::ObjectPose ToFObjectPoseOptimization::improveObjectPose(ObjectID objectID) {
......@@ -75,15 +71,11 @@ namespace armarx::manipulation::tof_based_grasping
armarx::core::time::StopWatch sw3;
ARMARX_CHECK(services_.robot.synchronize());
ARMARX_INFO << "time1: " << sw3.stopAndReset();
float pixelSize = 0;
float distance = 0;
Eigen::Vector3f tcp_current_pos;
ARMARX_INFO << "time2: " << sw3.stopAndReset();
auto tofWrapper = ToF(services_.robot, params_.hand);
ARMARX_INFO << "time3: " << sw3.stopAndReset();
VirtualRobot::RobotNodePtr cameraNode = tofWrapper.getCameraNode();
ARMARX_INFO << "time4: " << sw3.stopAndReset();
ARMARX_INFO << "Initial paramter setting time: " << sw2.stopAndReset();
......@@ -97,14 +89,9 @@ namespace armarx::manipulation::tof_based_grasping
return nullPose;
}
objpose::ObjectPose currPose = objectPoses_.at(index);
//ARMARX_INFO<<VAROUT(currPose.objectPoseGlobal);
ARMARX_INFO << "Object detection time: " << sw2.stopAndReset();
tofRenderer_.createOrUpdateObjectSet(objectPoses_, currPose, index);
// auto tableIndex = findObject(ObjectID("Interior/hund-table-180x80/0"));
// objpose::ObjectPose tablePose = objectPoses_.at(index);
// maskRenderer_.createOrUpdateObjectSet(objectPoses_, tablePose, tableIndex);
if (params_.hand == Hand::Left){
......@@ -117,10 +104,11 @@ namespace armarx::manipulation::tof_based_grasping
auto estimVec = tcp_current_pos - objectPos;
auto estimDist = sqrt(estimVec.x()*estimVec.x() + estimVec.y()*estimVec.y() + estimVec.z()*estimVec.z());
ARMARX_INFO <<"Distance to target: "<< estimDist << " / time needed: " << sw2.stopAndReset();
if (estimDist > 200) {
if (estimDist > 300 && !started_) {
ARMARX_INFO<<"not close enough: "<<estimDist;
return objectPoses_.at(index);
}
started_ = true;
......@@ -142,14 +130,12 @@ namespace armarx::manipulation::tof_based_grasping
}
tofWrapper.calculateCoordinates(realToF, cameraNode);
auto coordinates = tofWrapper.getCoordinates();
masking(realToF, simToF, cameraNode, coordinates);
tofWrapper.drawToF(services_.arviz, realToF, cameraNode, "realToF", simox::Color::green(), Draw::Real);
//ARMARX_INFO<<VAROUT(realToF);
ARMARX_INFO << "ToF measurement time: " << sw2.stopAndReset();
//calculating inital loss value between real ToF and sim ToF
float lossFunction = calculateLoss(tofWrapper, realToF, simToF, distance, pixelSize);
if (lossFunction == -1) {
......@@ -204,24 +190,23 @@ namespace armarx::manipulation::tof_based_grasping
//Final decision making to compare both loss functions and decide, which objPose to keep.
void ToFObjectPoseOptimization::decisionMaking(float lossFunction, float newLossFunction, int index, objpose::ObjectPose& currPose) {
auto layer = services_.arviz.layer("objPos");
auto layer = services_.arviz.layer("UpdatingObjectPose");
layer.clear();
bestEval_ = lossFunction;
ARMARX_INFO << "fixing best pose eval:" << lossFunction;
ARMARX_INFO << VAROUT(lossFunction);
ARMARX_INFO << VAROUT(newLossFunction);
ARMARX_INFO << "old loss: " << bestLoss_;
ARMARX_INFO << "loss 1:" << VAROUT(lossFunction);
ARMARX_INFO << "loss 2:" << VAROUT(newLossFunction);
bestLoss_ = lossFunction;
ARMARX_INFO << "fixed best loss:" << bestLoss_;
if (bestEval_ < 5.0F && newLossFunction >= lossFunction) {
//if (newLossFunction >= lossFunction) {
ARMARX_INFO << "skipped, position is worsening";
if (newLossFunction > 1.1 * lossFunction ) {
ARMARX_INFO << "skipped, position is worsening too much";
}
// else if (newLossFunction <= lossFunction) {
else {
ARMARX_INFO << "updating best position...";
objectPoses_.at(index) = currPose;
bestEval_ = newLossFunction;
bestLoss_ = newLossFunction;
}
auto filestring = "/home/armar-user/code/h2t/PriorKnowledgeData/data/PriorKnowledgeData/objects/" + objectPoses_.at(index).objectID.str() + "/" +objectPoses_.at(index).objectID.className() + "/" + objectPoses_.at(index).objectID.className() + ".xml";
layer.add(armarx::viz::Object("movedObj").file(filestring).pose(objectPoses_.at(index).objectPoseGlobal));
......@@ -246,7 +231,7 @@ namespace armarx::manipulation::tof_based_grasping
auto simValue = simToF[i];
auto value = realValue - simValue;
if (realValue > tof.getMinValid() && realValue < tof.getMaxValid()) {
if (realValue < tof.getMaxValid()) {
distance += realValue;
realValid ++;
}
......@@ -353,6 +338,9 @@ namespace armarx::manipulation::tof_based_grasping
void ToFObjectPoseOptimization::masking(std::vector<float>& realToF, std::vector<float>& simToF, VirtualRobot::RobotNodePtr cameraNode, std::vector<Eigen::Vector3f> coordinates){
ARMARX_INFO << "masking...";
armarx::core::time::StopWatch sw;
// auto tofWrapper2 = ToF(services_.robot, params_.hand);
// auto mask = maskRenderer_.renderToFImage(cameraNode);
// ARMARX_INFO<<VAROUT(mask);
......@@ -386,20 +374,72 @@ namespace armarx::manipulation::tof_based_grasping
// }
// }
// ARMARX_INFO << VAROUT(realToF);
ARMARX_INFO << VAROUT(realToF.size());
ARMARX_INFO << VAROUT(coordinates.size());
std::map<int,int> histogram;
std::vector<float> copy;
for (Vector3 i : coordinates) {
if (i.z != -1) {
copy.push_back(i.z);
}
}
std::sort(copy.begin(), copy.end());
ARMARX_INFO << copy;
double bin = static_cast<int>(copy.at(0)) / 10 * 10; //Choose your starting bin
const double bin_width = 10;
int biggestGap = 2* bin_width;
int biggestLowerBinCount = 0;
int cutOffRange = 0;
int currentBinStart = bin; // Track the start of the current bin
int currentBinCount = 0; // Number of elements in the current bin
int previousBinStart = currentBinStart; // Track the previous bin start
int previousBinCount = 0;
for (const auto& e : copy)
{
if (e >= currentBinStart + bin_width) {
while (e >= currentBinStart + bin_width) {
currentBinStart += bin_width;
}
int gap = currentBinStart - previousBinStart;
if (gap > biggestGap || (gap >= biggestGap && previousBinCount > biggestLowerBinCount)) {
biggestGap = gap;
cutOffRange = currentBinStart + bin_width;
biggestLowerBinCount = previousBinCount;
}
previousBinStart = currentBinStart;
previousBinCount = currentBinCount;
currentBinStart += bin_width;
currentBinCount = 0;
}
++currentBinCount;
++histogram[currentBinStart];
}
ARMARX_INFO << VAROUT(biggestGap);
ARMARX_INFO << VAROUT(cutOffRange);
ARMARX_INFO << VAROUT(biggestLowerBinCount);
for (const auto& x : histogram)
{
ARMARX_INFO << x.first << "," << x.first + bin_width << ": " << x.second;
}
ARMARX_CHECK(realToF.size() == coordinates.size());
ARMARX_INFO << VAROUT(realToF);
ARMARX_INFO << VAROUT(coordinates);
for (size_t i = 0; i < realToF.size(); i++) {
if (coordinates.at(i).z() >= 0 && coordinates.at(i).z() <= 725) {
if (coordinates.at(i).z() < cutOffRange) {
realToF.at(i) = 4000;
}
}
ARMARX_INFO << VAROUT(realToF);
ARMARX_INFO << "masking time: " <<sw.stopAndReset();
}
......@@ -426,5 +466,9 @@ namespace armarx::manipulation::tof_based_grasping
int index = ToFObjectPoseOptimization::findObject(objectID);
return objectPoses_.at(index);
}
float ToFObjectPoseOptimization::getLoss(){
return bestLoss_;
}
} // namespace armarx::manipulation::tofbased_grasping
source/armarx/manipulation/tof_based_grasping/ToFObjectPoseOptimization.h
+ 3
− 1
View file @ dc8d969e
......@@ -84,15 +84,17 @@ namespace armarx::manipulation::tof_based_grasping
void reloadObjects();
int findObject(ObjectID objectID);
objpose::ObjectPose getObjectPose(ObjectID objectID);
float getLoss();
private:
ToFRenderer tofRenderer_;
ToFRenderer maskRenderer_;
Services services_;
Params params_;
bool started_ = false;
objpose::ObjectPoseSeq objectPoses_;
float bestEval_ = 4000;
float bestLoss_ = 4000;
};
} // namespace armarx::manipulation::tof_based_grasping
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment