int main(int argc, char **argv)
{
std::string robotIp = "127.0.0.1";
if (argc < 2) {
std::cerr << "Usage: almotion_wbseteffectorcontrol robotIp "
<< "(optional default \"127.0.0.1\")."<< std::endl;
}
else {
robotIp = argv[1];
}
AL::ALMotionProxy motion(robotIp);
AL::ALRobotPostureProxy robotPosture(robotIp);
robotPosture.goToPosture("StandInit", 0.5f);
bool isEnabled = true;
motion.wbEnable(isEnabled);
// Example showing how to set orientation target for Head tracking.
std::string effectorName = "Head";
AL::ALValue targetCoordinate = AL::ALValue::array(0.1f, 0.0f, 0.0f);
motion.wbSetEffectorControl(effectorName, targetCoordinate);
isEnabled = false;
motion.wbEnable(isEnabled);
return 0;
}
// Public methods:
void Locomotion:: setStiffnessOnAndStand()
{
#ifdef __linux__
const AL::ALValue jointName = AL::ALValue::array("Body");
try
{
AL::ALMotionProxy motion(robotIp);
AL::ALValue stiffness = 1.0f;
AL::ALValue time = 1.0f;
motion.stiffnessInterpolation(jointName, stiffness, time);
AL::ALRobotPostureProxy robotPosture(robotIp);
float maxSpeedFraction = 0.5f; //[0;1]
bool postureReached = robotPosture.goToPosture("Stand",
maxSpeedFraction);
if (! postureReached) speech->say("Could not Stand");
}
catch (const AL::ALError& e) {
std::cerr << "Caught exception: " << e.what() << std::endl;
exit(EXIT_FAILURE);
}
#else
std::cout << "setStiffnessOnAndStand not on linux" << std::endl;
#endif
}
int main(int argc, char **argv)
{
std::string robotIp = "127.0.0.1";
if (argc < 2) {
std::cerr << "Usage: almotion_changetransform robotIp "
<< "(optional default \"127.0.0.1\")."<< std::endl;
}
else {
robotIp = argv[1];
}
AL::ALMotionProxy motion(robotIp);
AL::ALRobotPostureProxy robotPosture(robotIp);
robotPosture.goToPosture("StandInit", 0.5f);
// Example showing how to set Torso Transform, using a fraction of max speed
std::string chainName = "Torso";
int space = 2; // FRAME_ROBOT
std::vector<float> transform(12, 0.0f);
transform[0] = 1.0f; // 1.0f, 0.0f, 0.0f, 0.05f
transform[3] = 0.05f; // 0.0f, 1.0f, 0.0f, 0.0f
transform[5] = 1.0f; // 0.0f, 0.0f, 1.0f, 0.0f
transform[10] = 1.0f;
float fractionMaxSpeed = 0.2f;
int axisMask = 63;
motion.changeTransform(chainName, space, transform, fractionMaxSpeed, axisMask);
qi::os::sleep(2.0f);
return 0;
}
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
}
void Locomotion:: lookDown()
{
#ifdef __linux__
/** The name of the joint to be moved. */
const AL::ALValue jointName = AL::ALValue::array("HeadPitch", "HeadYaw");
try {
AL::ALRobotPostureProxy robotPosture(robotIp);
AL::ALMotionProxy motion(robotIp);
float maxSpeedFraction = 0.5f; //[0;1]
bool postureReached = robotPosture.goToPosture("Stand",
maxSpeedFraction);
motion.waitUntilMoveIsFinished();
if (! postureReached)
speech->say("Could not reach posture Stand.");
AL::ALNavigationProxy navigationProxy(robotIp);
float x = .5 ,y = 0, theta = 0;
// navigationProxy.moveTo(x, y, theta);
/** Make sure the head is stiff to be able to move it.
* To do so, make the stiffness go to the maximum in one second.
*/
// /** Target stiffness. */
// AL::ALValue stiffness = 1.0f;
// /** Time (in seconds) to reach the target. */
// AL::ALValue time = 1.0f;
// /** Call the stiffness interpolation method. */
// motion.stiffnessInterpolation(jointName, stiffness, time);
/** Set the target angle list, in radians.
Max pitch: 29.5 degrees = 0.514872129 rad
*/
float degrees = 21.0f;
float rads = degrees * M_PI / 180;
AL::ALValue targetAngles = AL::ALValue::array(rads, 0.0f);
/** Set the corresponding time lists, in seconds. */
AL::ALValue targetTimes = AL::ALValue::array(1.0f, 1.0f);
/** Specify that the desired angles are absolute. */
bool isAbsolute = true;
/** Call the angle interpolation method. The joint will reach the
* desired angles at the desired times.
*/
motion.angleInterpolation(jointName,
targetAngles,
targetTimes,
isAbsolute);
motion.waitUntilMoveIsFinished();
// /** Remove the stiffness on the head. */
// stiffness = 0.0f;
// time = 1.0f;
// motion.stiffnessInterpolation(jointName, stiffness, time);
}
catch (const AL::ALError& e) {
std::cerr << "Caught exception: " << e.what() << std::endl;
exit(EXIT_FAILURE);
}
#else
std::cout << "lookDown not on linux" << std::endl;
#endif
}
