CLTool* CLToolRegister::create(const CLToolOptions&ao){
if(ao.line.size()<1)return NULL;
CLTool* cltool;
if(check(ao.line[0])){
CLToolOptions nao( ao,mk[ao.line[0]] );
cltool=m[ao.line[0]](nao);
} else cltool=NULL;
return cltool;
}
Action* ActionRegister::create(const ActionOptions&ao){
if(ao.line.size()<1)return NULL;
// Create a copy of the manual locally. The manual is
// then added to the ActionOptions. This allows us to
// ensure during construction that all the keywords for
// the action have been documented. In addition, we can
// generate the documentation when the user makes an error
// in the input.
Action* action;
if( check(ao.line[0]) ){
Keywords keys; mk[ao.line[0]](keys);
ActionOptions nao( ao,keys );
action=m[ao.line[0]](nao);
keys.destroyData();
} else action=NULL;
return action;
}
void Locomotion:: walkToObject(Object& object,
SpaceOrientation& spaceOrientation)
{
std::cout << "1Object position: " << object.getPositionInRobotFrame() << std::endl;
// std::cout << "1NAO position: " << spaceOrientation.getNaoPositionInRobotFrame() << std::endl;
// turnToObject(object, spaceOrientation);
// float distance = spaceOrientation.computeDistance(object);
// std::cout << "computed distance" << distance << std::endl;
cv::Point2d obj = object.getPositionInRobotFrame();
obj.x += LATERAL_DISTANCE_CORRECTION_FOR_HAND_GRASPING;
obj.y += FORWARD_DISTANCE_CORRECTION_FOR_HAND_GRASPING;
std::vector<float> naoInSpace =
spaceOrientation.getNaoPositionInRobotFrame();
cv::Point2d nao(naoInSpace[1], naoInSpace[0]);
cv::Point2d distance = obj - nao;
move(distance.y, distance.x, 0.0);
#ifdef __linux__
try
{
AL::ALRobotPostureProxy robotPosture(robotIp);
AL::ALMotionProxy motion(robotIp);
bool postureReached = robotPosture.goToPosture("Stand", 0.5f);
motion.waitUntilMoveIsFinished();
if (! postureReached) speech->say("Could not stand");
std::cout << "NAO position after walk: " <<
spaceOrientation.getNaoPositionInRobotFrame() << std::endl;
}
catch (const AL::ALError& e)
{
std::cerr << "Caught exception: " << e.what() << std::endl;
exit(1);
}
#else
std::cout << "walkToObject run not on linux" << std::endl;
#endif
}
float Locomotion:: turnToObject(Object& object,
SpaceOrientation& spaceOrientation)
{
static cv::Point2d previousDirection(0,1);
std::vector<float> naoInSpace =
spaceOrientation.getNaoPositionInRobotFrame();
// static float previousAngleRad = 0;
// float x = naoInSpace[1];
// float y = naoInSpace[0];
// float theta = naoInSpace[2];
// // std::cout << "theta: " << theta << std::endl;
//
// float anotherX;
// float anotherY;
//
// // if (fabs(theta) < 0.01)
// // {
// // anotherX = x;
// // anotherY = y+1;
// // }
// // else
// // {
// // if ((theta >= 0.1) && (theta <= (M_PI/2 - 0.1)))
// // theta = M_PI/2 - theta;
// // if ( (theta <= 0.1) && (theta >= (-M_PI/2 + 0.1)) )
// // theta = -M_PI/2 - theta;
//
// // float slope = tan(theta);
// // std::cout << "slope: " << slope << std::endl;
// // float n = y - slope*x;
// // if (slope > 0) anotherX += 0.2;
// // else anotherX -= 0.2;
// // anotherY = slope*anotherX + n;
// // }
//
// // else
// // {
// // float a = (slope*slope + 1);
// // float b = (-2*x - 2*(y - n)*slope);
// // float c = x*x - 1 + (y - n)*(y - n);
// // float delta = b*b - 4*a*c;
// // if (delta < 0)
// // {
// // std::cout << "delta < 0" << std::endl;
// // exit(EXIT_FAILURE);
// // }
// // if (slope < 0) anotherX = (-b + sqrt(delta)) / (2*a);
// // else anotherX = (-b - sqrt(delta)) / (2*a);
// // anotherY = n + slope*anotherX;
// // }
cv::Point2d one = previousDirection*2;//(anotherX, anotherY);
// one = head->computePointInRobotFrameGivenPointOnImage(one);
cv::Point2d nao(naoInSpace[1], naoInSpace[0]);
cv::Point2d two = object.getPositionInRobotFrame();
// To correct for grasping, let's update the position where we need to go:
// two.x += LATERAL_DISTANCE_CORRECTION_FOR_HAND_GRASPING;
// two.y += FORWARD_DISTANCE_CORRECTION_FOR_HAND_GRASPING;
// std::cout << two << std::endl;
// What would happen when two.y gets negative? test it.
// two = head->pointFromWorldFrameToRobotFrame(two);
cv::Point2d direction = two - nao;
// cv::normalize(previousDirection);
// cv::norm(previousDirection);
// cv::normalize(direction);
float angle = acos(previousDirection.dot(direction) /
( norm(previousDirection) * norm(direction) ));
//nao.dot(two));
previousDirection = direction;
std::cout << angle << std::endl;
//!!! set the correct sign to the angle.
// two = head->computePointOnImageGivenPointInRobotFrame(two);
//head->computePointInRobotFrameGivenPointOnImage(
// object.getImage().getCenter());
// two.x += LATERAL_DISTANCE_CORRECTION_FOR_HAND_GRASPING;
// two.y += FORWARD_DISTANCE_CORRECTION_FOR_HAND_GRASPING;
std::cout << "Points: "<<one << std::endl << nao << std::endl <<
two << std::endl;
// cv::Point2d middle(1280/2, 960);
// cv::Point2d oneOnImage =
// head->computePointOnImageGivenPointInRobotFrame(one);
float angleRad = angle;//head->angleFromPoints(one, nao, two);
std::cout << angleRad*180/M_PI << std::endl;
{
float x1 = nao.x, x2 = two.x, y1 = nao.y, y2 = two.y;
float a = x2 - x1;
float b = y2 - y1;
if (a)
{
float slope2 = b/a;
float oneOnLine = slope2*one.x - slope2*x1 + y1;
if ((slope2 < 0) && (oneOnLine < one.y)) angleRad = -angleRad;
if ((slope2 > 0 ) && (oneOnLine > one.y)) angleRad = -angleRad;
}
}
// float twoXOnLine = slope*two.x + n;
// if (twoXOnLine < two.y) angleRad = -angleRad;
// if (nao.x > two.x) angleRad = -angleRad;
// else if ((nao.x == one.x) && (nao.x > two.x)) angleRad =
// -angleRad;
// angleRad = -angleRad;
// std::cout << "Previous angle: " << naoInSpace[2] << std::endl;
// angleRad += -naoInSpace[2];
// std::cout << "Previous angle to turn in degrees: " <<
// previousAngleRad*180/M_PI << std::endl;
std::cout << "Angle to turn in degrees: " << angleRad*180/M_PI << std::endl;
// navigationProxy.setSecurityDistance(.1);
// float degrees = 20.0f;
// float rads = degrees * 3.1415 / 180;
// for (int i = 0; i < 10; i++)
#ifdef __linux__
try
{
// lookDown(robotIp);
AL::ALNavigationProxy navigationProxy(robotIp,9559);
AL::ALMotionProxy motion(robotIp);
bool positionReached;// = navigationProxy.moveTo(0,0,-previousAngleRad);
// if (! positionReached) say("Could not turn to the previous
// angle");
motion.moveInit();
positionReached = navigationProxy.moveTo(0, 0.0, angleRad);//,
// AL::ALValue::array("MaxStepX", 0.06));
motion.waitUntilMoveIsFinished();
if (! positionReached) speech->say("Could not turn to the object");
else std::cout << "Turned." << std::endl;
// previousAngleRad = angleRad;
return angleRad;
}
catch (const AL::ALError& e)
{
std::cerr << "Caught exception: " << e.what() << std::endl;
exit(EXIT_FAILURE);
}
#else
std::cout << "turnToObject not on Linux" << std::endl;
return angleRad;
#endif
}
