/*! \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; } }
/*! \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; }
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); }