void click ()
{
int game = choice ("&savegameinfo;",
"&savegameinfo;",
"&savegameinfo;",
"&savegameinfo;",
"&savegameinfo;",
"&savegameinfo;",
"&savegameinfo;",
"&savegameinfo;",
"&savegameinfo;",
"&savegameinfo;",
"Nevermind");
if (game == 11 || !game_exist (game))
return;
stopmidi ();
stopcd ();
load_game (game);
kill_this_task ();
}
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;
}
}
void load( void )
{
Playsound(18,22050,0,0,0);
choice_start();
"&savegameinfo"
"&savegameinfo"
"&savegameinfo"
"&savegameinfo"
"&savegameinfo"
"&savegameinfo"
"&savegameinfo"
"&savegameinfo"
"&savegameinfo"
"&savegameinfo"
"Nevermind"
choice_end();
if (&result == 11)
return;
if (game_exist(&result) == 0)
return;
stopmidi();
stopcd();
sp_active(1, 1);
sp_x(1, 334);
sp_y(1, 161);
sp_base_walk(1, 70);
sp_base_attack(1, 100);
sp_dir(1, 4);
sp_brain(1, 1);
sp_que(1, 0);
sp_noclip(1, 0);
set_mode(2);
//script now can't die when the load is preformed..
init("load_sequence_now graphics\dink\walk\ds-w1- 71 43 38 72 -14 -9 14 9");
init("load_sequence_now graphics\dink\walk\ds-w2- 72 43 37 69 -13 -9 13 9");
init("load_sequence_now graphics\dink\walk\ds-w3- 73 43 38 72 -14 -9 14 9");
init("load_sequence_now graphics\dink\walk\ds-w4- 74 43 38 72 -12 -9 12 9");
init("load_sequence_now graphics\dink\walk\ds-w6- 76 43 38 72 -13 -9 13 9");
init("load_sequence_now graphics\dink\walk\ds-w7- 77 43 38 72 -12 -10 12 10");
init("load_sequence_now graphics\dink\walk\ds-w8- 78 43 37 69 -13 -9 13 9");
init("load_sequence_now graphics\dink\walk\ds-w9- 79 43 38 72 -14 -9 14 9");
init("load_sequence_now graphics\dink\idle\ds-i2- 12 250 33 70 -12 -9 12 9");
init("load_sequence_now graphics\dink\idle\ds-i4- 14 250 30 71 -11 -9 11 9");
init("load_sequence_now graphics\dink\idle\ds-i6- 16 250 36 70 -11 -9 11 9");
init("load_sequence_now graphics\dink\idle\ds-i8- 18 250 32 68 -12 -9 12 9");
init("load_sequence_now graphics\dink\hit\normal\ds-h2- 102 75 60 72 -19 -9 19 9");
init("load_sequence_now graphics\dink\hit\normal\ds-h4- 104 75 61 73 -19 -10 19 10");
init("load_sequence_now graphics\dink\hit\normal\ds-h6- 106 75 58 71 -18 -10 18 10");
init("load_sequence_now graphics\dink\hit\normal\ds-h8- 108 75 61 71 -19 -10 19 10");
load_game(&result);
kill_this_task();
}
