예제 #1
0
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
}
예제 #2
0
파일: vision.cpp 프로젝트: alon/burst
/**
 * 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");
}
예제 #3
0
/**
 * 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);
}
예제 #4
0
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);

}
예제 #5
0
/**
 * @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.");
}
예제 #6
0
파일: Sensors.cpp 프로젝트: kouretes/Monas
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;
}
예제 #7
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
}
예제 #8
0
/**
 * @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.");
}
예제 #9
0
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);
}
예제 #12
0
파일: vision.cpp 프로젝트: alon/burst
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 {
예제 #13
0
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);
}
예제 #14
0
/** 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);
}