/*****************************************************************************
******************************************************************************
Function Name : add_test_sine_task
Date : Feb 1999
Remarks:
adds the task to the task menu
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
void
add_test_sine_task( void )
{
int i, j;
static int firsttime = TRUE;
char string[100];
if (firsttime) {
firsttime = FALSE;
off = my_vector(1,n_dofs);
amp = my_matrix(1,n_dofs,1,MAX_SINE);
phase = my_matrix(1,n_dofs,1,MAX_SINE);
freq = my_matrix(1,n_dofs,1,MAX_SINE);
n_sine = my_ivector(1,n_dofs);
target = (SL_DJstate *)my_calloc(n_dofs+1,sizeof(SL_DJstate),MY_STOP);
addTask("Test Sine Task", init_test_sine_task,run_test_sine_task, change_test_sine_task);
for (i=1; i<=N_DOFS; ++i) {
sprintf(string,"%s_tar_th",joint_names[i]);
addVarToCollect((char *)&(target[i].th),string,"rad", DOUBLE,FALSE);
sprintf(string,"%s_tar_thd",joint_names[i]);
addVarToCollect((char *)&(target[i].thd),string,"rad/s", DOUBLE,FALSE);
sprintf(string,"%s_tar_thdd",joint_names[i]);
addVarToCollect((char *)&(target[i].thdd),string,"rad/s^2", DOUBLE,FALSE);
}
updateDataCollectScript();
}
}
/*!*****************************************************************************
*******************************************************************************
\note init_user_task
\date Nov. 2007
\remarks
initializes user task functionality
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
init_user_task(void)
{
int i,j;
addToMan("toggleSimBase","toggles the use of the simulated base state on/off",
toggleSimulatedBaseState);
initialize_base_state_estimation(&(misc_sensor[B_Q0_IMU]), &(misc_sensor[B_AD_A_IMU]),
&(misc_sensor[B_XACC_IMU]), task_servo_rate);
init_state_est_lin_task();
if(!raw_data.init())
return FALSE;
//data collect for raw joint states and misc sensors
char string[100];
for (i=1; i<=n_dofs; ++i) {
sprintf(string,"%s_raw_th",joint_names[i]);
addVarToCollect((char *)&(raw_joint_state[i].th),string,"rad", DOUBLE,FALSE);
sprintf(string,"%s_raw_thd",joint_names[i]);
addVarToCollect((char *)&(raw_joint_state[i].thd),string,"rad/s", DOUBLE,FALSE);
sprintf(string,"%s_raw_thdd",joint_names[i]);
addVarToCollect((char *)&(raw_joint_state[i].thdd),string,"rad/s^2", DOUBLE,FALSE);
// sprintf(string,"%s_raw_u",joint_names[i]);
// addVarToCollect((char *)&(raw_joint_state[i].u),string,"Nm", DOUBLE,FALSE);
// sprintf(string,"%s_raw_ufb",joint_names[i]);
// addVarToCollect((char *)&(raw_joint_state[i].ufb),string,"Nm", DOUBLE,FALSE);
sprintf(string,"%s_raw_load",joint_names[i]);
addVarToCollect((char *)&(raw_joint_state[i].load),string,"Nm", DOUBLE,FALSE);
}
for (i=1; i<=n_misc_sensors; ++i) {
sprintf(string,"%s_raw",misc_sensor_names[i]);
addVarToCollect((char *)&(raw_misc_sensor[i]),string,"-",DOUBLE,FALSE);
}
updateDataCollectScript();
return TRUE;
}
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 initDataCollection
\date Feb. 2004
\remarks
initializes all variables for data collection
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static void
initDataCollection(void)
{
int i;
char string[100];
/* this will be updated later by the servos */
initCollectData(simulation_servo_rate);
/* add variables to data collection */
for (i=1; i<=n_dofs; ++i) {
printf("%d...",i);
fflush(stdout);
sprintf(string,"%s_th",joint_names[i]);
addVarToCollect((char *)&(joint_sim_state[i].th),string,"rad", DOUBLE,FALSE);
sprintf(string,"%s_thd",joint_names[i]);
addVarToCollect((char *)&(joint_sim_state[i].thd),string,"rad/s", DOUBLE,FALSE);
sprintf(string,"%s_thdd",joint_names[i]);
addVarToCollect((char *)&(joint_sim_state[i].thdd),string,"rad/s^2", DOUBLE,FALSE);
sprintf(string,"%s_u",joint_names[i]);
addVarToCollect((char *)&(joint_sim_state[i].u),string,"Nm", DOUBLE,FALSE);
sprintf(string,"%s_ufb",joint_names[i]);
addVarToCollect((char *)&(joint_sim_state[i].ufb),string,"Nm", DOUBLE,FALSE);
sprintf(string,"%s_load",joint_names[i]);
addVarToCollect((char *)&(joint_sim_state[i].load),string,"Nm", DOUBLE,FALSE);
sprintf(string,"%s_cfx",joint_names[i]);
addVarToCollect((char *)&(ucontact[i].f[_X_]),string,"N", DOUBLE,FALSE);
sprintf(string,"%s_cfy",joint_names[i]);
addVarToCollect((char *)&(ucontact[i].f[_Y_]),string,"N", DOUBLE,FALSE);
sprintf(string,"%s_cfz",joint_names[i]);
addVarToCollect((char *)&(ucontact[i].f[_Z_]),string,"N", DOUBLE,FALSE);
sprintf(string,"%s_ctx",joint_names[i]);
addVarToCollect((char *)&(ucontact[i].t[_A_]),string,"Nm", DOUBLE,FALSE);
sprintf(string,"%s_cty",joint_names[i]);
addVarToCollect((char *)&(ucontact[i].t[_B_]),string,"Nm", DOUBLE,FALSE);
sprintf(string,"%s_ctz",joint_names[i]);
addVarToCollect((char *)&(ucontact[i].t[_G_]),string,"Nm", DOUBLE,FALSE);
}
/* the state of the base */
addVarToCollect((char *)&(base_state.x[_X_]),"base_x","m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.x[_Y_]),"base_y","m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.x[_Z_]),"base_z","m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xd[_X_]),"base_xd","m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xd[_Y_]),"base_yd","m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xd[_Z_]),"base_zd","m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xdd[_X_]),"base_xdd","m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xdd[_Y_]),"base_ydd","m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xdd[_Z_]),"base_zdd","m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.q[_Q0_]),"base_q0","-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.q[_Q1_]),"base_q1","-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.q[_Q2_]),"base_q2","-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.q[_Q3_]),"base_q3","-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.qd[_Q0_]),"base_qd0","-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.qd[_Q1_]),"base_qd1","-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.qd[_Q2_]),"base_qd2","-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.qd[_Q3_]),"base_qd3","-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.ad[_A_]),"base_ad","-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.ad[_B_]),"base_bd","-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.ad[_G_]),"base_gd","-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.add[_A_]),"base_add","-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.add[_B_]),"base_bdd","-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.add[_G_]),"base_gdd","-",DOUBLE,FALSE);
for (i=0; i<=n_contacts; ++i) {
sprintf(string,"CP%d_cstat",i);
addVarToCollect((char *)&(contacts[i].status),string,"-",INT,FALSE);
sprintf(string,"CP%d_cx",i);
addVarToCollect((char *)&(contacts[i].x[_X_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cy",i);
addVarToCollect((char *)&(contacts[i].x[_Y_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cz",i);
addVarToCollect((char *)&(contacts[i].x[_Z_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cnx",i);
addVarToCollect((char *)&(contacts[i].normal[_X_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cny",i);
addVarToCollect((char *)&(contacts[i].normal[_Y_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cnz",i);
addVarToCollect((char *)&(contacts[i].normal[_Z_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cnxd",i);
addVarToCollect((char *)&(contacts[i].normvel[_X_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cnyd",i);
addVarToCollect((char *)&(contacts[i].normvel[_Y_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cnzd",i);
addVarToCollect((char *)&(contacts[i].normvel[_Z_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_ctx",i);
addVarToCollect((char *)&(contacts[i].tangent[_X_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cty",i);
addVarToCollect((char *)&(contacts[i].tangent[_Y_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_ctz",i);
addVarToCollect((char *)&(contacts[i].tangent[_Z_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_ctxd",i);
addVarToCollect((char *)&(contacts[i].tanvel[_X_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_ctyd",i);
addVarToCollect((char *)&(contacts[i].tanvel[_Y_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_ctzd",i);
addVarToCollect((char *)&(contacts[i].tanvel[_Z_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cvxd",i);
addVarToCollect((char *)&(contacts[i].viscvel[_X_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cvyd",i);
addVarToCollect((char *)&(contacts[i].viscvel[_Y_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cvzd",i);
addVarToCollect((char *)&(contacts[i].viscvel[_Z_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cfx",i);
addVarToCollect((char *)&(contacts[i].f[_X_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cfy",i);
addVarToCollect((char *)&(contacts[i].f[_Y_]),string,"m",DOUBLE,FALSE);
sprintf(string,"CP%d_cfz",i);
addVarToCollect((char *)&(contacts[i].f[_Z_]),string,"m",DOUBLE,FALSE);
}
updateDataCollectScript();
}
/*!*****************************************************************************
*******************************************************************************
\note init_sensor_processing
\date May 2000
\remarks
Initializes all sensory processing
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
init_sensor_processing(void)
{
int i,j;
FILE *in;
char string[100];
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
joint_lin_rot = my_matrix(1,n_dofs,1,6);
pos_polar = my_vector(1,n_dofs);
load_polar = my_vector(1,n_dofs);
joint_raw_state = (SL_Jstate *)
my_calloc((unsigned long)(n_dofs+1),sizeof(SL_Jstate),MY_STOP);
misc_raw_sensor = (double *)
my_calloc((unsigned long)(n_misc_sensors+1),sizeof(double),MY_STOP);
fth = (Filter *)
my_calloc((unsigned long)(n_dofs+1),sizeof(Filter),MY_STOP);
fthd = (Filter *)
my_calloc((unsigned long)(n_dofs+1),sizeof(Filter),MY_STOP);
fthdd = (Filter *)
my_calloc((unsigned long)(n_dofs+1),sizeof(Filter),MY_STOP);
fload = (Filter *)
my_calloc((unsigned long)(n_dofs+1),sizeof(Filter),MY_STOP);
fmisc_sensor = (Filter *)
my_calloc((unsigned long)(n_misc_sensors+1),sizeof(Filter),MY_STOP);
}
/* initalizes translation to and from units */
if (!init_user_sensor_processing())
return FALSE;
/* initialize filtering */
if (!init_filters())
return FALSE;
/* read several variables from files */
/* first, get the calibration values for dealing with the linear to
rotary conversion */
if (!read_sensor_calibration(config_files[SENSORCALIBRATION],
joint_lin_rot,pos_polar,load_polar))
return FALSE;
/* second, get the max, min , and offsets of the position sensors */
if (!read_sensor_offsets(config_files[SENSOROFFSETS]))
return FALSE;
/* third, get the filter cutoff values for all variable */
if (!read_sensor_filters(config_files[SENSORFILTERS]))
return FALSE;
/* add function to man pages */
addToMan("where_off","sensor readings without offsets",where_off);
addToMan("where_raw","raw sensor readings",where_raw);
addToMan("monitor_min_max","records min/max values of sensors",monitor_min_max);
if (!real_robot_flag)
addToMan("toggle_filter","toggles sensor filtering on and off",toggle_filter);
/* make raw variables available for output */
for (i=1; i<=n_dofs; ++i) {
sprintf(string,"%s_rth",joint_names[i]);
addVarToCollect((char *)&(joint_raw_state[i].th),string,"rad", DOUBLE,FALSE);
sprintf(string,"%s_rthd",joint_names[i]);
addVarToCollect((char *)&(joint_raw_state[i].thd),string,"rad/s", DOUBLE,FALSE);
sprintf(string,"%s_rload",joint_names[i]);
addVarToCollect((char *)&(joint_raw_state[i].load),string,"Nm", DOUBLE,FALSE);
}
for (i=1; i<=n_misc_sensors; ++i) {
sprintf(string,"%s_r",misc_sensor_names[i]);
addVarToCollect((char *)&(misc_raw_sensor[i]),string,"-", DOUBLE,FALSE);
}
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note init_user_sensor_processing
\date Nov. 2007
\remarks
Initializes all user specific sensory processing
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int init_user_sensor_processing(void)
{
printf("2 was reached \n");
int i,j;
change_position_gains = false;
change_torque_gains = false;
change_valve_dac_bias = false;
change_dither = false;
use_feet = true;
for (i=1; i<global_argc; ++i)
{
if (strcmp(global_argv[i],"-no-imu")==0)
{
imu_mode=IMU_MODE_DONT_USE;
printf("WARNING >> Disabling IMU...\n");
}
if (strcmp(global_argv[i],"-no-feet")==0)
{
use_feet = false;
printf("WARNING >> Disabling Feet Sensors...\n");
}
}
int rc;
rc = rt_mutex_create(&gdc_mutex, NULL);
if(rc)
{
printf("Error init gdc mutex, error code %d\n", rc);
return FALSE;
}
if( (rc =rt_mutex_acquire(&gdc_mutex, TM_INFINITE)) )
{
printf("Error cannot acquire gdc mutex, error code %d\n", rc);
return FALSE;
}
//init the interface that deals with translating gdc values into SL friendly values
if(!gdc_sl_interface.initialize())
return FALSE;
//initialize the valve controller
if(!valve_controller.initialize())
{
printf("1 was reached \n");
return FALSE;
}
//initialize the GDC Network
if(!gdc_network.initialize("config/gdc_dof_config.cf"))
{
printf("cannot initialize GDC Network\n");
return FALSE;
}
//load the configuration from file and configure the cards
if(!gdc_network.loadGDCParameters("config/gdc_card_configuration.cf"))
{
printf("cannot load GDC configurations into cards\n");
return FALSE;
}
//get the current values of the GDC Cards (to update the actuals too)
hermes_communication_tools::GDCMsg tmp_msg;
printf("getting current values of GDC cards\n");
for(int i=0; i<(int)gdc_network.gdc_card_states_.size(); i++)
{
tmp_msg.readAllParameters();
gdc_network.sendMessage(tmp_msg, i);
}
RTIME sleep_time = 100000000;//in nanosec 100ms
rt_task_sleep(sleep_time);
gdc_network.checkForReceivedMessages();
//set multicast
printf("setting multicast \n");
if(!gdc_network.setMultiCast())
{
printf("error in setting the multicast\n");
return FALSE;
}
///// initialize cga_imu sensors
if(!imu_sensor.initialize()) {
printf("cga_imu sensor not initialized");
return FALSE;
}
//initialize imu communication
if(imu_mode != IMU_MODE_DONT_USE)
{
rt_printf("initializing imu communication\n");
switch (imu_mode) {
case IMU_MODE_RT_STREAM:
read_servoParameters(config_files[SERVOPARAMETERS],
imu_interface_rt.imu_comm_xeno_info_.keyword_,
&imu_interface_rt.imu_comm_xeno_info_.priority_,
&imu_interface_rt.imu_comm_xeno_info_.stacksize_,
&imu_interface_rt.imu_comm_xeno_info_.cpu_id_,
&imu_interface_rt.imu_comm_xeno_info_.delay_ns_);
if (!imu_interface_rt.initializeInSeparateThread()) {
printf("error in initializing the real-time imu in streaming mode\n");
return FALSE;
}
break;
case IMU_MODE_NONRT:
if (!imu_interface_nonrt.initialize()) {
printf("error in initializing the non-real-time imu in non-streaming mode\n");
return FALSE;
}
break;
case IMU_MODE_NONRT_STREAM:
if (!imu_interface_nonrt_stream.initialize()) {
printf("error in initializing the non-real-time imu in streaming mode\n");
return FALSE;
}
break;
case IMU_MODE_PF_NONRT_STREAM:
if(!pf_imu_interface_nonrt_stream.initialize()){
printf("error in initializing the pf non-real-time imu in streaming mode\n");
return FALSE;
}
break;
default:
rt_printf("bad imu_mode\n");
return false;
}
rt_printf("initialized imu successfully\n");
}
rt_printf("Init done.\n");
//read actuals such that the motor loop has a waiting message
for(int i=0; i<(int)gdc_network.gdc_card_states_.size(); i++)
{
tmp_msg.readActuals();
gdc_network.sendMessage(tmp_msg, i);
}
rt_task_sleep(sleep_time);
#ifdef HAS_LOWER_BODY
if(use_feet)
{
//reset foot sensors
hermes_communication_tools::FootSensorMsg foot_msg;
for(int i=0; i<8; ++i)
{
foot_msg.updateBridgeSoftwareOffset(i);
gdc_network.sendFootSensorMessage(foot_msg,0);
gdc_network.sendFootSensorMessage(foot_msg,1);
rt_task_sleep(sleep_time);
}
for(int i=0; i<3; ++i)
{
foot_msg.updateAccelSoftwareOffset(i);
gdc_network.sendFootSensorMessage(foot_msg,0);
gdc_network.sendFootSensorMessage(foot_msg,1);
rt_task_sleep(sleep_time);
}
//ask for foot sensors
foot_msg.getActuals();
gdc_network.sendFootSensorMessage(foot_msg, 0);
gdc_network.sendFootSensorMessage(foot_msg, 1);
}
#endif
//collect some data
for(int i=0; i<(int)gdc_network.gdc_card_states_.size(); ++i)
{
char var_name[20];
sprintf(var_name, "%s_gdc_des_th", gdc_network.gdc_card_states_[i].joint_name_.c_str());
addVarToCollect((char *)&(gdc_network.gdc_card_states_[i].desired_position_),
var_name, "rad", INT, 1);
sprintf(var_name, "%s_gdc_th", gdc_network.gdc_card_states_[i].joint_name_.c_str());
addVarToCollect((char *)&(gdc_network.gdc_card_states_[i].actual_position_),
var_name, "rad", INT, 1);
sprintf(var_name, "%s_gdc_thd", gdc_network.gdc_card_states_[i].joint_name_.c_str());
addVarToCollect((char *)&(gdc_network.gdc_card_states_[i].actual_velocity_),
var_name, "rad", INT, 1);
sprintf(var_name, "%s_gdc_load", gdc_network.gdc_card_states_[i].joint_name_.c_str());
addVarToCollect((char *)&(gdc_network.gdc_card_states_[i].actual_torque_),
var_name, "rad", SHORT, 1);
sprintf(var_name, "%s_gdc_des_load", gdc_network.gdc_card_states_[i].joint_name_.c_str());
addVarToCollect((char *)&(gdc_network.gdc_card_states_[i].desired_torque_),
var_name, "rad", SHORT, 1);
sprintf(var_name, "%s_gdc_des_val", gdc_network.gdc_card_states_[i].joint_name_.c_str());
addVarToCollect((char *)&(gdc_network.gdc_card_states_[i].desired_valve_command_),
var_name, "rad", SHORT, 1);
sprintf(var_name, "%s_gdc_val", gdc_network.gdc_card_states_[i].joint_name_.c_str());
addVarToCollect((char *)&(gdc_network.gdc_card_states_[i].actual_valve_command_),
var_name, "rad", SHORT, 1);
addVarToCollect((char *)&(imu_sensor.A_[0]), "imu_1_Ax","N",DOUBLE,TRUE);
addVarToCollect((char *)&(imu_sensor.A_[1]), "imu_1_Ay","N",DOUBLE,TRUE);
addVarToCollect((char *)&(imu_sensor.A_[2]), "imu_1_Az","N",DOUBLE,TRUE);
addVarToCollect((char *)&(imu_sensor.G_[0]), "imu_1_Gx","N",DOUBLE,TRUE);
addVarToCollect((char *)&(imu_sensor.G_[1]), "imu_1_Gy","N",DOUBLE,TRUE);
addVarToCollect((char *)&(imu_sensor.G_[2]), "imu_1_Gz","N",DOUBLE,TRUE);
addVarToCollect((char *)&(imu_sensor.A1_[0]), "imu_2_Ax","N",DOUBLE,TRUE);
addVarToCollect((char *)&(imu_sensor.A1_[1]), "imu_2_Ay","N",DOUBLE,TRUE);
addVarToCollect((char *)&(imu_sensor.A1_[2]), "imu_2_Az","N",DOUBLE,TRUE);
addVarToCollect((char *)&(imu_sensor.G1_[0]), "imu_2_Gx","N",DOUBLE,TRUE);
addVarToCollect((char *)&(imu_sensor.G1_[1]), "imu_2_Gy","N",DOUBLE,TRUE);
addVarToCollect((char *)&(imu_sensor.G1_[2]), "imu_2_Gz","N",DOUBLE,TRUE);
}
if( (rc = rt_mutex_release(&gdc_mutex)) )
{
printf("Error cannot release gdc mutex, error code %d\n", rc);
return FALSE;
}
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : init_turing_playback_task
Date : June 2014
Remarks:
initialization for task
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
init_turing_playback_task(void)
{
int j, i;
int ans;
char string[100];
static int firsttime = TRUE;
if (firsttime){
firsttime = FALSE;
//freq = 0.1; // frequency
//amp = 0.5; // amplitude
}
// prepare going to the default posture
bzero((char *)&(target[1]),N_DOFS*sizeof(target[1]));
for (i=1; i<=N_DOFS; i++)
target[i] = joint_default_state[i];
// go to the target using inverse dynamics (ID)
if (!go_target_wait_ID(target))
return FALSE;
// add trigger to data collection
sprintf(string,"emgTrig");
addVarToCollect((char *)&(emgTrig),string,"V", DOUBLE,TRUE);
/* add trigger signal to virtual oscilloscope:
add the variable name to prog/masterUser/prefs/task_default.osc*/
updateOscVars();
// clear start data collection flag
//dataFlag = 0;
// input file option to ask
// list files in directory
DIR *d;
struct dirent *dir;
d = opendir("selected_vars/.");
if (d)
{
while ((dir = readdir(d)) != NULL)
{
printf("%s\n", dir->d_name);
}
closedir(d);
}
// get user input
char file_selected_vars[80];
printf ("Enter a file name to open: ");
scanf ("%s", file_selected_vars);
// read position array from txt.
char path[100] = "selected_vars/";
strcat (path, file_selected_vars);
FILE *fp = fopen(path, "r");
if (!fp) {
fprintf(stderr, "Can't open file.\n");
//exit(EXIT_FAILURE);
}
/*
int *a,temp,count=0;
a=(int *)malloc(10*sizeof(int));
temp=fgetc(fp);
while(!feof(fp))
{
if(temp!=44&&temp!=10&&temp!=32)
{
count++;
a[count]=temp;
printf("%c\n", a[count]);
}
temp=fgetc(fp);
}
fclose(fp);
*/
// read the number of lines in the text file.
/*
char line[1024];
int NumberOfLines = 0;
while( fgets(line,sizeof(line),fp) != NULL)
NumberOfLines++;
printf("the number of lines : %i\n", NumberOfLines);
*/
double temp;
int p = 0;
//read lines
const char s[2] = ",";
char *token;
int m;
if(fp != NULL)
{
char line[100]; //10 is enough per line
while(fgets(line, sizeof line, fp) != NULL)
{
token = strtok(line, s);
for(m=0;m<2;m++)
{
if(m==0)
{
printf("%s\t",token);
token = strtok(NULL,s);
pos2DArray[p][0] = atof(token);
} else {
printf("%f\n",atof(token));
pos2DArray[p][1] = atof(token);
}
}
p++;
}
fclose(fp);
}
/*
while (!feof(fp)){ //repeat until end of file
if (p>ARRAYLEN) { p = ARRAYLEN; break;}
fscanf(fp,"%f",&temp);
pos2DArray[p][1] = temp; // time
fscanf(fp,"%f",&temp);
pos2DArray[p][2] = temp; // pos
p++;
printf("p is...%i\n", p);
}
fclose(fp);
*/
// print the trajectory
int l;
for (l = 0; l < ARRAYLEN ; l++) {
// printf("pos2dArray %i, %f\n", l, pos2DArray[l][0]);
}
// ready to go
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
return FALSE;
}
// only go when user really types the right thing
if (ans != 1)
return FALSE;
start_time = task_servo_time;
printf("start time = %.3f, task_servo_time = %.3f\n",
start_time, task_servo_time);
printf("Input var = %i\n",ivar);
// start data collection
scd();
printf("Data collection started\n");
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : add_gravityComp_task
Date : Feb 1999
Remarks:
adds the task to the task menu
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
void
add_gravityComp_task( void )
{
int i, j;
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
addTask("Gravity Compensation Task", init_gravityComp_task,
run_gravityComp_task, change_gravityComp_task);
/* add variables to data collection */
addVarToCollect((char *)&(fieldon),"force-field","-",INT,FALSE);
addVarToCollect((char *)&(joint_filt_state[1].thd),"R_SFE_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[2].thd),"R_SAA_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[3].thd),"R_HR_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[4].thd),"R_EB_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[5].thd),"R_WR_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[6].thd),"R_WFE_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[7].thd),"R_WAA_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[1].thdd),"R_SFE_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[2].thdd),"R_SAA_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[3].thdd),"R_HR_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[4].thdd),"R_EB_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[5].thdd),"R_WR_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[6].thdd),"R_WFE_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[7].thdd),"R_WAA_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(fieldtorque),"fieldtorque","N/m",DOUBLE,FALSE);
}
/* read the control gains and max controls */
if (!read_gains("Gains.cf",controller_gain_th, controller_gain_thd, controller_gain_int))
//if (!read_gains(controller_gain_th, controller_gain_thd, controller_gain_int))
printf("Error reading Gains.cf\n");
}
