void Kick::Recovering_state_code(void)
{
//BUILDER COMMENT. DO NOT REMOVE. begin Recovering_state_code
ALValue path;
path.arraySetSize(6);
path[0] = 0.0;
path[2] = 0.0;
path[3] = 0.0;
path[4] = 0.0;
path[5] = 0.0;
if(foot == LEFT)
{
path[1] = 0.05;
pmotion->positionInterpolation("LLeg", 2, path, 63, 1.5, false);
path[1] = -0.05;
pmotion->positionInterpolation("RLeg", 2, path, 63, 1.5, false);
}else
{
path[1] = -0.05;
pmotion->positionInterpolation("RLeg", 2, path, 63, 1.5, false);
path[1] = 0.05;
pmotion->positionInterpolation("LLeg", 2, path, 63, 1.5, false);
}
path[0] = 0.0;
path[1] = 0.00;
path[2] = -0.03;
path[3] = 0.0;
path[4] = 0.0;
path[5] = 0.0;
pmotion->positionInterpolation("Torso", 2, path, 63, 1.0, false);
//BUILDER COMMENT. DO NOT REMOVE. end Recovering_state_code
}
/**
* saveImageRemote : test remote image
* @param pName path of the file
*/
void vision::testRemote(){
//Now you can get the pointer to the video structure.
ALValue results;
results.arraySetSize(7);
try
{
results = ( camera->call<ALValue>( "getImageRemote", name ) );
}catch( ALError& e)
{
log->error( "vision", "could not call the getImageRemote method of the NaoCam module" );
}
if (results.getType()!= ALValue::TypeArray) return;
const char* dataPointerIn = static_cast<const char*>(results[6].GetBinary());
int size = results[6].getSize();
//You can get some informations of the image.
int width = (int) results[0];
int height = (int) results[1];
int nbLayers = (int) results[2];
int colorSpace = (int) results[3];
long long timeStamp = ((long long)(int)results[4])*1000000LL + ((long long)(int)results[5]);
// now you create an openCV image and you save it in a file.
IplImage* image = cvCreateImage( cvSize( width, height ), 8, nbLayers );
// image->imageData = ( char* ) imageIn->getFrame();
image->imageData = ( char* ) dataPointerIn;
std::string pName = "aaa";
//const char* imageName = ( pName + DecToString(results[4]) + ".jpg").c_str();
const char* imageName = ( pName + "test" + ".jpg").c_str();
printf("imageName %s\n", imageName);
cvSaveImage( imageName, image );
printf("image saved\n");
try
{
results = ( camera->call<ALValue>( "releaseImage", name ) );
}catch( ALError& e)
{
log->error( "vision", "could not call the releaseImage method of the NaoCam module" );
}
printf("image memory released\n");
// cvReleaseImage( &image );
cvReleaseImageHeader(&image);
printf("image released\n");
printf("testRemote finished\n");
}
/**
* Creates the appropriate aliases with the DCM
*/
void NaoEnactor::initDCMAliases(){
ALValue positionCommandsAlias;
positionCommandsAlias.arraySetSize(3);
positionCommandsAlias[0] = string("AllActuatorPosition");
positionCommandsAlias[1].arraySetSize(Kinematics::NUM_JOINTS);
ALValue hardCommandsAlias;
hardCommandsAlias.arraySetSize(3);
hardCommandsAlias[0] = string("AllActuatorHardness");
hardCommandsAlias[1].arraySetSize(Kinematics::NUM_JOINTS);
for (unsigned int i = 0; i<Kinematics::NUM_JOINTS; i++){
positionCommandsAlias[1][i] = jointsP[i];
hardCommandsAlias[1][i] = jointsH[i];
}
dcmProxy->createAlias(positionCommandsAlias);
dcmProxy->createAlias(hardCommandsAlias);
}
void SimuNaoProvider::initDCMAliases()
{
ALValue positionCommandsAlias;
positionCommandsAlias.arraySetSize(2);
positionCommandsAlias[0] = std::string("AllActuatorPosition");
ALValue hardCommandsAlias;
hardCommandsAlias.arraySetSize(2);
hardCommandsAlias[0] = std::string("AllActuatorHardness");
positionCommandsAlias[1].arraySetSize(numOfJoints);
hardCommandsAlias[1].arraySetSize(numOfJoints);
for (int i=0;i<numOfJoints;i++)
{
positionCommandsAlias[1][i] = std::string(jointNames[i]) + "/Position/Actuator/Value";
hardCommandsAlias[1][i] = std::string(jointNames[i]) + "/Hardness/Actuator/Value";
}
dcmProxy->createAlias(positionCommandsAlias);
dcmProxy->createAlias(hardCommandsAlias);
}
/**
* @brief
*/
void oru_walk::initPosition()
{
/// @attention Hardcoded parameter!
unsigned int init_time = 1200;
ALValue initPositionCommands;
initPositionCommands.arraySetSize(6);
initPositionCommands[0] = string("jointActuator");
initPositionCommands[1] = string("ClearAll");
initPositionCommands[2] = string("time-separate");
initPositionCommands[3] = 0;
initPositionCommands[4].arraySetSize(1);
initPositionCommands[5].arraySetSize(JOINTS_NUM);
for (int i = 0; i < JOINTS_NUM; i++)
{
initPositionCommands[5][i].arraySetSize(1);
}
initJointAngles (initPositionCommands[5]);
for (int i = 0; i < LOWER_JOINTS_NUM; i++)
{
ref_joint_angles[i] = initPositionCommands[5][i][0];
}
// set time
try
{
initPositionCommands[4][0] = dcm_proxy->getTime(init_time);
}
catch (const ALError &e)
{
ORUW_THROW_ERROR("Error on DCM getTime : ", e);
}
// send commands
try
{
dcm_proxy->setAlias(initPositionCommands);
}
catch (const AL::ALError &e)
{
ORUW_THROW_ERROR("Error with DCM setAlias : ", e);
}
qi::os::msleep(init_time);
qiLogInfo ("module.oru_walk", "Execution of initPosition() is finished.");
}
int Sensors::Execute()
{
if(firstrun)
{
//Starting US Sensors
ALValue commands;
commands.arraySetSize(3);
commands[0] = string("Device/SubDeviceList/US/Actuator/Value");
commands[1] = string("Merge");
commands[2].arraySetSize(1);
commands[2][0].arraySetSize(2);
commands[2][0][0] = 68.0;
commands[2][0][1] = dcm->getTime(10);
dcm->set(commands);
rtm.start();
#ifndef KROBOT_IS_REMOTE
KAlBroker::Instance().GetBroker()->getProxy("DCM")->getModule()->atPostProcess(KALBIND(&Sensors::synchronisedDCMcallback , this));
#endif
firstrun = false;
}
#ifdef KROBOT_IS_REMOTE
//Fetch into vectors
jointaccess.GetValues(jointValues);
sensoraccess.GetValues(sensorValues);
fetchValues();
publishData(ASM, "sensors");
if(updateButtons())
{
publishSignal(BM, "buttonevents");
}
#endif
float oldvalue;
vector<float> RobotValues = motion->getRobotPosition(true);
//A vector containing the World Absolute Robot Position. (Absolute Position X, Absolute Position Y, Absolute Angle Z)
for (unsigned i = 0; i < RobotValues.size(); i++)
{
oldvalue = RPM.sensordata(i).sensorvalue();
RPM.mutable_sensordata(i)->set_sensorvalue(RobotValues[i]);
RPM.mutable_sensordata(i)->set_sensorvaluediff(RobotValues[i] - oldvalue);
}
RPM.set_timediff(timediff);
publishData(RPM, "sensors");
return 0;
}
void Kick::Initial_state_code(void)
{
//BUILDER COMMENT. DO NOT REMOVE. begin Initial_state_code
//Primero ir a PoseInit
pmotion->setWalkTargetVelocity(0.0, 0.0, 0.0, 0.7);
ALValue path;
path.arraySetSize(6);
path[0] = 0.0;
path[2] = 0.0;
path[3] = 0.0;
path[4] = 0.0;
path[5] = 0.0;
path[1] = 0.03;
pmotion->positionInterpolation("LLeg", 2, path, 63, 0.5, false);
path[1] = -0.03;
pmotion->positionInterpolation("RLeg", 2, path, 63, 0.5, false);
HPoint3D ball3D;
/*HPoint2D ball2D;
ball2D.x = _BallDetectorOld->getX()*160.0+80.0;
ball2D.y = _BallDetectorOld->getY()*120.0+60.0;
ball2D.h = 1.0;
_Kinematics->get3DPositionZ(ball3D, ball2D, 0.04);
path[0] = 0.0;
path[2] = -0.03;
path[3] = 0.0;
path[4] = 0.0;
path[5] = 0.0;
*/
ball3D.Y = 1.0;
if(ball3D.Y > 0.0)
path[1] = -0.07;
else
path[1] = 0.07;
pmotion->positionInterpolation("Torso", 2, path, 63, 1.0, false);
//BUILDER COMMENT. DO NOT REMOVE. end Initial_state_code
}
/**
* @brief Set stiffness of joints.
*
* @param[in] stiffnessValue value of stiffness [0;1]
*/
void oru_walk::setStiffness(const float &stiffnessValue)
{
ALValue stiffnessCommands;
if ((stiffnessValue < 0) || (stiffnessValue > 1))
{
ORUW_THROW("Wrong parameters");
}
// Prepare one dcm command:
// it will linearly "Merge" all joint stiffness
// from last value to "stiffnessValue" in 1 seconde
stiffnessCommands.arraySetSize(3);
stiffnessCommands[0] = std::string("jointStiffness");
stiffnessCommands[1] = std::string("Merge");
stiffnessCommands[2].arraySetSize(1);
stiffnessCommands[2][0].arraySetSize(2);
stiffnessCommands[2][0][0] = stiffnessValue;
/// @attention Hardcoded parameter!
unsigned int stiffness_change_time = 1000;
try
{
stiffnessCommands[2][0][1] = dcm_proxy->getTime(stiffness_change_time);
}
catch (const ALError &e)
{
ORUW_THROW_ERROR("Error on DCM getTime : ", e);
}
try
{
dcm_proxy->set(stiffnessCommands);
}
catch (const ALError &e)
{
ORUW_THROW_ERROR("Error when sending stiffness to DCM : ", e);
}
qi::os::msleep(stiffness_change_time);
qiLogInfo ("module.oru_walk", "Execution of setStiffness() is finished.");
}
JNIEXPORT jlong JNICALL Java_jp_ac_fit_asura_naoji_jal_JALMemory__1createQuery(
JNIEnv *env, jclass, jlong jmemoryPtr, jobjectArray jstrings) {
JALMemory *jmemory = reinterpret_cast<JALMemory*> (jmemoryPtr);
assert(jmemory != NULL);
Query *query = new Query(jmemory);
jsize size = env->GetArrayLength(jstrings);
ALValue names;
names.arraySetSize(size);
for (int i = 0; i < size; i++) {
jstring jstr = static_cast<jstring> (env->GetObjectArrayElement(
jstrings, i));
jassert(env, jstr != NULL);
names[i] = toString(env, jstr);
env->DeleteLocalRef(jstr);
}
query->names = names;
return reinterpret_cast<jlong> (query);
}
Agent::Agent(AL::ALPtr<AL::ALBroker> pBroker, const std::string& pName)
: ALModule(pBroker, pName),
alwalk_state(INACTIVE),
shuttingDown(false),
skipped_frames(0),
sit_step(-1.0f),
limp(true),
head_limp(false),
chest_down(0),
chest_up(0),
chest_presses(0),
old_battery(0),
old_battery_status(0) {
uint8_t i;
// need to initialise first, before using log
log = new ALLoggerProxy(pBroker);
if (log == NULL)
throw ALERROR(name, "constructor", "log == NULL");
log->info(name, "Constructing");
try {
po::options_description cmdline_options =
store_and_notify(std::vector<string>(0), vm, NULL);
wirelessIwconfigArgs = vm["network.wireless.iwconfig_flags"].as<string>();
wirelessIfconfigArgs =
vm["network.wireless.static.ifconfig_flags"].as<string>();
wiredIfconfigArgs =
vm["network.wired.static.ifconfig_flags"].as<string>();
wirelessStatic = vm["network.wireless.static"].as<bool>();
wiredStatic = vm["network.wired.static"].as<bool>();
playerNum = vm["player.number"].as<int>();
teamNum = vm["player.team"].as<int>();
doNetworking();
} catch(po::error& e) {
log->error(name, "failed parsing command line arguments");
log->error(name, e.what());
} catch(std::exception& e) {
log->error(name, "failed parsing command line arguments");
log->error(name, e.what());
}
dcm = new DCMProxy(pBroker);
if (dcm == NULL)
throw ALERROR(name, "constructor", "dcm == NULL");
memory = new ALMemoryProxy(pBroker);
if (memory == NULL)
throw ALERROR(name, "constructor", "memory == NULL");
motion = new ALMotionProxy(pBroker);
motion->setWalkArmsEnable(false, false);
ALValue config;
config.arraySetSize(13);
for (i = 0; i < 13; ++i)
config[i].arraySetSize(2);
config[0][0] = "WALK_MAX_TRAPEZOID", config[0][1] = 4.5; // 4.5
config[1][0] = "WALK_MIN_TRAPEZOID", config[1][1] = 3.5; // 3.5
config[2][0] = "WALK_STEP_MAX_PERIOD", config[2][1] = 30; // 30
config[3][0] = "WALK_STEP_MIN_PERIOD", config[3][1] = 21; // 21
config[4][0] = "WALK_MAX_STEP_X", config[4][1] = 0.04; // 0.04
config[5][0] = "WALK_MAX_STEP_Y", config[5][1] = 0.04; // 0.04
config[6][0] = "WALK_MAX_STEP_THETA", config[6][1] = 20; // 20
config[7][0] = "WALK_STEP_HEIGHT", config[7][1] = 0.015; // 0.015
config[8][0] = "WALK_FOOT_SEPARATION", config[8][1] = 0.095; // 0.095
config[9][0] = "WALK_FOOT_ORIENTATION", config[9][1] = 0; // 0
config[10][0] = "WALK_TORSO_HEIGHT", config[10][1] = 0.31; // 0.31
config[11][0] = "WALK_TORSO_ORIENTATION_X", config[11][1] = 0; // 0
config[12][0] = "WALK_TORSO_ORIENTATION_Y", config[12][1] = 0; // 0
motion->setMotionConfig(config);
leg_names.push_back("LHipYawPitch"), stand_angles.push_back(0.f);
leg_names.push_back("LHipRoll"), stand_angles.push_back(0.f);
leg_names.push_back("LHipPitch"), stand_angles.push_back(DEG2RAD(-30.f));
leg_names.push_back("LKneePitch"), stand_angles.push_back(DEG2RAD(60.f));
leg_names.push_back("LAnklePitch"), stand_angles.push_back(DEG2RAD(-30.f));
leg_names.push_back("LAnkleRoll"), stand_angles.push_back(0.f);
leg_names.push_back("RHipRoll"), stand_angles.push_back(0.f);
leg_names.push_back("RHipPitch"), stand_angles.push_back(DEG2RAD(-30.f));
leg_names.push_back("RKneePitch"), stand_angles.push_back(DEG2RAD(60.f));
leg_names.push_back("RAnklePitch"), stand_angles.push_back(DEG2RAD(-30.f));
leg_names.push_back("RAnkleRoll"), stand_angles.push_back(0.f);
// open shared memory as RW, create if !exist, permissions 600
shared_fd = shm_open(AGENT_MEMORY, O_RDWR | O_CREAT, 0600);
if (shared_fd < 0)
throw ALERROR(name, "constructor", "shm_open() failed");
// make shared memory file correct size
if (ftruncate(shared_fd, sizeof(AgentData)) == -1)
throw ALERROR(name, "constructor", "ftruncate() failed");
// map shared memory to process memory
shared_data = (AgentData*) mmap(NULL, sizeof(AgentData),
PROT_READ | PROT_WRITE,
MAP_SHARED, shared_fd, 0);
if (shared_data == MAP_FAILED)
throw ALERROR(name, "constructor", "mmap() failed");
// Initialise shared memory
shared_data->sensors_read = 0;
shared_data->sensors_latest = 0;
shared_data->actuators_read = 0;
shared_data->actuators_latest = 0;
SensorValues null_sensors;
JointValues null_joints;
ActionCommand::LED null_leds;
for (i = 0; i < Joints::NUMBER_OF_JOINTS; ++i) {
null_joints.angles[i] = 0.0f;
null_joints.stiffnesses[i] = 0.0f;
null_joints.temperatures[i] = 0.0f;
}
null_sensors.joints = null_joints;
for (i = 0; i < Sensors::NUMBER_OF_SENSORS; ++i)
null_sensors.sensors[i] = 0.0f;
for (i = 0; i < 3; ++i) {
shared_data->sensors[i] = null_sensors;
shared_data->joints[i] = null_joints;
shared_data->leds[i] = null_leds;
shared_data->sonar[i] = 0.0f;
}
shared_data->standing = false;
pthread_mutexattr_t attr;
pthread_mutexattr_init(&attr);
pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_ERRORCHECK);
pthread_mutex_init(&shared_data->lock, &attr);
// create semaphore with permissions 600, value 0
semaphore = sem_open(AGENT_SEMAPHORE, O_RDWR | O_CREAT, 0600, 0);
if (semaphore < 0)
throw ALERROR(name, "constructor", "sem_open() failed");
// Initialise ALMemory pointers
std::vector<std::string> key_names;
for (i = 0; i < Sensors::NUMBER_OF_SENSORS; ++i)
sensor_pointers.push_back((float*) memory->getDataPtr(
std::string("Device/SubDeviceList/") +
Sensors::sensorNames[i] + "/Sensor/Value"));
for (i = 0; i < Joints::NUMBER_OF_JOINTS; ++i) {
joint_pointers.push_back((float*) memory->getDataPtr(
std::string("Device/SubDeviceList/") +
Joints::jointNames[i] +
"/Position/Sensor/Value"));
temperature_pointers.push_back((float*) memory->getDataPtr(
std::string("Device/SubDeviceList/") +
Joints::jointNames[i] +
"/Temperature/Sensor/Value"));
}
for (i = 0; i < Sonar::NUMBER_OF_READINGS; ++i)
sonar_pointers.push_back((float*) memory->getDataPtr(
std::string("Device/SubDeviceList/") +
Sonar::readingNames[i]));
battery_status_pointer = (int*) memory->
getDataPtr("Device/SubDeviceList/Battery/Charge/Sensor/Status");
// DCM Command Aliases
ALValue alias;
alias.arraySetSize(2);
alias[0] = string("JointAngles");
alias[1].arraySetSize(Joints::NUMBER_OF_JOINTS - 2);
for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
alias[1][i-2] = string(Joints::jointNames[i]).
append("/Position/Actuator/Value");
}
dcm->createAlias(alias);
alias[0] = string("HeadAngles");
alias[1].arraySetSize(2);
for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
alias[1][i] = string(Joints::jointNames[i]).
append("/Position/Actuator/Value");
}
dcm->createAlias(alias);
alias[0] = string("LEDs");
alias[1].arraySetSize(LEDs::NUMBER_OF_LEDS);
for (i = 0; i < LEDs::NUMBER_OF_LEDS; ++i) {
alias[1][i] = string(LEDs::ledNames[i]);
}
dcm->createAlias(alias);
alias[0] = string("JointStiffnesses");
alias[1].arraySetSize(Joints::NUMBER_OF_JOINTS - 2);
for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
alias[1][i-2] = string(Joints::jointNames[i]).
append("/Hardness/Actuator/Value");
}
dcm->createAlias(alias);
alias[0] = string("HeadStiffnesses");
alias[1].arraySetSize(2);
for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
alias[1][i] = string(Joints::jointNames[i]).
append("/Hardness/Actuator/Value");
}
dcm->createAlias(alias);
alias[0] = string("Sonar");
alias[1].arraySetSize(1);
alias[1][0] = Sonar::actuatorName;
dcm->createAlias(alias);
// Set up Commands ALValue
angle_command.arraySetSize(6);
angle_command[0] = string("JointAngles");
angle_command[1] = string("ClearAfter");
angle_command[2] = string("time-separate");
angle_command[3] = 0;
angle_command[4].arraySetSize(1);
angle_command[5].arraySetSize(Joints::NUMBER_OF_JOINTS - 2);
for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
angle_command[5][i-2].arraySetSize(1);
}
head_angle_command.arraySetSize(6);
head_angle_command[0] = string("HeadAngles");
head_angle_command[1] = string("ClearAfter");
head_angle_command[2] = string("time-separate");
head_angle_command[3] = 0;
head_angle_command[4].arraySetSize(1);
head_angle_command[5].arraySetSize(2);
for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
head_angle_command[5][i].arraySetSize(1);
}
led_command.arraySetSize(6);
led_command[0] = string("LEDs");
led_command[1] = string("ClearAfter");
led_command[2] = string("time-separate");
led_command[3] = 0;
led_command[4].arraySetSize(1);
led_command[5].arraySetSize(LEDs::NUMBER_OF_LEDS);
for (i = 0; i < LEDs::NUMBER_OF_LEDS; ++i) {
led_command[5][i].arraySetSize(1);
}
stiffness_command.arraySetSize(6);
stiffness_command[0] = string("JointStiffnesses");
stiffness_command[1] = string("ClearAfter");
stiffness_command[2] = string("time-separate");
stiffness_command[3] = 0;
stiffness_command[4].arraySetSize(1);
stiffness_command[5].arraySetSize(Joints::NUMBER_OF_JOINTS - 2);
for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
stiffness_command[5][i-2].arraySetSize(1);
}
head_stiffness_command.arraySetSize(6);
head_stiffness_command[0] = string("HeadStiffnesses");
head_stiffness_command[1] = string("ClearAfter");
head_stiffness_command[2] = string("time-separate");
head_stiffness_command[3] = 0;
head_stiffness_command[4].arraySetSize(1);
head_stiffness_command[5].arraySetSize(2);
for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
head_stiffness_command[5][i].arraySetSize(1);
}
sonar_command.arraySetSize(6);
sonar_command[0] = string("Sonar");
sonar_command[1] = string("ClearAfter");
sonar_command[2] = string("time-separate");
sonar_command[3] = 0;
sonar_command[4].arraySetSize(1);
sonar_command[4][0] = dcm->getTime(20);
sonar_command[5].arraySetSize(1);
sonar_command[5][0].arraySetSize(1);
sonar_command[5][0][0] = 68.0f;
dcm->setAlias(sonar_command);
// Get offset between our clock and the DCM's
time_offset = (int)dcm->getTime(0) - timeNow();
// Set up callbacks
dcm->getModule()->atPostProcess(boost::bind(postCallback_, this));
dcm->getModule()->atPreProcess(boost::bind(preCallback_, this));
// Fix the clock if an ntp server is available
ret = system("/bin/su -c '/etc/init.d/openntpd restart'");
ret = system("/usr/bin/amixer -qs < /home/nao/data/volumeinfo.dat");
// Play the 'oneg-nook' jingle
ret =
system("/usr/bin/aplay -q /opt/naoqi/share/naoqi/wav/start_jingle.wav");
// Start rUNSWift
// Uncomment this to have rUNSWift start when you turn the robot on
/*
SAY("loaded runswift");
if (!fork()) {
struct sched_param s;
s.sched_priority = 0;
int ret = sched_setscheduler(0, SCHED_OTHER, &s);
ret = execlp("/home/nao/bin/runswift", "runswift",
(char*)NULL);
::exit(ret);
}
*/
log->info(name, "Constructed");
}
void Agent::preCallback() {
// Avoid race condition on shutdown
if (shuttingDown) {
return;
}
const unsigned int now = timeNow();
shared_data->actuators_read = shared_data->actuators_latest;
JointValues& joints = shared_data->joints[shared_data->actuators_read];
SensorValues& sensors = shared_data->sensors[shared_data->sensors_latest];
doLEDs(shared_data->leds[shared_data->actuators_read]);
if (limp || shared_data->standing) {
uint8_t i;
for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
head_angle_command[5][i][0] = sit_angles[i];
head_stiffness_command[5][i][0] = 0.0f;
}
for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
angle_command[5][i-2][0] = sit_angles[i];
stiffness_command[5][i-2][0] = 0.0f;
}
// Make chest button blink green if limp but still in contact
// or blink red if not in contact or purple if ready to stand
float blink = now / 200 & 1;
if (shared_data->standing) {
led_command[5][LEDs::ChestRed][0] = blink;
led_command[5][LEDs::ChestGreen][0] = blink;
led_command[5][LEDs::ChestBlue][0] = 0.0;
} else if (skipped_frames <= MAX_SKIPS) {
led_command[5][LEDs::ChestRed][0] = 0.0f;
led_command[5][LEDs::ChestGreen][0] = blink;
led_command[5][LEDs::ChestBlue][0] = 0.0f;
} else {
led_command[5][LEDs::ChestRed][0] = blink;
led_command[5][LEDs::ChestGreen][0] = 0.0f;
led_command[5][LEDs::ChestBlue][0] = 0.0f;
}
} else if (skipped_frames <= MAX_SKIPS && sit_step == -1.0f) {
uint8_t i;
for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
head_angle_command[5][i][0] = joints.angles[i];
head_stiffness_command[5][i][0] = joints.stiffnesses[i];
}
if (joints.AL_command == AL_ON) {
static int stand_t = 0;
motion->setStiffnesses("Body", 0.66);
ALValue config;
switch (alwalk_state) {
case INACTIVE:
config.arraySetSize(13);
for (i = 0; i < 13; ++i)
config[i].arraySetSize(2);
config[0][0] = "WALK_MAX_TRAPEZOID";
config[0][1] = 4.5; // 4.5
config[1][0] = "WALK_MIN_TRAPEZOID";
config[1][1] = 3.5; // 3.5
config[2][0] = "WALK_STEP_MAX_PERIOD";
config[2][1] = 30; // 30
config[3][0] = "WALK_STEP_MIN_PERIOD";
config[3][1] = 21; // 21
config[4][0] = "WALK_MAX_STEP_X";
config[4][1] = 0.04; // 0.04
config[5][0] = "WALK_MAX_STEP_Y";
config[5][1] = 0.04; // 0.04
config[6][0] = "WALK_MAX_STEP_THETA";
config[6][1] = 20; // 20
config[7][0] = "WALK_STEP_HEIGHT";
config[7][1] = 0.015; // 0.015
config[8][0] = "WALK_FOOT_SEPARATION";
config[8][1] = 0.095; // 0.095
config[9][0] = "WALK_FOOT_ORIENTATION";
config[9][1] = 0; // 0
config[10][0] = "WALK_TORSO_HEIGHT";
config[10][1] = joints.AL_height; // 0.31
config[11][0] = "WALK_TORSO_ORIENTATION_X";
config[11][1] = 0; // 0
config[12][0] = "WALK_TORSO_ORIENTATION_Y";
config[12][1] = 0; // 0
motion->setMotionConfig(config);
stand_angles.clear();
stand_angles.push_back(0.f);
stand_angles.push_back(0.f);
stand_angles.push_back(-joints.AL_bend);
stand_angles.push_back(2*joints.AL_bend);
stand_angles.push_back(-joints.AL_bend);
stand_angles.push_back(0.f);
stand_angles.push_back(0.f);
stand_angles.push_back(-joints.AL_bend);
stand_angles.push_back(2*joints.AL_bend);
stand_angles.push_back(-joints.AL_bend);
stand_angles.push_back(0.f);
alwalk_state = WALKING;
break;
case WALKING:
if (joints.AL_stop) {
// motion->walkTo(0.001, 0.001, 0.001);
motion->post.angleInterpolation(leg_names, stand_angles,
0.25, true);
alwalk_state = STANDING;
} else {
motion->setWalkTargetVelocity(joints.AL_x, joints.AL_y,
joints.AL_theta,
joints.AL_frequency);
}
break;
case STOPPING:
if (!motion->walkIsActive()) {
motion->post.angleInterpolation(leg_names, stand_angles,
0.25, true);
alwalk_state = STANDING;
}
break;
case STANDING:
if (stand_t++ > 25) {
stand_t = 0;
motion->killWalk();
alwalk_state = STOPPED;
}
break;
case STOPPED:
alwalk_state = INACTIVE;
break;
}
} else {
for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
angle_command[5][i-2][0] = joints.angles[i];
stiffness_command[5][i-2][0] = joints.stiffnesses[i];
}
}
} else {
if (sit_step == -1.0f) {
sit_step = 0.0f;
for (uint8_t i = 0; i < Joints::NUMBER_OF_JOINTS; ++i)
sit_joints.angles[i] = sensors.joints.angles[i];
SAY("lost contact");
motion->killAll();
}
if (sit_step < 1.0f) {
// interpolate legs over 2s, arms over 0.2s
sit_step += 0.005f;
uint8_t i;
for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
head_angle_command[5][i][0] = (1 - sit_step) * sit_joints.angles[i]
+ sit_step * sit_angles[i];
head_stiffness_command[5][i][0] = 1.0f;
}
for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
float k = (i >= Joints::LShoulderPitch &&
i <= Joints::LElbowRoll) ||
(i >= Joints::RShoulderPitch &&
i <= Joints::RElbowRoll) ? 10 : 1;
k = MIN(1.0f, k * sit_step);
angle_command[5][i-2][0] = (1 - k) * sit_joints.angles[i] +
k * sit_angles[i];
stiffness_command[5][i-2][0] = 1.0f;
}
} else {
limp = true;
sit_step = -1.0f;
uint8_t i;
for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
head_angle_command[5][i][0] = sit_angles[i];
head_stiffness_command[5][i][0] = 0.0f;
}
for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
angle_command[5][i-2][0] = sit_angles[i];
stiffness_command[5][i-2][0] = 0.0f;
}
}
// Make chest button solid red/green if sitting down
// (depending on whether or not we are back in contact)
if (skipped_frames > MAX_SKIPS) {
led_command[5][LEDs::ChestRed][0] = 1.0f;
led_command[5][LEDs::ChestGreen][0] = 0.0f;
} else {
led_command[5][LEDs::ChestRed][0] = 0.0f;
led_command[5][LEDs::ChestGreen][0] = 1.0f;
}
led_command[5][LEDs::ChestBlue][0] = 0.0f;
}
// sonar_command[5][0][0] = shared_data->sonar[shared_data->actuators_read];
sonar_command[5][0][0] = Sonar::Mode::CONTINUOUS + Sonar::Mode::ON;
if (head_limp) {
float blink = now / 200 & 1;
led_command[5][LEDs::LeftEyeGreen8][0] = blink;
led_command[5][LEDs::RightEyeGreen8][0] = blink;
head_stiffness_command[5][Joints::HeadYaw][0] = 0.0f;
head_stiffness_command[5][Joints::HeadPitch][0] = 0.0f;
}
head_stiffness_command[4][0] = (int)now + time_offset;
head_angle_command[4][0] = (int)now + time_offset;
stiffness_command[4][0] = (int)now + time_offset;
angle_command[4][0] = (int)now + time_offset;
led_command[4][0] = (int)now + time_offset;
sonar_command[4][0] = (int)now + time_offset;
dcm->setAlias(head_stiffness_command);
dcm->setAlias(head_angle_command);
if (limp || shared_data->standing ||
!(skipped_frames <= MAX_SKIPS && sit_step == -1.0f &&
joints.AL_command == AL_ON)) {
dcm->setAlias(stiffness_command);
dcm->setAlias(angle_command);
}
dcm->setAlias(led_command);
// dcm->setAlias(sonar_command);
}
ALValue vision::getBalls() {
ALValue resultBallRect;
resultBallRect.arraySetSize(4);
resultBallRect[0] = 0;
resultBallRect[1] = 0;
resultBallRect[2] = 0;
resultBallRect[3] = 0;
//First you have to declare an ALVisionImage to get the video buffer.
// ( definition included in alvisiondefinitions.h and alvisiondefinitions.cpp )
ALVisionImage* imageIn;
//Now you can get the pointer to the video structure.
try
{
imageIn = ( ALVisionImage* ) ( camera->call<int>( "getImageLocal", name ) );
}catch( ALError& e)
{
log->error( "vision", "could not call the getImageLocal method of the NaoCam module" );
}
//You can get some informations of the image.
int width = imageIn->fWidth;
int height = imageIn->fHeight;
int nbLayers = imageIn->fNbLayers;
int colorSpace = imageIn->fColorSpace;
long long timeStamp = imageIn->fTimeStamp;
int seconds = (int)(timeStamp/1000000LL);
// log->info( "vision", "Creating OpenCV image" );
//You can get the pointer of the image.
uInt8 *dataPointerIn = imageIn->getFrame();
// now you create an openCV image and you save it in a file.
IplImage* src = cvCreateImage( cvSize( width, height ), 8, nbLayers );
// src->imageData = ( char* ) imageIn->getFrame();
src->imageData = ( char* ) dataPointerIn;
//log->info( "vision", "Searching field" );
IplImage* mask = 0;
IplImage* imageClipped = 0;
// Get field
CvRect* fieldRect = new CvRect[1];
//printf("before getLargestColoredContour\n");
// Green field
// parameters for pan/tilt camera
//CvSeq* field = getLargestColoredContour(src, 155, 5, 100, 300, fieldRect);
// parameters for Nao camera in lab
// CvSeq* field = getLargestColoredContour(src, 175, 30, 25, 1000, fieldRect);
// Params for WEBOTS
CvSeq* field = getLargestColoredContour(src, 125, 30, 25, 100, &fieldRect, 1)[0];
if (field != NULL) {
// printf("Field: %d, %d, %d, %d\n", fieldRect.x, fieldRect.y, fieldRect.width, fieldRect.height);
//log->info( "vision", "Searching ball1" );
CvSize imageSize = cvSize(src->width, src->height);
mask = cvCreateImage( imageSize, 8, 1 );
cvZero(mask);
CvScalar colorWHITE = CV_RGB(255, 255, 255);
int elementCount = field->total;
CvPoint* temp = new CvPoint[elementCount];
CvPoint pt0 = **CV_GET_SEQ_ELEM( CvPoint*, field, elementCount - 1 );
for (int i = 0; i < elementCount; i++) {
CvPoint pt = **CV_GET_SEQ_ELEM( CvPoint*, field, i );
temp[i].x = pt.x;
temp[i].y = pt.y;
}
cvFillConvexPoly(mask, temp, elementCount, colorWHITE, 8, 0);
imageClipped = cvCreateImage( imageSize, 8, 3 );
cvZero(imageClipped);
cvCopy(src, imageClipped, mask);
// Get ball
CvRect* ballRect= new CvRect[10];
//log->info( "vision", "Searching ball2" );
// parameters for pan/tilt camera
//getLargestColoredContour(imageClipped, 17, 10, 100, 50, ballRect);
// parameters for Nao camera in lab
// CvSeq* ballHull = getLargestColoredContour(imageClipped, 40, 25, 50, 30, ballRect);
// Params for webots
CvSeq** ballHull = getLargestColoredContour(imageClipped, 55, 125, 50, 30, &ballRect, 0);
//log->info( "vision", "Searching ball3" );
int* X_Arr= new int[10];
int* Y_Arr= new int[10];
int* Width_Arr= new int[10];
int* Height_Arr= new int[10];
for(int i=0; ballHull[i] != NULL; i++) {
X_Arr[i]= ballRect[i].x;
Y_Arr[i]= ballRect[i].y;
Width_Arr[i]= ballRect[i].width;
Height_Arr[i]= ballRect[i].height;
}
if (ballHull != NULL) {
// printf("ballrect: %d, %d, %d, %d\n", ballRect.x, ballRect.y, ballRect.width, ballRect.height);
resultBallRect[0] = X_Arr;
resultBallRect[1] = Y_Arr;
resultBallRect[2] = Width_Arr;
resultBallRect[3] = Height_Arr;
// printf("Clearing ball Hull\n");
// cvClearSeq(ballHull);
// printf("Ball Hull cleared\n");
} else {
// printf("Ball not found!\n");
resultBallRect[0] = -2;
resultBallRect[1] = -2;
resultBallRect[2] = -2;
resultBallRect[3] = -2;
}
} else {
void * urgThread(void * arg) {
#if defined (__linux__)
// thread name
prctl(PR_SET_NAME, "ALLaser::urgThread", 0, 0, 0);
#endif
ALValue urgdata;
long *data = NULL;
int data_max;
int ret;
int n;
int i,imemory,refTime,end,sampleTime, beginThread;
std::string valueName="Device/Laser/Value";
/* Connection */
connectToLaser();
/* Reserve the Receive data buffer */
data_max = urg_dataMax(&urg);
if (data_max <= 0) {
perror("data_max is less than 0");
pthread_exit((void *)NULL);
}
data = (long*)malloc(sizeof(long) * data_max);
memset(data, 0, sizeof(long) * data_max);
if (data == NULL) {
perror("data buffer");
pthread_exit((void *)NULL);
}
/* prepare ALValue for ALMemory*/
urgdata.arraySetSize(data_max);
for (i=0; i< data_max;i++)
{
urgdata[i].arraySetSize(4);
urgdata[i][0]= (int)0;
urgdata[i][1]= (double)0.0;
urgdata[i][2]= (int)0;
urgdata[i][3]= (int)0;
}
/* insert ALvalue in ALMemory*/
gSTM->insertData(valueName,urgdata);
gSTM->insertData("Device/Laser/LaserEnable", (bool) 1);
gSTM->insertData("Device/Laser/MinAngle",(float)((2.0 * M_PI)
* (DEFAULT_MIN_ANGLE - MIDDLE_ANGLE) / RESOLUTION_LASER));
gSTM->insertData("Device/Laser/MaxAngle",(float)((2.0 * M_PI)
* (DEFAULT_MAX_ANGLE - MIDDLE_ANGLE) / RESOLUTION_LASER));
gSTM->insertData("Device/Laser/MinLength",(float)(length_min));
gSTM->insertData("Device/Laser/MaxLength",(float)(length_max));
stringstream ss;
ss << "ALLaser running";
qiLogInfo("hardware.laser") << ss.str() << std::endl;
while(1)
{
if(mode==SEND_MODE_ON){
ret = urg_laserOn(&urg);
if (ret < 0) {
urg_exit(&urg, "urg_requestData()");
}
mode=MODE_ON;
/* Connection */
connectToLaser();
}
if(mode==SEND_MODE_OFF){
scip_qt(&urg.serial_, NULL, ScipNoWaitReply);
mode=MODE_OFF;
}
if(mode==MODE_ON){
/* Request Data using GD-Command */
ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
if (ret < 0) {
urg_exit(&urg, "urg_requestData()");
}
refTime = getLocalTime();
/* Obtain Data */
n = urg_receiveData(&urg, data, data_max);
qiLogDebug("hardware.laser") << " n " << n << " expected " <<
angle_max - angle_min << std::endl;
if (n < 0) {
urg_exit(&urg, "urg_receiveData()");
}
end= getLocalTime();
sampleTime=end-refTime;
imemory=0;
for (i = 0; i < n; ++i) {
int x, y;
double angle = urg_index2rad(&urg, i);
qiLogDebug("hardware.laser") << i << " angle " << angle <<
" urgAngle " << urg_index2rad(&urg, i) <<
" dist " << data[i] << std::endl;
int length = data[i];
if( length >= length_min && length <= length_max
&& i >= angle_min && i <= angle_max ){
x = (int)((double)length * cos(angle));
y = (int)((double)length * sin(angle));
urgdata[imemory][0]= length;
urgdata[imemory][1]= angle;
urgdata[imemory][2]= x;
urgdata[imemory][3]= y;
imemory++;
}
}
for(;imemory<data_max;imemory++){
urgdata[imemory][0]= 0;
urgdata[imemory][1]= 0;
urgdata[imemory][2]= 0;
urgdata[imemory][3]= 0;
}
gSTM->insertData(valueName,urgdata);
usleep(1000);
}else{
usleep(10000);
}
}
urg_disconnect(&urg);
free(data);
}
/** Run the kick. */
void
NaoQiMotionKickTask::run()
{
const char *shoot_hip_roll_name = NULL;
const char *support_hip_roll_name = NULL;
const char *shoot_hip_pitch_name = NULL;
const char *support_hip_pitch_name = NULL;
const char *shoot_knee_pitch_name = NULL;
const char *shoot_ankle_pitch_name = NULL;
const char *shoot_ankle_roll_name = NULL;
const char *support_ankle_roll_name = NULL;
float shoot_hip_roll = 0;
float support_hip_roll = 0;
float shoot_hip_pitch = 0;
float support_hip_pitch = 0;
float shoot_knee_pitch = 0;
float shoot_ankle_pitch = 0;
float shoot_ankle_roll = 0;
float support_ankle_roll = 0;
float BALANCE_HIP_ROLL = 0;
float BALANCE_ANKLE_ROLL = 0;
float STRIKE_OUT_HIP_ROLL = 0;
if ( __leg == fawkes::HumanoidMotionInterface::LEG_LEFT ) {
shoot_hip_roll_name = "LHipRoll";
support_hip_roll_name = "RHipRoll";
shoot_hip_pitch_name = "LHipPitch";
support_hip_pitch_name = "RHipPitch";
shoot_knee_pitch_name = "LKneePitch";
shoot_ankle_pitch_name = "LAnklePitch";
shoot_ankle_roll_name = "LAnkleRoll";
support_ankle_roll_name = "RAnkleRoll";
BALANCE_HIP_ROLL = 20;
BALANCE_ANKLE_ROLL = -25;
STRIKE_OUT_HIP_ROLL = 30;
} else if (__leg == fawkes::HumanoidMotionInterface::LEG_RIGHT ) {
shoot_hip_roll_name = "RHipRoll";
support_hip_roll_name = "LHipRoll";
shoot_hip_pitch_name = "RHipPitch";
support_hip_pitch_name = "LHipPitch";
shoot_knee_pitch_name = "RKneePitch";
shoot_ankle_pitch_name = "RAnklePitch";
shoot_ankle_roll_name = "RAnkleRoll";
support_ankle_roll_name = "LAnkleRoll";
BALANCE_HIP_ROLL = -20;
BALANCE_ANKLE_ROLL = 25;
STRIKE_OUT_HIP_ROLL = -30;
}
if (__quit) return;
goto_start_pos(0.2);
ALValue names;
ALValue target_angles;
float speed = 0;
// Balance on supporting leg
names.arraySetSize(4);
target_angles.arraySetSize(4);
support_hip_roll = BALANCE_HIP_ROLL;
shoot_hip_roll = BALANCE_HIP_ROLL;
shoot_ankle_roll = BALANCE_ANKLE_ROLL;
support_ankle_roll = BALANCE_ANKLE_ROLL;
names = ALValue::array(support_hip_roll_name, shoot_hip_roll_name,
support_ankle_roll_name, shoot_ankle_roll_name);
target_angles = ALValue::array(deg2rad(support_hip_roll), deg2rad(shoot_hip_roll),
deg2rad(support_ankle_roll), deg2rad(shoot_ankle_roll));
speed = 0.15;
//if (__quit) return;
__almotion->angleInterpolationWithSpeed(names, target_angles, speed);
names.clear();
target_angles.clear();
// Raise shooting leg
names.arraySetSize(3);
target_angles.arraySetSize(3);
shoot_hip_roll = STRIKE_OUT_HIP_ROLL;
shoot_knee_pitch = 90;
shoot_ankle_pitch = -50;
names = ALValue::array(shoot_hip_roll_name, shoot_knee_pitch_name, shoot_ankle_pitch_name);
target_angles = ALValue::array(deg2rad(shoot_hip_roll), deg2rad(shoot_knee_pitch),
deg2rad(shoot_ankle_pitch));
speed = 0.2;
if (__quit) return;
__almotion->angleInterpolationWithSpeed(names, target_angles, speed);
names.clear();
target_angles.clear();
// Strike out
names.arraySetSize(2);
target_angles.arraySetSize(2);
shoot_hip_pitch = 20;
support_hip_pitch = -50;
names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name);
target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch));
speed = 0.1;
if (__quit) return;
__almotion->angleInterpolationWithSpeed(names, target_angles, speed);
names.clear();
target_angles.clear();
// Shoot
names.arraySetSize(4);
target_angles.arraySetSize(4);
shoot_hip_pitch = -80;
support_hip_pitch = -40;
shoot_knee_pitch = 50;
shoot_ankle_pitch = -30;
names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name,
shoot_knee_pitch_name, shoot_ankle_pitch_name);
target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch),
deg2rad(shoot_knee_pitch), deg2rad(shoot_ankle_pitch));
speed = 1.0;
if (__quit) return;
__almotion->angleInterpolationWithSpeed(names, target_angles, speed);
names.clear();
target_angles.clear();
// Move to upright position
names.arraySetSize(2);
target_angles.arraySetSize(2);
shoot_hip_pitch = -25;
support_hip_pitch = -25;
names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name);
target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch));
speed = 0.1;
if (__quit) return;
__almotion->angleInterpolationWithSpeed(names, target_angles, speed);
//names.clear();
//target_angles.clear();
if (__quit) return;
goto_start_pos(0.1);
}