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