/*!***************************************************************************** ******************************************************************************* \note init_controller \date Feb 1999 \remarks initializes all necessary things ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ int init_controller( void ) { int i, j; FILE *in; char string[100]; static int firsttime = TRUE; if (firsttime) { firsttime = FALSE; u = my_vector(1,n_dofs); ufb = my_vector(1,n_dofs); upd = my_vector(1,n_dofs); controller_gain_th = my_vector(1,n_dofs); controller_gain_thd = my_vector(1,n_dofs); controller_gain_int = my_vector(1,n_dofs); integrator_state = my_vector(1,n_dofs); } /* read the control gains and max controls */ if (!read_gains(config_files[GAINS],controller_gain_th, controller_gain_thd, controller_gain_int)) return FALSE; addToMan("ck","change controller",ck); addToMan("setGains","Change PID gain settings",setGainsSim); return TRUE; }
/*!***************************************************************************** ******************************************************************************* \note initUserSim \date Nov. 2007 \remarks initializes the user simulation ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ void initUserSim(void) { // a simple tool to display existing user simulation functions addToMan("listUserSimulation","list all user simulation entries",listUserSimulation); addToMan("clearUserSimulation","clears all active user simulation entries",clearUserSimulation); }
/*!**************************************************************************** ****************************************************************************** \note initOsc \date Aug. 2010 \remarks initializes the oscilloscope processing ***************************************************************************** Function Parameters: [in]=input,[out]=output none *****************************************************************************/ void initOsc(void) { int rc; char string[40]; char fname[100]; FILE *fp; if (no_graphics_flag) return; if (read_parameter_pool_int(config_files[PARAMETERPOOL],"osc_enabled", &rc)) osc_enabled = macro_sign(abs(rc)); if (osc_enabled) { addToMan("oscMenu","interactively change oscilloscope settings",oscMenu); sprintf(osc_vars_name,"%s_default.osc",servo_name); // make sure that the default exists sprintf(fname,"%s%s",PREFS,osc_vars_name); fp = fopen(fname,"a"); fclose(fp); sprintf(string,"osc_vars_%s_file",servo_name); if (read_parameter_pool_string(config_files[PARAMETERPOOL],string,fname)) strcpy(osc_vars_name,fname); readOscVarsScript(osc_vars_name,FALSE); } }
/*!***************************************************************************** ******************************************************************************* \note add_goto_cart_task \date Feb 1999 \remarks adds the task to the task menu ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ void add_goto_cart_task( void ) { int i, j; static int firsttime = TRUE; if (firsttime) { firsttime = FALSE; cart = my_vector(1,n_endeffs*6); ctarget = (SL_Cstate *) my_calloc(n_endeffs+1,sizeof(SL_Cstate),MY_STOP); cnext = (SL_Cstate *) my_calloc(n_endeffs+1,sizeof(SL_Cstate),MY_STOP); cstatus = my_ivector(1,n_endeffs*6); target = (SL_DJstate *)my_calloc(n_dofs+1,sizeof(SL_DJstate),MY_STOP); fthdd = (Filter *)my_calloc(n_dofs+1,sizeof(Filter),MY_STOP); target = (SL_DJstate *)my_calloc(n_dofs+1,sizeof(SL_DJstate),MY_STOP); // initialize the filters init_filters(); for (i=1; i<=n_dofs; ++i) fthdd[i].cutoff = 5; addTask("Goto Cart Task", init_goto_cart_task, run_goto_cart_task, change_goto_cart_task); addToMan("goVisTarget","move one endeff to blob1",goVisTarget); } }
/*!***************************************************************************** ******************************************************************************* \note init_user_openGL \date July 1998 \remarks initializes everything needed for the graphics for this simulation ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] argc : number of elements in argv \param[in] argv : array of argc character strings ******************************************************************************/ int init_user_openGL(int argc, char** argv) { int i,j,n; int rc; int ans; // we need the kinematics initialized init_kinematics(); // read objects readObjects(config_files[OBJECTS]); // assign contact force mappings #include "LEKin_contact.h" // create simulation windows if (!createWindows()) return FALSE; // user graphics addToUserGraphics("phantom","Display a phantom robot", displayPhantom,(N_CART+N_QUAT+n_dofs)*sizeof(float)); addToMan("comsDisplay","toggle drawing COMs per link",toggleCOMsDisplay); return TRUE; }
/*!***************************************************************************** ******************************************************************************* \note init_vxworks \date Feb 1999 \remarks initializes all necessary things ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ int init_vxworks( void ) { int i, j; STATUS stat; char *path; /* set the current path of the target */ path = getenv("path"); if (path != NULL) { cd(path); printf("Path set to: %s\n",path); } else { printf("WARNING: set the >path< variable to current working directory\n"); } /* set the main clock rate */ stat = sysClkRateSet(SYS_CLOCK_RATE); if (stat == ERROR) { printf("ERROR in init_vxworks\n"); return FALSE; } /* avoid floating point underflow */ fppCreateHookRtn = (FUNCPTR)usrFppCreateHook; /* add to man pages */ addToMan("ems","enables the motor servo",NULL); addToMan("dms","disables the motor servo",NULL); addToMan("status","displays status information about servo",NULL); /* real robot flag needs to be set */ real_robot_flag = TRUE; real_time_clock_flag = TRUE; setRealRobotOptions(); return TRUE; }
/*!***************************************************************************** ******************************************************************************* \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; }
/*!***************************************************************************** ******************************************************************************* \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(); initialize_fall_detector(); return TRUE; }
/*!***************************************************************************** ******************************************************************************* \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 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; }
/*!***************************************************************************** ******************************************************************************* \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 SL_IntegrateEuler \date June 1999 \remarks Euler integration ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in,out] state : the state and commands used for integration \param[in,out] cbase : the position state of the base \param[in,out] obase : the orientational state of the base \param[in] ux : the external forces acting on each joint \param[in] leff : the leffector parameters \param[in] dt : the time step \param[in] ndofs : the number of DOFS \param[in] flag : compute forward dynamics yes/no ******************************************************************************/ void SL_IntegrateEuler(SL_Jstate *state, SL_Cstate *cbase, SL_quat *obase, SL_uext *ux, SL_endeff *leff, double dt, int ndofs, int flag) { register int i; double aux=0; static int firsttime = TRUE; if (firsttime) { addToMan("freezeBase","freeze the base at orgin",freezeBaseToggle); firsttime = FALSE; } if (flag) { // compute the accelerations SL_ForDyn(state, cbase, obase, ux, leff); } // Euler integrate forward // the DOFs for(i=1; i<=ndofs; i++) { state[i].thd += dt*state[i].thdd; state[i].th += dt*state[i].thd; } if (!freeze_base) { // optional freezing of base coordinates // translations of the base for(i=1; i<=N_CART; i++) { cbase->xd[i] += dt*cbase->xdd[i]; cbase->x[i] += dt*cbase->xd[i]; } // orientation of the base in quaternions for(i=1; i<=N_CART; i++) { obase->ad[i] += dt*obase->add[i]; } // compute quaternion velocity and acceleration quatDerivatives(obase); // integrate to obtain new angular orientation for(i=1; i<=N_QUAT; i++) { obase->q[i] += dt*obase->qd[i]; aux += sqr(obase->q[i]); } aux = sqrt(aux); if (aux == 0) aux = 1.e-10; // important: renormalize quaternions for(i=1; i<=N_QUAT; i++) { obase->q[i]/=aux; } } else { // if base coordinates are frozen bzero((void *)cbase,sizeof(SL_Cstate)); bzero((void *)obase,sizeof(SL_quat)); obase->q[_Q0_] = freeze_base_quat[_Q0_]; obase->q[_Q1_] = freeze_base_quat[_Q1_]; obase->q[_Q2_] = freeze_base_quat[_Q2_]; obase->q[_Q3_] = freeze_base_quat[_Q3_]; cbase->x[_X_] = freeze_base_pos[_X_]; cbase->x[_Y_] = freeze_base_pos[_Y_]; cbase->x[_Z_] = freeze_base_pos[_Z_]; } // update the simulated link positions such that contact forces are correct linkInformation(state,cbase,obase,leff, joint_cog_mpos_sim,joint_axis_pos_sim,joint_origin_pos_sim, link_pos_sim,Alink_sim,Adof_sim); // check for contacts with objects checkContacts(); }