示例#1
0
/*!

\brief Constructor.
\param[in] AppName String containing unit test name.
\param[in] argc Number of arguments in argv
\param[in] argv Array of char arrays containing the command line arguments.

*/
dmz::Test::Test (const String &AppName, int argc, char *argv[]) :
      obs (rt.get_context ()),
      config ("global"),
      log ("", rt.get_context ()),
      error (False) {

   const String LogLevelEnvStr (get_env ("DMZ_TEST_LOG_LEVEL"));
   LogLevelEnum level (LogLevelWarn);
   if (LogLevelEnvStr) { level = string_to_log_level (LogLevelEnvStr); }
   obs.set_level (level);

   log.out
      << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-" << endl
      << "Running " << AppName << " unit test." << endl
      << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-" << endl;

   CommandLine cl (argc, argv);
   CommandLineConfig clconfig;

   if (!clconfig.process_command_line (cl, config)) {

      log.error << "Unable to process command line: " << clconfig.get_error () << endl;
      error = True;
   }
}
示例#2
0
/*!

\brief Process the command line.
\details Loads all XML configuration files specified by the command line.
\param[in] CL CommandLine object to process.
\return Returns dmz::True if command line was successfully processed. Returns
dmz::False if there was an error parsing the XML configuration files.

*/
dmz::Boolean
dmz::Application::process_command_line (const CommandLine &CL) {

   CommandLineConfig clconfig;

   if (!clconfig.process_command_line (CL, _state.global, &(_state.log))) {

      _state.errorMsg.flush () << "Unable to process command line: "
         << clconfig.get_error ();

      _state.log.error << _state.errorMsg << endl;

      _state.error = True;
   }

   if (!_state.error) { _state.cache.configure (_state.global); }

//   format_config_to_xml (global, _state.log.debug, True);

   return !_state.error;
}
示例#3
0
void App::solveGazeProblem(){

  // Publish the query for visualisation in Director
  bot_core::pose_t goalMsg;
  goalMsg.utime = rstate_.utime;  
  goalMsg.pos[0] = cl_cfg_.gazeGoal(0);
  goalMsg.pos[1] = cl_cfg_.gazeGoal(1);
  goalMsg.pos[2] = cl_cfg_.gazeGoal(2);
  goalMsg.orientation[0] = 1;
  goalMsg.orientation[1] = 0;
  goalMsg.orientation[2] = 0;
  goalMsg.orientation[3] = 0;
  lcm_->publish("POSE_BODY_ALT", &goalMsg);

  // Solve the IK problem for the neck:
  VectorXd q_star(robotStateToDrakePosition(rstate_, dofMap_, model_.num_positions));
  VectorXd q_sol(model_.num_positions);
  int info = getConstraints(q_star, q_sol);
  if (info != 1) {
    std::cout << "Problem not solved\n";
    return;
  }


  bool mode = 0;
  if (mode==0){ // publish utorso-to-head as orientation, not properly tracking but works with different orientations
    // Get the utorso to head frame:
    int pelvis_link = model_.findLinkId("pelvis");
    int head_link = model_.findLinkId("head");
    int utorso_link = model_.findLinkId("torso");
    KinematicsCache<double> cache = model_.doKinematics(q_sol,0);
    Eigen::Isometry3d world_to_pelvis = matrixdToIsometry3d( model_.forwardKin(cache, Vector3d::Zero().eval(), pelvis_link, 0, 2, 0).value() );
    Eigen::Isometry3d world_to_head = matrixdToIsometry3d( model_.forwardKin(cache, Vector3d::Zero().eval(), head_link, 0, 2, 0).value() );
    Eigen::Isometry3d pelvis_to_head = world_to_head.inverse()*world_to_pelvis;
    Eigen::Isometry3d pelvis_to_utorso  = matrixdToIsometry3d( model_.forwardKin(cache, Vector3d::Zero().eval(), utorso_link, 0, 2, 0).value() ).inverse()*world_to_pelvis;
    Eigen::Isometry3d utorso_to_head =  pelvis_to_utorso.inverse()*pelvis_to_head;

    // Apply 180 roll as head orientation control seems to be in z-up frame
    Eigen::Isometry3d rotation_frame;
    rotation_frame.setIdentity();
    Eigen::Quaterniond quat = euler_to_quat( M_PI, 0, 0);
    rotation_frame.rotate(quat); 
    utorso_to_head = utorso_to_head*rotation_frame;

    bot_core::pose_t world_to_head_frame_pose_msg =  getPoseAsBotPose(world_to_head, rstate_.utime);
    lcm_->publish("POSE_VICON",&world_to_head_frame_pose_msg);// for debug
    bot_core::pose_t utorso_to_head_frame_pose_msg =  getPoseAsBotPose(utorso_to_head, rstate_.utime);
    lcm_->publish("DESIRED_HEAD_ORIENTATION",&utorso_to_head_frame_pose_msg);// temp
  }else{ // publish neck pitch and yaw joints as orientation. this works ok when robot is facing 1,0,0,0
    // Fish out the two neck joints (in simulation) and send as a command:
    std::vector<string> jointNames;
    for (int i=0 ; i <model_.num_positions ; i++){
      // std::cout << model.getPositionName(i) << " " << i << "\n";
      jointNames.push_back( model_.getPositionName(i) ) ;
    }
    drc::robot_state_t robot_state_msg;
    getRobotState(robot_state_msg, 0*1E6, q_sol , jointNames);
    lcm_->publish("CANDIDATE_ROBOT_ENDPOSE",&robot_state_msg);

    std::vector<std::string>::iterator it1 = find(jointNames.begin(),
        jointNames.end(), "lowerNeckPitch");
    int lowerNeckPitchIndex = std::distance(jointNames.begin(), it1);
    float lowerNeckPitchAngle = q_sol[lowerNeckPitchIndex];

    std::vector<std::string>::iterator it2 = find(jointNames.begin(),
        jointNames.end(), "neckYaw");
    int neckYawIndex = std::distance(jointNames.begin(), it2);
    float neckYawAngle = q_sol[neckYawIndex];

    std::cout << lowerNeckPitchAngle << " (" << lowerNeckPitchAngle*180.0/M_PI << ") is lowerNeckPitchAngle\n";
    std::cout << neckYawAngle << " (" << neckYawAngle*180.0/M_PI << ") is neckYawAngle\n";

    bot_core::pose_t headOrientationMsg;
    headOrientationMsg.utime = rstate_.utime;
    headOrientationMsg.pos[0] = 0;
    headOrientationMsg.pos[1] = 0;
    headOrientationMsg.pos[2] = 0;
    Eigen::Quaterniond quat = euler_to_quat(0, lowerNeckPitchAngle, neckYawAngle);
    headOrientationMsg.orientation[0] = quat.w();
    headOrientationMsg.orientation[1] = quat.x();
    headOrientationMsg.orientation[2] = quat.y();
    headOrientationMsg.orientation[3] = quat.z();
    lcm_->publish("DESIRED_HEAD_ORIENTATION",&headOrientationMsg);
    lcm_->publish("POSE_VICON",&headOrientationMsg); // for debug
  }

  //std::cout << "Desired orientation sent, exiting\n";
  //exit(-1);
}