void BaseStateEstimation::initialize(const double* imu_quaternion,
const double* imu_angrate,
const double* imu_linacc, int update_rate)
{
update_rate_ = update_rate;
imu_quaternion_ = imu_quaternion;
imu_angrate_ = imu_angrate;
imu_linacc_ = imu_linacc;
double row_first_mat[10];
read_parameter_pool_double_array("bs_est_config.cf", "IMU_BASE_ROT",
N_CART*N_CART, row_first_mat);
for(int i=0;i<9;i++)
I_rot_B_[i/3+1][i%3+1] = row_first_mat[i+1];
read_parameter_pool_double_array("bs_est_config.cf", "IMU_BASE_TRANS",
N_CART, I_linpos_B_);
// read_parameter_pool_double_array("bs_est_config.cf", "MISC_SENSOR_GRAV",
// N_CART, gravity_);
read_parameter_pool_double("bs_est_config.cf", "LEAKAGE_FACTOR",
&leakage_factor_);
}
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 createWindows
\date July 1998
\remarks
initializes graphic windows
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
createWindows(void)
{
int i;
OpenGLWPtr w;
int width_eye=640/3;
int height_eye=480/3;
int height_main = 400;
int width_main = 400;
int hspace = 10;
int vspace = 60;
Display *disp;
int screen_num;
int display_width;
int display_height;
char string[100];
char xstring[100];
double eye[N_CART+1];
// 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);
/* get a window structure, initialized with default values */
w=getOpenGLWindow();
if (w==NULL)
return FALSE;
w->display = display;
w->idle = idle;
w->width = width_main;
w->height = height_main;
// check for user parameters
if (read_parameter_pool_string(config_files[PARAMETERPOOL],
"main_window_geometry", string))
parseWindowSpecs(string, display_width,display_height,xstring,
&(w->x),
&(w->y),
&(w->width),
&(w->height));
if (read_parameter_pool_double_array(config_files[PARAMETERPOOL],
"main_window_camera_pos", N_CART, eye))
for (i=1; i<=N_CART; ++i)
w->eye[i] = eye[i];
// finally create the window
for (i=1; i<=N_CART; ++i)
w->eye0[i] = w->eye[i];
if (!createWindow(w))
return FALSE;
/* get additional windows for the eyes */
w=getOpenGLWindow();
if (w==NULL)
return FALSE;
w->x += width_main+hspace;
w->fovea = FOVEA_ANGLE;
w->width = width_eye;
w->height = height_eye;
w->display = display;
w->idle = idle;
sprintf(w->name,"LeftEyeFovea");
if (!createWindow(w))
return FALSE;
toggleHideWindow(w);
w_left_eye_fovea = w;
w=getOpenGLWindow();
if (w==NULL)
return FALSE;
w->x += width_main+hspace+width_eye+hspace;
w->fovea = FOVEA_ANGLE;
w->width = width_eye;
w->height = height_eye;
w->display = display;
w->idle = idle;
sprintf(w->name,"RightEyeFovea");
if (!createWindow(w))
return FALSE;
toggleHideWindow(w);
w_right_eye_fovea = w;
w=getOpenGLWindow();
if (w==NULL)
return FALSE;
w->x += width_main+hspace;
w->y += height_eye+vspace;
w->fovea = WIDE_ANGLE;
w->width = width_eye;
w->height = height_eye;
w->display = display;
w->idle = idle;
sprintf(w->name,"LeftEyeWide");
if (!createWindow(w))
return FALSE;
toggleHideWindow(w);
w_left_eye_wide = w;
w=getOpenGLWindow();
if (w==NULL)
return FALSE;
w->x += width_main+hspace+width_eye+hspace;
w->y += height_eye+vspace;
w->fovea = WIDE_ANGLE;
w->width = width_eye;
w->height = height_eye;
w->display = display;
w->idle = idle;
sprintf(w->name,"RightEyeWide");
if (!createWindow(w))
return FALSE;
toggleHideWindow(w);
w_right_eye_wide = w;
return TRUE;
}
