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