/*!*****************************************************************************
*******************************************************************************
\note setRealRobotOptions
\date June 1999
\remarks
allows parameter changes if SL runs for a real robot instead
of a simulation
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
void
setRealRobotOptions(void)
{
double w;
if (!real_robot_flag) {
sprintf(config_files[CONFIGFILES],"ConfigFilesSim.cf");
if (read_parameter_pool_double(config_files[PARAMETERPOOL],"foot_scale", &w)) {
MAG = w;
XMETATARSALINNER=(XMETATARSAL_ORG-XFOOTASYM)*MAG;
XMETATARSALOUTER=(XMETATARSAL_ORG+XFOOTASYM)*MAG;
XTOEINNER=(XTOE_ORG-XFOOTASYM)*MAG;
XTOEOUTER=(XTOE_ORG+XFOOTASYM)*MAG;
XHEELINNER=(XHEEL_ORG-XFOOTASYM)*MAG;
XHEELOUTER=(XHEEL_ORG+XFOOTASYM)*MAG;
YHEEL=YHEEL_ORG*MAG;
YMETATARSAL=YMETATARSAL_ORG*MAG;
YTOE=YTOE_ORG*MAG;
}
}
else
sprintf(config_files[CONFIGFILES],"ConfigFiles.cf");
// update the config file names
read_config_files(config_files[CONFIGFILES]);
return;
}
/*!*****************************************************************************
*******************************************************************************
\note display
\date August 7, 1992
\remarks
this function updates the OpenGL graphics
*******************************************************************************
Function Parameters: [in]=input,[out]=output
******************************************************************************/
void
display(void)
{
int i;
static SL_Jstate *state = joint_sim_state;
static SL_endeff *eff = endeff;
static SL_Cstate *basec = &base_state;
static SL_quat *baseo = &base_orient;
GLfloat objscolor[4]={(float)0.2,(float)0.2,(float)0.2,(float)1.0};
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
double w;
if (read_parameter_pool_double(config_files[PARAMETERPOOL],"contact_force_scale",&w))
fscale = w;
}
drawRobot(state,basec,baseo);
// the standard display functions for openGL
#include "SL_user_display_core.h"
drawCenterOfPressure();
drawCOG();
drawIMU();
drawFootSensor();
if (comsDisplay)
drawCOMs(state, eff, basec, baseo);
}
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_);
}
/*!*****************************************************************************
*******************************************************************************
\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 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;
}
