/***************************************************************************** ****************************************************************************** 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(); }