/*!**************************************************************************** ****************************************************************************** \note initOsc \date Aug. 2010 \remarks initializes the oscilloscope processing ***************************************************************************** Function Parameters: [in]=input,[out]=output none *****************************************************************************/ void initOsc(void) { int rc; char string[40]; char fname[100]; FILE *fp; if (no_graphics_flag) return; if (read_parameter_pool_int(config_files[PARAMETERPOOL],"osc_enabled", &rc)) osc_enabled = macro_sign(abs(rc)); if (osc_enabled) { addToMan("oscMenu","interactively change oscilloscope settings",oscMenu); sprintf(osc_vars_name,"%s_default.osc",servo_name); // make sure that the default exists sprintf(fname,"%s%s",PREFS,osc_vars_name); fp = fopen(fname,"a"); fclose(fp); sprintf(string,"osc_vars_%s_file",servo_name); if (read_parameter_pool_string(config_files[PARAMETERPOOL],string,fname)) strcpy(osc_vars_name,fname); readOscVarsScript(osc_vars_name,FALSE); } }
/*!***************************************************************************** ******************************************************************************* \note oscMenu \date Aug. 2010 \remarks a menu to configure the variables for the oscilloscope ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ static void oscMenu(void) { int aux = 2; static double time_window=5.0; static int periods_ad=2.0; double temp; int itemp; static char fname[100] = "default.script"; if (!osc_enabled) return; if (read_parameter_pool_double(config_files[PARAMETERPOOL],"osc_time_window_vars", &temp)) time_window = temp; if (read_parameter_pool_int(config_files[PARAMETERPOOL],"osc_periods_ad", &itemp)) periods_ad = itemp; AGAIN: printf("\n\n\nCHANGE OSCILLOSCOPE SETTINGS:\n\n"); printf(" Time Window of Plots ---> %d\n",1); printf(" Read Oscilloscope Variables ---> %d\n",2); printf(" Number of Periods in AD Plot ---> %d\n",3); printf(" Quit ---> q\n"); printf("\n"); if (!get_int(" ----> Input",aux,&aux)) return; if (aux > 3 || aux < 1) { goto AGAIN; } if (aux == 1) { if (get_double("Time Window of Plots [s]?\0",time_window,&temp)) if (temp > 0) { updateOscTimeWindow(temp); time_window = temp; } } else if (aux == 2) { ONCE_MORE: // get the file name if (!get_string("Name of the Oscilloscope Variables File\0",osc_vars_name,fname)) goto AGAIN; // try to read this file if (!readOscVarsScript(fname,TRUE)) goto ONCE_MORE; strcpy(osc_vars_name,fname); } else if (aux == 3) { if (get_int("Number of Periods in AD Plot?\0",periods_ad,&itemp)) if (itemp > 0) { updateOscPeriodsAD(itemp); periods_ad = itemp; } } goto AGAIN; }
HInvDynExample::HInvDynExample() : config_file_("HInvDynExampleConfig.cf"), contact_helper_(config_file_), hinvdyn_solver_(kinematics_, momentum_helper_, contact_helper_, endeff_kinematics_), dyn_eqs_(hinvdyn_solver_), cog_ctrl_(hinvdyn_solver_), left_foot_constr_(hinvdyn_solver_), right_foot_constr_(hinvdyn_solver_), joint_ctrl_(hinvdyn_solver_), left_frc_reg_(hinvdyn_solver_), right_frc_reg_(hinvdyn_solver_){ // stop data collection to avoid crashes stopcd(); //set the endeffector constraints for double support std::cout << "N_ENDEFFS : " << N_ENDEFFS << std::endl; for(int i=1; i<=N_ENDEFFS; ++i){ endeff_constraints_[i] = endeff[i]; for(int j=1; j<=6; ++j) endeff_constraints_[i].c[j] = 1; } for (int i = 1; i < N_DOFS; ++i) { std::cout << "joint_names " << joint_names[i] << " " << i << std::endl; } // initialize our helpers kinematics_.initialize(joint_state, base_state, base_orient,endeff_constraints_); momentum_helper_.initialize(); contact_helper_.initialize(&kinematics_); hinvdyn_solver_.initialize(); // read simulation parameters if(!read_parameter_pool_double(config_file_.c_str(),"push_force",&push_force_)) assert(false && "reading parameter push_force failed"); if(!read_parameter_pool_double(config_file_.c_str(),"push_dur",&push_dur_)) assert(false && "reading parameter push_dur failed"); // read ranks from config file if(!read_parameter_pool_int(config_file_.c_str(),"foot_constr_rank",&foot_constr_rank_)) assert(false && "reading parameter foot_constr_rank failed"); if(!read_parameter_pool_int(config_file_.c_str(),"joint_ctrl_rank",&joint_ctrl_rank_)) assert(false && "reading parameter joint_ctrl_rank failed"); if(!read_parameter_pool_int(config_file_.c_str(),"cog_ctrl_rank",&cog_ctrl_rank_)) assert(false && "reading parameter cog_ctrl_rank failed"); if(!read_parameter_pool_int(config_file_.c_str(),"frc_reg_rank",&frc_reg_rank_)) assert(false && "reading parameter frc_reg_rank failed"); // read PD gains from config file double buffer[N_DOFS+6+1]; if(!read_parameter_pool_double_array(config_file_.c_str(),"COG_P_GAINS",3,buffer)) assert(false && "reading parameter COG_P_GAINS failed"); cog_p_gains_ = Eigen::Map<Eigen::Vector3d>(&buffer[1]); if(!read_parameter_pool_double_array(config_file_.c_str(),"COG_D_GAINS",3,buffer)) assert(false && "reading parameter COG_D_GAINS failed"); cog_d_gains_ = Eigen::Map<Eigen::Vector3d>(&buffer[1]); if(!read_parameter_pool_double_array(config_file_.c_str(),"POSTURE_P_GAINS",N_DOFS+6,buffer)) assert(false && "reading parameter POSTURE_P_GAINS failed"); posture_p_gains_ = Eigen::Map<Eigen::Matrix<double, N_DOFS+6,1> >(&buffer[1]); if(!read_parameter_pool_double_array(config_file_.c_str(),"POSTURE_D_GAINS",N_DOFS+6,buffer)) assert(false && "reading parameter POSTURE_D_GAINS failed"); posture_d_gains_ = Eigen::Map<Eigen::Matrix<double, N_DOFS+6,1> >(&buffer[1]); // read weights from config file if(!read_parameter_pool_double_array(config_file_.c_str(),"FOOT_CONSTR_WEIGHT", 6, buffer)) assert(false && "reading parameter FOOT_CONSTR_WEIGHT failed"); foot_constr_weight_ = Eigen::Map<Eigen::Matrix<double, 6,1> >(&buffer[1]); if(!read_parameter_pool_double_array(config_file_.c_str(),"COG_CTRL_WEIGHT", 6, buffer)) assert(false && "reading parameter COG_CTRL_WEIGHT failed"); cog_ctrl_weight_ = Eigen::Map<Eigen::Matrix<double, 6,1> >(&buffer[1]); if(!read_parameter_pool_double_array(config_file_.c_str(),"FRC_REG_WEIGHT", 6, buffer)) assert(false && "reading parameter FRC_REG_WEIGHT failed"); frc_reg_weight_ = Eigen::Map<Eigen::Matrix<double, 6,1> >(&buffer[1]); if(!read_parameter_pool_double_array(config_file_.c_str(),"JOINT_CTRL_WEIGHT", N_DOFS+6, buffer)) assert(false && "reading parameter JOINT_CTRL_WEIGHT failed"); joint_ctrl_weight_ = Eigen::Map<Eigen::Matrix<double, N_DOFS+6,1> >(&buffer[1]); // setup task composers dyn_eqs_.initialize(0); right_foot_constr_.initialize(foot_constr_rank_, link2endeffmap[RIGHT_FOOT]); right_foot_constr_.weightingMat() = foot_constr_weight_.asDiagonal(); left_foot_constr_.initialize(foot_constr_rank_, link2endeffmap[LEFT_FOOT]); left_foot_constr_.weightingMat() = foot_constr_weight_.asDiagonal(); cog_ctrl_.initialize(cog_ctrl_rank_); cog_ctrl_.weightingMat() = cog_ctrl_weight_.asDiagonal(); joint_ctrl_.initialize(joint_ctrl_rank_); joint_ctrl_.weightingMat() = joint_ctrl_weight_.asDiagonal(); right_frc_reg_.initialize(frc_reg_rank_, RIGHT_FOOT, true); right_frc_reg_.weightingMat() = frc_reg_weight_.asDiagonal(); left_frc_reg_.initialize(frc_reg_rank_, LEFT_FOOT, true); left_frc_reg_.weightingMat() = frc_reg_weight_.asDiagonal(); // subscribe our controllers and constraints to the hierarchical solver hinvdyn_solver_.sub_cost_composers_.push_back( static_cast<HierarchAffineCost*>(&dyn_eqs_)); hinvdyn_solver_.sub_cost_composers_.push_back( static_cast<HierarchAffineCost*>(&joint_ctrl_)); hinvdyn_solver_.sub_cost_composers_.push_back( static_cast<HierarchAffineCost*>(&cog_ctrl_)); hinvdyn_solver_.sub_cost_composers_.push_back( static_cast<HierarchAffineCost*>(&right_foot_constr_)); hinvdyn_solver_.sub_cost_composers_.push_back( static_cast<HierarchAffineCost*>(&left_foot_constr_)); hinvdyn_solver_.sub_cost_composers_.push_back( static_cast<HierarchAffineCost*>(&right_frc_reg_)); hinvdyn_solver_.sub_cost_composers_.push_back( static_cast<HierarchAffineCost*>(&left_frc_reg_)); // get some initial states of the robot for tracking cog_des_ = kinematics_.cog(); default_posture_ = kinematics_.generalizedJointPositions(); for(int i=1; i<=N_DOFS; ++i){ default_posture_[i-1] = joint_des_state[i].th; std::string varname = std::string(joint_names[i]); varname.append("_ref_th"); std::cout << "adding variable " << varname << std::endl; addVarToCollect((char *)&(default_posture_[i-1]), varname.c_str(),"rad",DOUBLE,TRUE); } // register variables for data collection addVarToCollect((char *)&(cog_des_[0]), "cog_des_x","m",DOUBLE,TRUE); addVarToCollect((char *)&(cog_des_[1]), "cog_des_y","m",DOUBLE,TRUE); addVarToCollect((char *)&(cog_des_[2]), "cog_des_z","m",DOUBLE,TRUE); addVarToCollect((char *)&(cog_ref_[0]), "cog_ref_ddx","m/s^2",DOUBLE,TRUE); addVarToCollect((char *)&(cog_ref_[1]), "cog_ref_ddy","m/s^2",DOUBLE,TRUE); addVarToCollect((char *)&(cog_ref_[2]), "cog_ref_ddz","m/s^2",DOUBLE,TRUE); // update SL data collection and start collecting data updateDataCollectScript(); scd(); std::cout << "Initialization done." << std::endl; task_start_time_ = task_servo_time; for(int i=1; i<=N_DOFS; ++i){ init_joint_state_uff_[i-1] = joint_des_state[i].uff; init_joint_state_th_[i-1] = joint_des_state[i].th; init_joint_state_thd_[i-1] = joint_des_state[i].thd; init_joint_state_thdd_[i-1] = joint_des_state[i].thdd; } }
/*!***************************************************************************** ******************************************************************************* \note init_ros_servo \date Dec. 1997 \remarks initializes servo specific things ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ void init_ros_servo(void) { int j, i; char string[100]; if (ros_servo_initialized) { printf("Task Servo is already initialized\n"); return; } // the servo name sprintf(servo_name,"ros"); // initialize shared memories and shared semaphores if (!init_shared_memory()) return; /* inverse dynamics and other basic kinematic stuff*/ if (!init_dynamics()) return; // initialize kinematics init_kinematics(); // add variables to data collection ros_servo_rate=servo_base_rate/task_servo_ratio; initCollectData(ros_servo_rate); for (i=1; i<=n_dofs; ++i) { printf("%d...",i); fflush(stdout); sprintf(string,"%s_th",joint_names[i]); addVarToCollect((char *)&(joint_state[i].th),string,(char *)"rad", DOUBLE,FALSE); sprintf(string,"%s_thd",joint_names[i]); addVarToCollect((char *)&(joint_state[i].thd),string,(char *)"rad/s", DOUBLE,FALSE); sprintf(string,"%s_thdd",joint_names[i]); addVarToCollect((char *)&(joint_state[i].thdd),string,(char *)"rad/s^2", DOUBLE,FALSE); sprintf(string,"%s_u",joint_names[i]); addVarToCollect((char *)&(joint_state[i].u),string,(char *)"Nm", DOUBLE,FALSE); sprintf(string,"%s_ufb",joint_names[i]); addVarToCollect((char *)&(joint_state[i].ufb),string,(char *)"Nm", DOUBLE,FALSE); sprintf(string,"%s_load",joint_names[i]); addVarToCollect((char *)&(joint_state[i].load),string,(char *)"Nm", DOUBLE,FALSE); sprintf(string,"%s_des_th",joint_names[i]); addVarToCollect((char *)&(joint_des_state[i].th),string,(char *)"rad",DOUBLE,FALSE); sprintf(string,"%s_des_thd",joint_names[i]); addVarToCollect((char *)&(joint_des_state[i].thd),string,(char *)"rad/s",DOUBLE,FALSE); sprintf(string,"%s_des_thdd",joint_names[i]); addVarToCollect((char *)&(joint_des_state[i].thdd),string,(char *)"rad/s^2",DOUBLE,FALSE); sprintf(string,"%s_uff",joint_names[i]); addVarToCollect((char *)&(joint_des_state[i].uff),string,(char *)"Nm",DOUBLE,FALSE); } // Cartesian variables for (i=1; i<=n_endeffs; ++i) { sprintf(string,"%s_x",cart_names[i]); addVarToCollect((char *)&(cart_state[i].x[_X_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_y",cart_names[i]); addVarToCollect((char *)&(cart_state[i].x[_Y_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_z",cart_names[i]); addVarToCollect((char *)&(cart_state[i].x[_Z_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_xd",cart_names[i]); addVarToCollect((char *)&(cart_state[i].xd[_X_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_yd",cart_names[i]); addVarToCollect((char *)&(cart_state[i].xd[_Y_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_zd",cart_names[i]); addVarToCollect((char *)&(cart_state[i].xd[_Z_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_xdd",cart_names[i]); addVarToCollect((char *)&(cart_state[i].xdd[_X_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_ydd",cart_names[i]); addVarToCollect((char *)&(cart_state[i].xdd[_Y_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_zdd",cart_names[i]); addVarToCollect((char *)&(cart_state[i].xdd[_Z_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_ad",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].ad[_X_]),string,(char *)"rad/s",DOUBLE,FALSE); sprintf(string,"%s_bd",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].ad[_Y_]),string,(char *)"rad/s",DOUBLE,FALSE); sprintf(string,"%s_gd",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].ad[_Z_]),string,(char *)"rad/s",DOUBLE,FALSE); sprintf(string,"%s_add",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].add[_X_]),string,(char *)"rad/s2",DOUBLE,FALSE); sprintf(string,"%s_bdd",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].add[_Y_]),string,(char *)"rad/s2",DOUBLE,FALSE); sprintf(string,"%s_gdd",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].add[_Z_]),string,(char *)"rad/s2",DOUBLE,FALSE); sprintf(string,"%s_des_x",cart_names[i]); addVarToCollect((char *)&(cart_des_state[i].x[_X_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_des_y",cart_names[i]); addVarToCollect((char *)&(cart_des_state[i].x[_Y_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_des_z",cart_names[i]); addVarToCollect((char *)&(cart_des_state[i].x[_Z_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_des_xd",cart_names[i]); addVarToCollect((char *)&(cart_des_state[i].xd[_X_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_des_yd",cart_names[i]); addVarToCollect((char *)&(cart_des_state[i].xd[_Y_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_des_zd",cart_names[i]); addVarToCollect((char *)&(cart_des_state[i].xd[_Z_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_des_xdd",cart_names[i]); addVarToCollect((char *)&(cart_des_state[i].xdd[_X_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_des_ydd",cart_names[i]); addVarToCollect((char *)&(cart_des_state[i].xdd[_Y_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_des_zdd",cart_names[i]); addVarToCollect((char *)&(cart_des_state[i].xdd[_Z_]),string,(char *)"m",DOUBLE,FALSE); sprintf(string,"%s_des_ad",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].ad[_X_]),string,(char *)"rad/s",DOUBLE,FALSE); sprintf(string,"%s_des_bd",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].ad[_Y_]),string,(char *)"rad/s",DOUBLE,FALSE); sprintf(string,"%s_des_gd",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].ad[_Z_]),string,(char *)"rad/",DOUBLE,FALSE); sprintf(string,"%s_q0",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].q[_Q0_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_q1",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].q[_Q1_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_q2",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].q[_Q2_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_q3",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].q[_Q3_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_q0d",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].qd[_Q0_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_q1d",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].qd[_Q1_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_q2d",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].qd[_Q2_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_q3d",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].qd[_Q3_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_q0dd",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].qdd[_Q0_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_q1dd",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].qdd[_Q1_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_q2dd",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].qdd[_Q2_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_q3dd",cart_names[i]); addVarToCollect((char *)&(cart_orient[i].qdd[_Q3_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_des_q0",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].q[_Q0_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_des_q1",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].q[_Q1_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_des_q2",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].q[_Q2_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_des_q3",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].q[_Q3_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_des_q0d",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].qd[_Q0_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_des_q1d",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].qd[_Q1_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_des_q2d",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].qd[_Q2_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_des_q3d",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].qd[_Q3_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_des_q0dd",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q0_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_des_q1dd",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q1_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_des_q2dd",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q2_]),string,(char *)"-",DOUBLE,FALSE); sprintf(string,"%s_des_q3dd",cart_names[i]); addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q3_]),string,(char *)"-",DOUBLE,FALSE); } // misc sensors for (i=1; i<=n_misc_sensors; ++i) { sprintf(string,"%s",misc_sensor_names[i]); addVarToCollect((char *)&(misc_sensor[i]),string,(char *)"-",DOUBLE,FALSE); } // the state of the base addVarToCollect((char *)&(base_state.x[_X_]),(char *)"base_x",(char *)"m",DOUBLE,FALSE); addVarToCollect((char *)&(base_state.x[_Y_]),(char *)"base_y",(char *)"m",DOUBLE,FALSE); addVarToCollect((char *)&(base_state.x[_Z_]),(char *)"base_z",(char *)"m",DOUBLE,FALSE); addVarToCollect((char *)&(base_state.xd[_X_]),(char *)"base_xd",(char *)"m",DOUBLE,FALSE); addVarToCollect((char *)&(base_state.xd[_Y_]),(char *)"base_yd",(char *)"m",DOUBLE,FALSE); addVarToCollect((char *)&(base_state.xd[_Z_]),(char *)"base_zd",(char *)"m",DOUBLE,FALSE); addVarToCollect((char *)&(base_state.xdd[_X_]),(char *)"base_xdd",(char *)"m",DOUBLE,FALSE); addVarToCollect((char *)&(base_state.xdd[_Y_]),(char *)"base_ydd",(char *)"m",DOUBLE,FALSE); addVarToCollect((char *)&(base_state.xdd[_Z_]),(char *)"base_zdd",(char *)"m",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.q[_Q0_]),(char *)"base_q0",(char *)"-",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.q[_Q1_]),(char *)"base_q1",(char *)"-",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.q[_Q2_]),(char *)"base_q2",(char *)"-",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.q[_Q3_]),(char *)"base_q3",(char *)"-",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.qd[_Q0_]),(char *)"base_qd0",(char *)"-",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.qd[_Q1_]),(char *)"base_qd1",(char *)"-",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.qd[_Q2_]),(char *)"base_qd2",(char *)"-",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.qd[_Q3_]),(char *)"base_qd3",(char *)"-",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.ad[_A_]),(char *)"base_ad",(char *)"-",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.ad[_B_]),(char *)"base_bd",(char *)"-",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.ad[_G_]),(char *)"base_gd",(char *)"-",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.add[_A_]),(char *)"base_add",(char *)"-",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.add[_B_]),(char *)"base_bdd",(char *)"-",DOUBLE,FALSE); addVarToCollect((char *)&(base_orient.add[_G_]),(char *)"base_gdd",(char *)"-",DOUBLE,FALSE); printf("done\n"); updateDataCollectScript(); // add to man pages addToMan((char *)"status",(char *)"displays information about the servo",status); addToMan((char *)"drs",(char *)"disables the ros servo",drs); int argc = 1; char name[] = "SL2ROS_Publisher"; char* argv[1]; argv[0] = name; ros::init(argc, argv, "SL2ROS_Publisher"); if (!read_parameter_pool_int(config_files[PARAMETERPOOL], "default_publisher_enabled", &default_publisher_enabled_)) default_publisher_enabled_ = 0; if(default_publisher_enabled_ && !ros_communicator_.initialize()) { printf("ERROR: could not initialize ros communication\n"); return; } // initializes user specific issues init_user_ros(); // if all worked out, we mark the servo as ready to go ros_servo_initialized = TRUE; // set oscilloscope to start value initOsc(); setOsc(d2a_cr,0.0); scd(); return; }
/*!***************************************************************************** ******************************************************************************* \note initNI6259 \date Nov. 2009 \remarks Initializes the communication with the DAQ ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] ad_only_flag: only initialize A/D convert \return TRUE for sucess, or FALSE if there was a problem ******************************************************************************/ int init_NI6259(int ad_only_flag) { int i,rc; static int ni6259_initialized = FALSE; unsigned int data[10]; a4l_insn_t insn; int real_osc_enabled = FALSE; if (ni6259_initialized) return TRUE; // Open the device rc = a4l_open(&desc, DEVICE); if (rc < 0) { printf("initNI6259: a4l_open %s failed (rc=%d)\n",DEVICE, rc); return FALSE; } // Fill the descriptor completely, as a4l_open only does a partial job. // First, allocate a buffer so as to get more info (subd, chan, rng) desc.sbdata = malloc(desc.sbsize); if (desc.sbdata == NULL) { printf("initNI6259: malloc failed \n"); return FALSE; } // Second, get the data rc = a4l_fill_desc(&desc); if (rc < 0) { printf("initNI6259: a4l_fill_desc %s failed (rc=%d)\n",DEVICE, rc); return FALSE; } // activate the analog outputs for the oscilloscope debugging if (read_parameter_pool_int(config_files[PARAMETERPOOL],"real_osc_enabled", &rc)) real_osc_enabled = rc; if (real_osc_enabled) setD2AFunction(d2a_NI6259); if (!ad_only_flag) { // plot some output about device //printDeviceInfo(&desc); // enable the counter clock // makeCounterClock(1000, FALSE); // route DIO16 (PFI) to PFI channels for (i=0; i<16; ++i) { insn.type = A4L_INSN_CONFIG; insn.idx_subd = SUBDEV_PFI; insn.chan_desc = i; insn.data_size = sizeof(data[0])*2; data[0] = A4L_INSN_CONFIG_SET_ROUTING; data[1] = NI_PFI_OUTPUT_PFI_DO; insn.data = data; rc = a4l_snd_insn(&desc, &insn); if (rc < 0) fprintf(stderr,"re-route PFI channels: routing of channe %d failed (rc=%d)\n",i,rc); } // set DIO32 channels to READ mode by default dio32_NI6259_config(0xffffffff, DIO_READ); // set DIO16 channels to READ mode by default dio16_NI6259_config(0xffffffff, DIO_READ); } // all done ni6259_initialized = TRUE; return TRUE; }
/*!***************************************************************************** ******************************************************************************* \note initOscWindow \date May 2010 \remarks initializes the oscilloscope window ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ int initOscWindow(void) { int i,j; int x = 600; int y = 20; int width = 600; int height = 400; char string[100]; char xstring[100]; Display *disp; int screen_num; int display_width; int display_height; double aux; // is the oscilloscope enabled? if (read_parameter_pool_int(config_files[PARAMETERPOOL],"osc_enabled", &i)) osc_enabled = macro_sign(abs(i)); if (!osc_enabled) return TRUE; // how many oscilloscope graphs? if (read_parameter_pool_int(config_files[PARAMETERPOOL],"n_osc_plots", &i)) { if (i > 0) n_oscilloscope_plots = i; } // #periods of oscilloscope A/D plot if (read_parameter_pool_int(config_files[PARAMETERPOOL],"osc_periods_ad", &i)) { if (i > 0) periods_window_AD = i; } // window size of variable display if (read_parameter_pool_double(config_files[PARAMETERPOOL],"osc_time_window_vars", &aux)) { if (aux > 0) time_window_vars = aux; } // allocate memory for the plots and initialize osc_data = (OscData *) my_calloc(n_oscilloscope_plots+1,sizeof(OscData),MY_STOP); for (i=0; i<=n_oscilloscope_plots; ++i) { for (j=1; j<=MAX_VARS_PER_PLOT; ++j) { osc_data[i].current_index[j] = 1; } osc_data[i].max = -1.e10; osc_data[i].min = 1.e10; } // connect to X server using the DISPLAY environment variable if ( (disp=XOpenDisplay(NULL)) == NULL ) { printf("Cannot connect to X servo %s\n",XDisplayName(NULL)); exit(-1); } // get screen size from display structure macro screen_num = DefaultScreen(disp); display_width = DisplayWidth(disp, screen_num); display_height = DisplayHeight(disp, screen_num); // overwrite the default window options from ParameterPool.cf if (read_parameter_pool_string(config_files[PARAMETERPOOL], "osc_window_geometry", string)) parseWindowSpecs(string, display_width,display_height,xstring, &x, &y, &width, &height); // create the window glutInitWindowPosition(x,y); glutInitWindowSize(width,height); openGLId_osc = glutCreateWindow("Oscilloscope"); // makes window current, too // attach appropriate OpenGL functions to the current window glutDisplayFunc(osc_display); glutReshapeFunc(osc_reshape); /* glutIdleFunc(idle); glutKeyboardFunc(keyboard); glutMouseFunc(mouse); glutMotionFunc(motion); glutSpecialFunc(special); glutMenu(wptr); */ return TRUE; }