/** Simulator Entry Point */ int main( int argc, char* argv[] ) { // Initialize process identifiers controller_pid = 0; //dynamics_pid = 0; // Get the sim process identifier sim_pid = getpid(); // Get priority for the sim process sim_priority = getpriority( PRIO_PROCESS, sim_pid ); //printf("Sim PID: %d\n", sim_pid ); //printf("Sim Priority: %d\n", sim_priority ); // Initialize before forking as children will inherit fd's initialize_ipc_pipes(); // Fork all child processes fork_controller(); fork_sensor(); init_dynamics(); // Transition Main Process to Coordinator main loop coordinator(); return 0; }
/*!***************************************************************************** ******************************************************************************* \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 initSimulation \date July 1998 \remarks initializes everything needed for the simulation. ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] argc : number of elements in argv \param[in] argv : array of argc character strings ******************************************************************************/ int initSimulation(int argc, char** argv) { int i,j,n; int rc; int ans; /* initialize the servos */ init_task_servo(); /* the first servo sets the sampling rate in collect data */ read_whichDOFs("task_sim_servo"); init_motor_servo(); read_whichDOFs("motor_sim_servo"); init_vision_servo(); read_whichDOFs("vision_sim_servo"); init_invdyn_servo(); read_whichDOFs("invdyn_sim_servo"); /* we need the dynamics initalized */ init_dynamics(); /* user specific tasks */ initUserTasks(); /* create simulation windows */ if (!createWindows()) return FALSE; /* reset motor_servo variables */ servo_enabled = 1; motor_servo_calls = 0; servo_time = 0; motor_servo_rate = SERVO_BASE_RATE; zero_integrator(); bzero((char *)&(joint_sim_state[1]),sizeof(SL_DJstate)*N_DOFS); for (i=1; i<=N_DOFS; ++i) { joint_sim_state[i].th = joint_default_state[i].th; joint_des_state[i].th = joint_default_state[i].th; } /* reset task_servo variables */ servo_enabled = 1; task_servo_calls = 0; task_servo_time = 0; task_servo_errors = 0; task_servo_rate = SERVO_BASE_RATE/(double) task_servo_ratio; setTaskByName(NO_TASK); setDefaultPosture(); changeCollectFreq(task_servo_rate); /* reset vision servo variables */ servo_enabled = 1; vision_servo_calls = 0; vision_servo_time = 0; vision_servo_errors = 0; vision_servo_rate = VISION_SERVO_RATE; /* initialize all vision variables to safe values */ init_vision_states(); init_learning(); /* reset the invdyn servo variables */ servo_enabled = 1; invdyn_servo_errors = 0; invdyn_lookup_data = 0; invdyn_learning_data = 0; /* initalize objects in the environment */ readObjects(); /* assign contact force mappings */ #include "LEKin_contact.h" initContacts(); return TRUE; }
/*!***************************************************************************** ******************************************************************************* \note init_simulation_servo \date Nov. 2007 \remarks initializes everything for this servo ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ int init_simulation_servo(void) { int i, j; // the servo name sprintf(servo_name,"sim"); // initialization of variables simulation_servo_rate = servo_base_rate; // read controller gains controller_gain_th = my_vector(1,n_dofs); controller_gain_thd = my_vector(1,n_dofs); controller_gain_int = my_vector(1,n_dofs); if (!read_gains(config_files[GAINS],controller_gain_th, controller_gain_thd, controller_gain_int)) return FALSE; /* inverse dynamics */ if (!init_dynamics()) return FALSE; /* initialize kinematicss */ init_kinematics(); // get shared memory if (!init_shared_memory()) return FALSE; // object handling if (!initObjects()) return FALSE; // need sensor offsets if (!read_sensor_offsets(config_files[SENSOROFFSETS])) return FALSE; // initializes user specific issues if (!init_user_simulation()) return FALSE; // add to man pages addToMan("setIntRate","set number of integration cycles",setIntRate); addToMan("setIntMethod","set integration method",setIntMethod); addToMan("realTime","toggle real-time processing",toggleRealTime); addToMan("status","displays status information about servo",status); addToMan("dss","disables the simulation servo",dss); addToMan("where_gains","Print gains of the joints",where_gains); // data collection initDataCollection(); // oscilloscope initOsc(); // initialize user specific simulations initUserSim(); // general initialization if (!initUserSimulation()) // user specific intialization return FALSE; servo_enabled = TRUE; return TRUE; }