void Robot::print(std::ostream& st) const
{
st << "robot_" << getName() << "(" << getOrientation().getName() << "," << getLife() << "," << getColor() << ")";
if (getHand() != nullptr)
st << "[" << *(getHand()) << "]";
}
void Robot::save(std::ostream& st) const
{
st << "robot_" << getName() << "_" << getOrientation().getName() << "_" << getLife() << "_" << getColor();
if (getHand() != nullptr) {
st << "_";
getHand()->save(st);
}
}
/*!
Start running the task
First gets the hand and object, then creates a starting position for the planner
Then it generates a loop planner and begins planning.
*/
void GraspPlanningTask::start()
{
//sets mHand
getHand();
if(!mHand)
{
DBGA("Failed to start planner");
return;
}
//sets mObject
getObject();
//initialize the planner
GraspPlanningState seed(mHand);
seed.setObject(mObject);
seed.setPositionType(SPACE_AXIS_ANGLE);
seed.setPostureType(POSE_EIGEN);
seed.setRefTran(mObject->getTran());
seed.reset();
mPlanner = new LoopPlanner(mHand);
mPlanner->setEnergyType(ENERGY_STRICT_AUTOGRASP);
mPlanner->setContactType(CONTACT_PRESET);
// mPlanner->setMaxSteps(65000);
mPlanner->setRepeat(true);
//max time set from database record
if (mRecord.taskTime >= 0){
mPlanner->setMaxTime(mRecord.taskTime);
} else {
mPlanner->setMaxTime(-1);
}
static_cast<SimAnnPlanner*>(mPlanner)->setModelState(&seed);
startPlanner();
}
void Robot::loseHP(int nbHP) {
assert(nbHP > 0);
int newLife = getLife() - nbHP;
newLife = newLife < 0 ? 0:newLife;
m_life = newLife;
// Si un robot perd des PVs alors qu'il porte un rocher et que ce rocher devient plus lourd que lui, il est écrasé et meurt sur le coup.
if (getHand() != nullptr) {
if (std::shared_ptr<Rock> rock = std::dynamic_pointer_cast<Rock>(getHand())) {
if (rock->getWeight() > m_life) {
destroy();
return;
}
}
}
// Plus de vie
if (m_life == 0) {
destroy();
}
}
void DialogCt::grabAndShow(){
yuv2Mat(buffers[0].start,imgWidth,imgHeight);
//new frame grabed,process start
if(mode == 2){//ct mode
while(1){
if(first_flag){
cvtColor(frame, first_frame, CV_RGB2GRAY);
first_flag=0;
}
cvtColor(frame, current_gray, CV_RGB2GRAY);
absdiff(first_frame,current_gray,fore_frame);
if(!gotHand){
getHand();
if(gotHand){
ctInitFlag=1;
}
break;
}
// CT initialization
if(ctInitFlag){
for(int i=0;i<HANDNUM;i++)
ct[i].init(fore_frame, box[i]);
ctInitFlag=0;
}
for(int i=0;i<HANDNUM;i++){
ct[i].processFrame(fore_frame, box[i]);
rectangle(frame, box[i], Scalar(rgb_b[i],rgb_g[i],rgb_r[i]));
}
flip(frame, frame, 1);
break;
}
} else if (mode == 1){//only dispaly mode
flip(frame, frame, 1);
}
qImg=MatToQImage(frame);
ui->label->setPixmap(QPixmap::fromImage(qImg));
ui->label->resize(ui->label->pixmap()->size());
}
void ArmContourFinder::updateHands() {
vector < Hand > newHands;
newHands.clear();
for (int i = 0; i < size(); ++i)
{
unsigned int l = getLabel(i);
if(handFound[l]) {
Hand blob;
blob.label = l;
blob.side = side[l];
blob.line = getHand(i);
blob.centroid = blob.line.getCentroid2D();
blob.tip = tips[l];
blob.wrists = wrists[l];
blob.end = ends[l];
blob.boxCenter = ofxCv::toOf(getCenter(i));
blob.index = i;
newHands.push_back(blob);
}
}
sort(newHands.begin(), newHands.end());
// Remove dead ones
for (int i = 0; i < hands.size(); ++i)
{
bool found = false;
unsigned int label = hands[i].label;
for (int j = 0; j < newHands.size(); ++j)
{
if(newHands[j].label == label) {
hands[i].index = newHands[j].index;
found = true;
break;
}
}
if(!found) {
hands.erase( hands.begin() + i );
i--; //So that it doesn't skip the next one
}
}
//Add new ones
for (int i = 0; i < newHands.size(); ++i)
{
bool found = false;
unsigned int label = newHands[i].label;
for (int j = 0; j < hands.size(); ++j)
{
if(hands[j].label == label) {
hands[j].index = newHands[i].index;
found = true;
break;
}
}
if(!found) {
hands.push_back(newHands[i]);
}
}
sort(hands.begin(), hands.end());
//Finally, the magic
for (int i = 0; i < hands.size(); ++i)
{
Hand handCopy = newHands[i];
// Doesn't copy vectors, do it by 'hand' for now
handCopy.wrists = newHands[i].wrists;
handCopy.velocity = newHands[i].velocity;
ofPoint oldKeypoints[] = {hands[i].centroid, hands[i].end, hands[i].tip, hands[i].wrists[0], hands[i].wrists[1]};
ofPoint * keypoints[] = {&handCopy.centroid, &handCopy.end, &handCopy.tip, &handCopy.wrists[0], &handCopy.wrists[1]};
if( !(newHands[i].centroid.x == 0 and newHands[i].centroid.y == 0) )
{
for (int j = 0; j < 5; ++j)
{
float smoothedX = ofLerp(keypoints[j]->x, oldKeypoints[j].x, smoothingRate);
float smoothedY = ofLerp(keypoints[j]->y, oldKeypoints[j].y, smoothingRate);
*keypoints[j] = ofPoint(smoothedX, smoothedY);
}
// handCopy.velocity = ofVec2f((keypoints[0]->x - oldKeypoints[0].x)/2, (keypoints[0]->y - oldKeypoints[0].y)/2);
}
// Lerped veloctiy
vector< ofPoint > prevs = hands[i].previousPositions;
prevs.insert(prevs.begin(),handCopy.centroid);
ofPoint oldAverage = ofPoint(0,0);
ofPoint presentAverage = ofPoint(0,0);
for (int j = 0; j < prevs.size()/2; ++j)
{
oldAverage.x += prevs[j + prevs.size()/2].x;
presentAverage.x += prevs[j].x;
oldAverage.y += prevs[j + prevs.size()/2].y;
presentAverage.y += prevs[j].y;
}
oldAverage.x /= prevs.size()/2;
presentAverage.x /= prevs.size()/2;
oldAverage.y /= prevs.size()/2;
presentAverage.y /= prevs.size()/2;
handCopy.velocity = ofVec2f((presentAverage.x - oldAverage.x), (presentAverage.y - oldAverage.y));
if(prevs.size() > 12) prevs.resize(12);
handCopy.previousPositions = prevs;
hands[i] = handCopy;
}
}
