static int
read_simulated_base(void)
{
// base state
if (semTake(sm_base_state_sem,ns2ticks(TIME_OUT_NS)) == ERROR)
return FALSE;
sm_base_state_data[1] = sm_base_state->state[1];
cSL_Cstate(&base_state-1, sm_base_state_data, 1, FLOAT2DOUBLE);
semGive(sm_base_state_sem);
// base orient
if (semTake(sm_base_orient_sem,ns2ticks(TIME_OUT_NS)) == ERROR)
return FALSE;
sm_base_orient_data[1] = sm_base_orient->orient[1];
cSL_quat(&base_orient-1, sm_base_orient_data, 1, FLOAT2DOUBLE);
semGive(sm_base_orient_sem);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note send_sim_state
\date Nov. 2007
\remarks
sends the entire joint_sim_state to shared memory
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
send_sim_state(void)
{
int i;
// joint state
if (semTake(sm_joint_sim_state_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++simulation_servo_errors;
return FALSE;
}
cSL_Jstate(joint_sim_state,sm_joint_sim_state_data,n_dofs,DOUBLE2FLOAT);
for (i=1; i<=n_dofs; ++i)
sm_joint_sim_state->joint_sim_state[i] = sm_joint_sim_state_data[i];
sm_joint_sim_state->ts = simulation_servo_time;
semGive(sm_joint_sim_state_sem);
// base state
if (semTake(sm_base_state_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++simulation_servo_errors;
return FALSE;
}
cSL_Cstate((&base_state)-1, sm_base_state_data, 1, DOUBLE2FLOAT);
sm_base_state->state[1] = sm_base_state_data[1];
sm_base_state->ts = simulation_servo_time;
semGive(sm_base_state_sem);
// base orient
if (semTake(sm_base_orient_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++simulation_servo_errors;
return FALSE;
}
cSL_quat(&base_orient-1, sm_base_orient_data, 1, DOUBLE2FLOAT);
sm_base_orient->orient[1] = sm_base_orient_data[1];
sm_base_orient->ts = simulation_servo_time;
semGive(sm_base_orient_sem);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note send_des_command
\date Nov. 2007
\remarks
send the commands from the joint_sim_state shared memory
structure
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
send_des_command(void)
{
int i;
extern double *upd;
if (semTake(sm_des_commands_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++motor_servo_errors;
return FALSE;
}
for (i=1; i<=n_dofs; ++i) {
sm_des_commands->des_command[i].th = (float) joint_des_state[i].th;
sm_des_commands->des_command[i].thd = (float) joint_des_state[i].thd;
sm_des_commands->des_command[i].uff = (float) joint_des_state[i].uff;
sm_des_commands->des_command[i].u = (float) joint_sim_state[i].u;
sm_des_commands->des_command[i].upd = (float) upd[i];
}
sm_des_commands->ts = motor_servo_time;
semGive(sm_des_commands_sem);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note receive_sim_state
\date Nov. 2007
\remarks
recieves the entire joint_sim_state from shared memory
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
receive_sim_state(void)
{
int i;
if (semTake(sm_joint_sim_state_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++motor_servo_errors;
return FALSE;
}
for (i=1; i<=n_dofs; ++i)
sm_joint_sim_state_data[i] = sm_joint_sim_state->joint_sim_state[i];
cSL_Jstate(joint_sim_state,sm_joint_sim_state_data,n_dofs,FLOAT2DOUBLE);
// get time stamp and check for synchronization errors
motor_servo_time = servo_time = sm_joint_sim_state->ts;
semGive(sm_joint_sim_state_sem);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note send_contacts
\date Nov. 2007
\remarks
sends the contact forces to shared memory
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
send_contacts(void)
{
int i,j;
if (semTake(sm_contacts_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++simulation_servo_errors;
return FALSE;
}
for (i=0; i<=n_contacts; ++i) {
sm_contacts->contact[i].status = contacts[i].status;
if (contacts[i].status) {
for (j=1; j<=N_CART; ++j) {
sm_contacts->contact[i].f[j] = contacts[i].f[j];
sm_contacts->contact[i].n[j] = contacts[i].n[j];
}
strncpy(sm_contacts->contact[i].name,contacts[i].optr->name,STRING100);
} else {
for (j=1; j<=N_CART; ++j) {
sm_contacts->contact[i].f[j] = 0.0;
sm_contacts->contact[i].n[j] = 0.0;
}
strncpy(sm_contacts->contact[i].name,"\0",STRING100);
}
}
semGive(sm_contacts_sem);
return TRUE;
}
void
receiveOscilloscopeData(void)
{
int i,j,r;
int count;
if (!osc_enabled)
return;
#ifdef __XENO__
// we want to be in real-time mode here
//printf("..\n");
#if (CONFIG_XENO_VERSION_MAJOR < 2) || (CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR < 6)
// we are on xenomai version < 2.6
rt_task_set_mode(0,T_PRIMARY,NULL);
#else
// we are on xenomai version < 2.6
rt_task_set_mode(0,T_CONFORMING,NULL);
#endif
#endif
// try to take semaphore with very short time-out -- we don't care if we
// cannot copy the data to shared memory every time, as this is not a time
// critical operation
if (semTake(sm_oscilloscope_sem,ns2ticks(TIME_OUT_SHORT_NS)) == ERROR)
{
#ifdef __XENO__
// we want to be in secondary mode here
//rt_task_set_mode(T_PRIMARY,0,NULL);
#endif
return;
}
// copy first-in-first-out data from shared memory into ring buffer
count = sm_oscilloscope->n_entries;
for (i=1; i<=count; ++i)
addOscData(sm_oscilloscope->entries[i]);
sm_oscilloscope->n_entries = 0;
// give back semaphore
semGive(sm_oscilloscope_sem);
#ifdef __XENO__
// we want to be in secondary mode here
//rt_task_set_mode(T_PRIMARY,0,NULL);
#endif
// update the oscilloscope window
if (pause_flag || stand_alone_flag)
return;
else if (count > 0)
glutPostWindowRedisplay(openGLId_osc);
}
/*!*****************************************************************************
*******************************************************************************
\note receive_ros_state
\date July 2010
\remarks
receives all relevant state variables to ROS process
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
receive_ros_state(void)
{
int i,j;
SL_fJstate *fJstate;
SL_fDJstate *fDJstate;
SL_fCstate *fCstate;
SL_fquat *fquat;
float *misc;
double ts;
int dticks;
if (semTake(sm_ros_state_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++ros_servo_errors;
return FALSE;
}
// create pointers to share memory
fJstate = (SL_fJstate *) sm_ros_state->data;
fDJstate = (SL_fDJstate *) &(sm_ros_state->data[sizeof(SL_fJstate)*(n_dofs+1)]);
fCstate = (SL_fCstate *) &(sm_ros_state->data[sizeof(SL_fJstate)*(n_dofs+1)+
sizeof(SL_fDJstate)*(n_dofs+1)]);
fquat = (SL_fquat *) &(sm_ros_state->data[sizeof(SL_fJstate)*(n_dofs+1)+
sizeof(SL_fDJstate)*(n_dofs+1)+
sizeof(SL_fCstate)*(1+1)]);
misc = (float *) &(sm_ros_state->data[sizeof(SL_fJstate)*(n_dofs+1)+
sizeof(SL_fDJstate)*(n_dofs+1)+
sizeof(SL_fCstate)*(1+1)+
sizeof(SL_fquat)*(1+1)]);
memcpy((void *)(&sm_joint_state_data[1]),(const void*)(&(fJstate[1])),sizeof(SL_fJstate)*n_dofs);
memcpy((void *)(&sm_joint_des_state_data[1]),(const void*)(&(fDJstate[1])),sizeof(SL_fDJstate)*n_dofs);
memcpy((void *)(&sm_base_state_data[1]),(const void*)(&(fCstate[1])),sizeof(SL_fCstate)*1);
memcpy((void *)(&sm_base_orient_data[1]),(const void*)(&(fquat[1])),sizeof(SL_fquat)*1);
memcpy((void *)(&sm_misc_sensor_data[1]),(const void*)(&(misc[1])),sizeof(float)*n_misc_sensors);
cSL_Jstate(joint_state,sm_joint_state_data,n_dofs,FLOAT2DOUBLE);
cSL_DJstate(joint_des_state, sm_joint_des_state_data, n_dofs,FLOAT2DOUBLE);
cSL_Cstate((&base_state)-1, sm_base_state_data, 1, FLOAT2DOUBLE);
cSL_quat((&base_orient)-1, sm_base_orient_data, 1, FLOAT2DOUBLE);
for (i=1; i<=n_misc_sensors; ++i)
misc_sensor[i] = (double) sm_misc_sensor_data[i];
// get time stamp and adjust time
ros_servo_time = servo_time = sm_ros_state->ts;
semGive(sm_ros_state_sem);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note receive_des_commands
\date Nov. 2007
\remarks
reads the commands from the joint_sim_state shared memory
structure
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
receive_des_commands(void)
{
int i;
extern double *controller_gain_th;
extern double *controller_gain_thd;
if (semTake(sm_des_commands_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++simulation_servo_errors;
return FALSE;
}
for (i=1; i<=n_dofs; ++i) {
joint_sim_state[i].u = (double) sm_des_commands->des_command[i].u -
(double) sm_des_commands->des_command[i].upd;
joint_des_state[i].th = (double) sm_des_commands->des_command[i].th;
joint_des_state[i].thd = (double) sm_des_commands->des_command[i].thd;
}
// re-generate and add the PD command from the motor servo, as the
// delay between motor servo and simulation servo can cause instabilities
// in case of stiff contacts -- the reason is that the state info on the
// motor servo is one tick delayed; the old PD command has been subtracted
// from the total command above
for (i=1; i<=n_dofs; ++i) {
if (sm_des_commands->des_command[i].upd != 0.0) {
joint_sim_state[i].u += (joint_des_state[i].th - joint_sim_state[i].th) *
controller_gain_th[i];
joint_sim_state[i].u += (joint_des_state[i].thd - joint_sim_state[i].thd) *
controller_gain_thd[i];
}
if (fabs(joint_sim_state[i].u) > u_max[i]) {
joint_sim_state[i].u = macro_sign(joint_sim_state[i].u) * u_max[i];
}
}
semGive(sm_des_commands_sem);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note go_cart_target_wait
\date
\remarks
go to the given cartesian target
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] ctar : cartesian states
\param[in] stat : 1/0 for active target or not
\param[in] mt : movement time
******************************************************************************/
int
go_cart_target_wait(SL_Cstate *ctar,int *stat, double mt)
{
int i,j;
double last_time;
double last_draw_time;
double aux;
special_flag = TRUE;
/* initialize some variables */
init_vars();
/* assign the target variables */
for (i=1; i<=n_endeffs; ++i) {
for (j= _X_; j<= _Z_; ++j) {
cstatus[(i-1)*6+j] = stat[(i-1)*6+j];
aux = ctar[i].x[j];
ctarget[i].x[j] = aux;
}
}
/* the movement time */
tau = mt;
if (!setTaskByName("Goto Cart Task")) {
special_flag = FALSE;
return FALSE;
}
last_time = last_draw_time = task_servo_time;
while (strcmp(current_task_name,NO_TASK) != 0) {
if (task_servo_time - last_time > 2*mt) {
printf("time out in go_cart_target_wait\n");
special_flag = FALSE;
return FALSE;
}
taskDelay(ns2ticks(10000000)); // wait 10ms
}
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note checkForMessages
\date Nov. 2007
\remarks
Messages can be given to the servo for hard-coded tasks.This allows
some information passing between the different processes on variables
of common interest, e.g., the endeffector specs, object information,
etc.
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
checkForMessages(void)
{
int i,j;
char name[20];
// check whether a message is available
if (semTake(sm_ros_message_ready_sem,NO_WAIT) == ERROR)
return FALSE;
// receive the message
if (semTake(sm_ros_message_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++ros_servo_errors;
printf("Couldn't take task message semaphore\n");
return FALSE;
}
for (i=1; i<=sm_ros_message->n_msgs; ++i) {
// get the name of this message
strcpy(name,sm_ros_message->name[i]);
// act according to the message name
// ---------------------------------------------------------------------------
if (strcmp(name,"status") == 0) {
status();
}
}
// give back semaphore
sm_ros_message->n_msgs = 0;
sm_ros_message->n_bytes_used = 0;
semGive(sm_ros_message_sem);
return TRUE;
}
int
setOsc(int channel, double pval)
{
int rc;
double ts;
char string[40];
// if the user provide a special oscilloscope function --------------------
if (d2a_function != NULL) {
if (semTake(sm_oscilloscope_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
return FALSE;
} else {
rc = (*d2a_function)(channel,pval);
semGive(sm_oscilloscope_sem);
if (!rc)
return FALSE;
}
}
// the graphics oscillscope -----------------------------------------------
if (!osc_enabled)
return TRUE;
#ifdef __XENO__
RTIME t = rt_timer_read();
ts = (double)t/1.e9;
//struct timespec t;
//clock_gettime(CLOCK_MONOTONIC,&t);
//ts = (double) t.tv_sec + ((double)t.tv_nsec)/1.e9;
#else
struct timeval t;
gettimeofday(&t,NULL);
ts = (double) t.tv_sec + ((double)t.tv_usec)/1.e6;
#endif
sprintf(string,"D2A_%s [%%]",servo_name);
addEntryOscBuffer(string, pval, ts, 0);
return TRUE;
}
static int receive_contacts(void)
{
int i,j;
if (semTake(sm_contacts_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
return FALSE;
}
for (i=0; i<=n_contacts; ++i) {
contacts[i].status = sm_contacts->contact[i].status;
for (j=1; j<=N_CART; ++j)
contacts[i].f[j] = sm_contacts->contact[i].f[j];
}
semGive(sm_contacts_sem);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note send_misc_sensors
\date Nov. 2007
\remarks
sends the entire misc_sim_sensors to shared memory
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
send_misc_sensors(void)
{
int i;
if (semTake(sm_misc_sim_sensor_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++simulation_servo_errors;
return FALSE;
}
for (i=1; i<=n_misc_sensors; ++i)
sm_misc_sim_sensor->value[i] = misc_sim_sensor[i];
sm_misc_sim_sensor->ts = simulation_servo_time;
semGive(sm_misc_sim_sensor_sem);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note receive_misc_sensors
\date Nov. 2007
\remarks
receives the entire misc_sim_sensors from shared memory
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
receive_misc_sensors(void)
{
int i;
if (n_misc_sensors <= 0)
return TRUE;
if (semTake(sm_misc_sim_sensor_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++motor_servo_errors;
return FALSE;
}
for (i=1; i<=n_misc_sensors; ++i)
misc_sim_sensor[i] = sm_misc_sim_sensor->value[i];
semGive(sm_misc_sim_sensor_sem);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note run_user_task
\date Nov. 2007
\remarks
this function is clocked out of the task servo
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
run_user_task(void)
{
int i,j;
MY_MATRIX(M,1,N_CART,1,N_CART);
MY_VECTOR(v,1,N_CART);
// compute the contact forces in world coordinates at the feet from the
// force sensors
/* rotation matrix from world to L_AAA coordinates:
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
#ifdef HAS_LOWER_BODY
// transform forces
v[_X_] = misc_sensor[L_CFx];
v[_Y_] = misc_sensor[L_CFy];
v[_Z_] = misc_sensor[L_CFz];
mat_vec_mult_size(Alink[L_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[LEFT_FOOT].cf);
// transform torques
v[_A_] = misc_sensor[L_CTa];
v[_B_] = misc_sensor[L_CTb];
v[_G_] = misc_sensor[L_CTg];
mat_vec_mult_size(Alink[L_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[LEFT_FOOT].ct);
/* rotation matrix from world to R_AAA coordinates :
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
// transform forces
// transform forces
v[_X_] = misc_sensor[R_CFx];
v[_Y_] = misc_sensor[R_CFy];
v[_Z_] = misc_sensor[R_CFz];
mat_vec_mult_size(Alink[R_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[RIGHT_FOOT].cf);
// transform torques
v[_A_] = misc_sensor[R_CTa];
v[_B_] = misc_sensor[R_CTb];
v[_G_] = misc_sensor[R_CTg];
mat_vec_mult_size(Alink[R_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[RIGHT_FOOT].ct);
#endif
differentiate_cog(&cog);
test_fall(&cog);
// use the simulated base state if required
if (use_simulated_base_state)
read_simulated_base();
else
{
// do base state estimation
// update_base_state_estimation(&base_orient, &base_state);
// run_state_est_lin_task();
// getPelv(&base_orient, &base_state);
// base state
if (semTake(sm_base_state_sem,ns2ticks(NO_WAIT)) == ERROR)
{
//printf("sm_base_state_sem take error\n");
return TRUE;
}
cSL_Cstate((&base_state)-1, sm_base_state_data, 1, DOUBLE2FLOAT);
sm_base_state->state[1] = sm_base_state_data[1];
semGive(sm_base_state_sem);
// base orient
if (semTake(sm_base_orient_sem,ns2ticks(NO_WAIT)) == ERROR)
{
//printf("sm_base_orien_sem take error\n");
return TRUE;
}
cSL_quat(&base_orient-1, sm_base_orient_data, 1, DOUBLE2FLOAT);
sm_base_orient->orient[1] = sm_base_orient_data[1];
semGive(sm_base_orient_sem);
}
return TRUE;
}
int
acquire_blobs(Blob2D blobs[][2+1])
{
int i,j,r,m;
int rc;
static char buffer[MAX_CHARS];
static int n_buffer = 0;
char buffer2[MAX_CHARS];
int n_buffer2 = 0;
int count = 0;
int run = TRUE;
// reset all the blobs to be non existent
for (i = 1; i<=max_blobs; ++i) {
the_frame.blobinfo[i][CAMERA_1].status = FALSE;
the_frame.blobinfo[i][CAMERA_2].status = FALSE;
}
while( run ) {
// check for data in the serial port buffer
if ( (rc=check_serial(serial_fd)) > 0) {
if (rc > MAX_CHARS-n_buffer) {
rc = MAX_CHARS-n_buffer;
printf("buffer overflow\n");
}
rc = read_serial(serial_fd,rc,&(buffer[n_buffer]));
n_buffer += rc;
}
// look for a complete frame in the data
for (j=n_buffer-1; j>=0; --j) {
if (buffer[j] == '\n') // found the end of a frame at j
break;
}
for (i=j; i>=0; --i) {
if (buffer[i] == '^') { // found the beginning of a frame at i
run = FALSE;
break;
}
}
if (i<0 && j>=0) { // no beginning of frame found despite end of frame
for (r = j+1; r<n_buffer; ++r)
buffer[r-(j+1)] = buffer[r];
n_buffer -= j+1;
printf("discard early buffer\n");
}
if (!run)
break;
taskDelay(ns2ticks(WAIT_IN_NS));
if (++count > 10000) {
printf("Error: Timeout when reading from vision hardware\n");
printf("Switch to no-hardware mode\n");
no_hardware_flag = TRUE;
return TRUE;
}
}
// re-arrange the buffer
for (r = i+1; r<j; ++r)
buffer2[r-(i+1)] = buffer[r];
buffer2[j-(i+1)] = '\0';
n_buffer2 = j-(i+1);
for (r = j+1; r<n_buffer; ++r)
buffer[r-(j+1)] = buffer[r];
n_buffer -= j+1;
/* now we are at the beginning of a frame */
last_counter = the_frame.counter;
/* read the entire frame */
rc = read_frame(buffer2,n_buffer2);
++frame_counter;
if (vision_servo_calls < 10) {
/* synchronize the v->frame_counter and the_frame.counter */
frame_counter = the_frame.counter;
last_counter = frame_counter - 1;
}
++count_all_frames;
/* frame_counter and the_frame.counter should always coincide,
anything else counts as a lost frame */
count_lost_frames += abs(frame_counter - the_frame.counter);
frame_counter = the_frame.counter;
/* was the frame counter advanced and was read_frame successful ?*/
if ( (the_frame.counter == last_counter + 1) && rc ) {
;
} else {
/* reset all the blobs to be non existent */
for (i = 1; i<=max_blobs; ++i) {
the_frame.blobinfo[i][CAMERA_1].status = FALSE;
the_frame.blobinfo[i][CAMERA_2].status = FALSE;
}
++count_lost_frames;
}
/* copy the data into the global structures */
for (i=1; i<=max_blobs; ++i) {
/*
if (the_frame.blobinfo[i][CAMERA_1].status &&
the_frame.blobinfo[i][CAMERA_2].status) {
blobs[i][CAMERA_1].status = blobs[i][CAMERA_2].status = TRUE;
blobs[i][CAMERA_1].x[_X_] = the_frame.blobinfo[i][CAMERA_1].x;
blobs[i][CAMERA_1].x[_Y_] = the_frame.blobinfo[i][CAMERA_1].y;
blobs[i][CAMERA_2].x[_X_] = the_frame.blobinfo[i][CAMERA_2].x;
blobs[i][CAMERA_2].x[_Y_] = the_frame.blobinfo[i][CAMERA_2].y;
} else {
blobs[i][CAMERA_1].status = blobs[i][CAMERA_2].status = FALSE;
} */
if (the_frame.blobinfo[i][CAMERA_1].status) {
blobs[i][CAMERA_1].status = TRUE;
blobs[i][CAMERA_1].x[_X_] = the_frame.blobinfo[i][CAMERA_1].x;
blobs[i][CAMERA_1].x[_Y_] = the_frame.blobinfo[i][CAMERA_1].y;
} else {
blobs[i][CAMERA_1].status = FALSE;
}
if (the_frame.blobinfo[i][CAMERA_2].status) {
blobs[i][CAMERA_2].status = TRUE;
blobs[i][CAMERA_2].x[_X_] = the_frame.blobinfo[i][CAMERA_2].x;
blobs[i][CAMERA_2].x[_Y_] = the_frame.blobinfo[i][CAMERA_2].y;
} else {
blobs[i][CAMERA_2].status = FALSE;
}
}
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note send_real_state
\date Nov. 2007
\remarks
sends the entire joint_state to shared memory (used for the openGL)
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int updateOpenGL(SL_Jstate *j_state, double* misc)
{
// joint state
if (semTake(sm_joint_sim_state_sem,ns2ticks(NO_WAIT)) == ERROR)
{
return FALSE;
}
cSL_Jstate(j_state,sm_joint_sim_state_data,n_dofs,DOUBLE2FLOAT);
for (int i=1; i<=n_dofs; ++i)
sm_joint_sim_state->joint_sim_state[i] = sm_joint_sim_state_data[i];
semGive(sm_joint_sim_state_sem);
// // base state
// if (semTake(sm_base_state_sem,ns2ticks(NO_WAIT)) == ERROR)
// {
// return FALSE;
// }
//
// cSL_Cstate((&base_state)-1, sm_base_state_data, 1, DOUBLE2FLOAT);
//
// sm_base_state->state[1] = sm_base_state_data[1];
//
// semGive(sm_base_state_sem);
//
//
// // base orient
// if (semTake(sm_base_orient_sem,ns2ticks(NO_WAIT)) == ERROR)
// {
// return FALSE;
//
// }
// SL_quat imu_orient_quat;
// imu_orient_quat.q[_Q0_] = misc[B_Q0_IMU];
// imu_orient_quat.q[_Q1_] = misc[B_Q1_IMU];
// imu_orient_quat.q[_Q2_] = misc[B_Q2_IMU];
// imu_orient_quat.q[_Q3_] = misc[B_Q3_IMU];
//
// cSL_quat(&imu_orient_quat-1, sm_base_orient_data, 1, DOUBLE2FLOAT);
//
// sm_base_orient->orient[1] = sm_base_orient_data[1];
//
// semGive(sm_base_orient_sem);
//misc sensors
if (semTake(sm_misc_sim_sensor_sem,ns2ticks(NO_WAIT)) == ERROR)
{
return FALSE;
}
for (int i=1; i<=n_misc_sensors; ++i)
sm_misc_sim_sensor->value[i] = misc_sensor[i];
semGive(sm_misc_sim_sensor_sem);
//contacts
if (semTake(sm_contacts_sem,ns2ticks(NO_WAIT)) == ERROR) {
return FALSE;
}
//TO BE IMPLEMENTED
for (int i=0; i<=n_links; ++i)
{
sm_contacts->contact[i].status = 0; //contacts[i].status;
for (int j=1; j<=N_CART; ++j)
sm_contacts->contact[i].f[j] = 0; //contacts[i].f[j];
}
semGive(sm_contacts_sem);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note monitor_min_max
\date Dec. 2000
\remarks
monitors the min & max values of all joints continuously until keyboard
is hit. Then, an appropriate min_max structure is printed out for usuage
in SensorOffsets.cf
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
void
monitor_min_max(void)
{
int i,j;
Matrix minmax;
int n_bytes;
double offset = 0.05;
if (!servo_enabled) {
beep(1);
printf("ERROR: motor servo is not running!!\n");
return;
}
minmax = my_matrix(1,n_dofs,MIN_THETA,MAX_THETA);
for (i=1; i<=n_dofs; ++i) {
minmax[i][MIN_THETA] = 1.e10;
minmax[i][MAX_THETA] = -1.e10;
}
printf("Hit any key to stop monitoring .....");
fflush(stdout);
/* just check for min & max values */
while (TRUE) {
for (i=1; i<=n_dofs; ++i) {
/* update min/max values */
if (joint_state[i].th > minmax[i][MAX_THETA])
minmax[i][MAX_THETA] = joint_state[i].th;
if (joint_state[i].th < minmax[i][MIN_THETA])
minmax[i][MIN_THETA] = joint_state[i].th;
}
/* check whether the keyboard was hit */
#ifdef VX
ioctl(0 , FIONREAD, (int) (&n_bytes));
#else
ioctl( 0, FIONREAD, (void *) (&n_bytes) );
#endif
if (n_bytes != 0)
break;
taskDelay(ns2ticks(10000000)); // wait 10ms
}
getchar();
get_double("Safety margin from joint stop?",offset,&offset);
/* print the min/max values in SensorOffset.cf format */
for (i=1; i<=n_dofs; ++i) {
printf("%10s %f %f %f %f %f %f\n",
joint_names[i],
minmax[i][MIN_THETA]+offset,
minmax[i][MAX_THETA]-offset,
joint_default_state[i].th,
joint_opt_state[i].th,
joint_opt_state[i].w,
joint_range[i][THETA_OFFSET]);
}
printf("\n");
fflush(stdout);
my_free_matrix(minmax,1,n_dofs,MIN_THETA,MAX_THETA);
}
/*!*****************************************************************************
*******************************************************************************
\note checkForMessages
\date Nov. 2007
\remarks
Messages can be given to the servo for hard-coded tasks.This allows
some information passing between the different processes on variables
of common interest, e.g., the endeffector specs, object information,
etc.
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
checkForMessages(void)
{
int i,j,k;
char name[20];
// check whether a message is available
if (semTake(sm_simulation_message_ready_sem,NO_WAIT) == ERROR) {
return FALSE;
}
// receive the message
if (semTake(sm_simulation_message_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
printf("Couldn't take simulation message semaphore\n");
return FALSE;
}
for (k=1; k<=sm_simulation_message->n_msgs; ++k) {
// get the name of this message
strcpy(name,sm_simulation_message->name[k]);
// act according to the message name
// -------------------------------------------------------------------------
if (strcmp(name,"reset") == 0) { // reset simulation -----------------------
float buf[N_CART+N_QUAT+1];
memcpy(&(buf[1]),sm_simulation_message->buf+sm_simulation_message->moff[k],
sizeof(float)*(N_CART+N_QUAT));
j = 0;
for (i=1; i<=N_CART; ++i) {
freeze_base_pos[i] = buf[++j];
}
for (i=1; i<=N_QUAT; ++i) {
freeze_base_quat[i] = buf[++j];
}
reset();
// -------------------------------------------------------------------------
} else if (strcmp(name,"changePIDGains") == 0) { // change the gains -------
float buf[n_dofs*3+1];
memcpy(&(buf[1]),sm_simulation_message->buf+sm_simulation_message->moff[k],
sizeof(float)*(3*n_dofs));
for (i=1; i<=n_dofs; ++i) {
controller_gain_th[i] = (double) buf[i];
controller_gain_thd[i] = (double) buf[i+n_dofs];
controller_gain_int[i] = (double) buf[i+2*n_dofs];
}
// ---------------------------------------------------------------------------
} else if (strcmp(name,"where_gains") == 0) {
where_gains();
// -------------------------------------------------------------------------
} else if (strcmp(name,"setUextSim") == 0) { // receive external simulated forces
int count = 0;
float buf[n_dofs*N_CART*2+1];
memcpy(&(buf[1]),sm_simulation_message->buf+sm_simulation_message->moff[k],
sizeof(float)*(2*n_dofs*N_CART));
for (i=1; i<=n_dofs; ++i) {
for (j=1; j<=N_CART; ++j) {
uext_sim[i].f[j] = buf[++count];
uext_sim[i].t[j] = buf[++count];
}
}
// -------------------------------------------------------------------------
} else if (strcmp(name,"changeRealTime") == 0) { // real time simualtion on/off
float buf[1+1];
memcpy(&(buf[1]),sm_simulation_message->buf+sm_simulation_message->moff[k],
sizeof(float));
if (buf[1] == 0) {
real_time = FALSE;
printf("Real-time processing switched off\n");
} else {
real_time = TRUE;
printf("Real-time processing switched on\n");
}
// -------------------------------------------------------------------------
} else if (strcmp(name,"freezeBase") == 0) { // freeze base in simualtion on/off
float buf[1+1];
memcpy(&(buf[1]),sm_simulation_message->buf+sm_simulation_message->moff[k],
sizeof(float));
if (buf[1] == 0) {
freeze_base = FALSE;
printf("Freeze base switched off\n");
} else {
freeze_base = TRUE;
printf("Freeze base switched on\n");
}
// -------------------------------------------------------------------------
} else if (strcmp(name,"setG") == 0) { // change the gravity constant
float buf[1+1];
memcpy(&(buf[1]),sm_simulation_message->buf+sm_simulation_message->moff[k],
sizeof(float));
gravity = buf[1];
printf("Gravity set to %f\n",gravity);
// ---------------------------------------------------------------------------
} else if (strcmp(name,"addObject") == 0) {
struct {
char name[STRING100]; /*!< object name */
int type; /*!< object type */
double trans[N_CART+1]; /*!< translatory offset of object */
double rot[N_CART+1]; /*!< rotational offset of object */
double scale[N_CART+1]; /*!< scaling in x,y,z */
double rgb[N_CART+1]; /*!< color information */
double object_parms[MAX_OBJ_PARMS+1]; /*!< object parameters */
int contact_model; /*!< which contact model to be used */
double contact_parms[MAX_CONTACT_PARMS+1]; /*!< contact parameters */
} data;
memcpy(&data,sm_simulation_message->buf+sm_simulation_message->moff[k],sizeof(data));
addObject(data.name, data.type, data.contact_model, data.rgb, data.trans, data.rot,
data.scale, data.contact_parms,data.object_parms);
// ---------------------------------------------------------------------------
} else if (strcmp(name,"hideObject") == 0) {
struct {
int hide;
char obj_name[100];
} data;
memcpy(&data,sm_simulation_message->buf+sm_simulation_message->moff[k],sizeof(data));
changeHideObjByName(data.obj_name, data.hide);
// ---------------------------------------------------------------------------
} else if (strcmp(name,"changeObjectPos") == 0) {
struct {
char obj_name[100];
double pos[N_CART+1];
double rot[N_CART+1];
} data;
memcpy(&data,sm_simulation_message->buf+sm_simulation_message->moff[k],sizeof(data));
changeObjPosByName(data.obj_name, data.pos, data.rot);
// ---------------------------------------------------------------------------
} else if (strcmp(name,"deleteObject") == 0) {
struct {
char obj_name[100];
} data;
memcpy(&data,sm_simulation_message->buf+sm_simulation_message->moff[k],sizeof(data));
deleteObjByName(data.obj_name);
// ---------------------------------------------------------------------------
} else if (strcmp(name,"status") == 0) {
status();
}
// ---------------------------------------------------------------------------
}
// give back semaphore
sm_simulation_message->n_msgs = 0;
sm_simulation_message->n_bytes_used = 0;
semGive(sm_simulation_message_sem);
return TRUE;
}
void
sendOscilloscopeData(void)
{
int i,j,r;
int count;
double temp = 0;
OscInfo *optr;
extern double servo_time;
if (!osc_enabled)
return;
// add data to the buffer
optr = osc_info_ptr;
while (optr != NULL) {
switch (optr->type) {
case DOUBLE:
temp = (float) *((double *) optr->ptr);
break;
case FLOAT:
temp = (float) *((float *) optr->ptr);
break;
case INT:
temp = (float) *((int *) optr->ptr);
break;
case SHORT:
temp = (float) *((short *) optr->ptr);
break;
case LONG:
temp = (float) *((long *) optr->ptr);
break;
case CHAR:
temp = (float) *((char *) optr->ptr);
break;
default:
printf("WARNING unknown data type in sendOscilloscopeData(void)\n");
}
addEntryOscBuffer(optr->name, temp, servo_time, optr->pID);
optr = (OscInfo *) optr->nptr;
}
// try to take semaphore with very short time-out -- we don't care if we
// cannot copy the data to shared memory every time, as this is not a time
// critical operation
if (semTake(sm_oscilloscope_sem,ns2ticks(TIME_OUT_SHORT_NS)) == ERROR)
return;
// copy first-in-first-out data into shared memory as long as there is enough
// space
if (sm_oscilloscope->n_entries+n_osc_entries > OSC_SM_BUFFER_SIZE) { // cannot copy all
count = OSC_SM_BUFFER_SIZE - sm_oscilloscope->n_entries;
} else
count = n_osc_entries;
j = sm_oscilloscope->n_entries;
for (i=1; i<=count; ++i) {
r = current_osc_index - n_osc_entries + i;
if ( r > OSC_BUFFER_SIZE)
r -= OSC_BUFFER_SIZE;
if ( r < 1)
r += OSC_BUFFER_SIZE;
sm_oscilloscope->entries[j+i] = oscbuf[r];
}
n_osc_entries -= count;
sm_oscilloscope->n_entries += count;
// give back semaphore
semGive(sm_oscilloscope_sem);
}
/*!*****************************************************************************
*******************************************************************************
\note run_simulation_servo
\date Nov 2007
\remarks
main functions executed during running the simulation servo
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
run_simulation_servo(void)
{
int i;
double k,kd;
double delta;
double dt;
int dtick;
double max_vel = 10.;
double aux;
static double last_time = 0;
static double current_time = 0;
// advance the simulation servo
++simulation_servo_calls;
simulation_servo_time += 1./(double)simulation_servo_rate;
servo_time = simulation_servo_time;
// check for missed calls to the servo
dtick = round((simulation_servo_time - last_simulation_servo_time)*(double)simulation_servo_rate);
if (dtick != 1 && simulation_servo_calls > 2) // need transient ticks to sync servos
simulation_servo_errors += abs(dtick-1);
// first, send out all the current state variables
// to shared memory
send_sim_state();
send_misc_sensors();
send_contacts();
// zero any external forces
bzero((void *)uext_sim,sizeof(SL_uext)*(n_dofs+1));
// read the commands
receive_des_commands();
// check for messages
checkForMessages();
// only after the write/read steps above, trigger the motor servo to avoid that
// the motor servo can read data that has the wrong time stamp
semGive(sm_motor_servo_sem);
// real-time processing if needed
#ifdef __XENO__
RTIME t = rt_timer_read();
current_time = (double)t/1.e9;
if (real_time) {
double delta_time;
delta_time = (1./(double)simulation_servo_rate-(current_time - last_time));
if (delta_time < 0)
delta_time = 0;
else
taskDelay(ns2ticks((long)(delta_time*1.e9)));
RTIME t = rt_timer_read();
current_time = (double)t/1.e9;
}
#else
struct timeval t;
gettimeofday(&t,NULL);
current_time = (double) t.tv_sec + ((double)t.tv_usec)/1.e6;
if (real_time) {
while (current_time - last_time < 1./(double)simulation_servo_rate) {
taskDelay(ns2ticks((long)(1./((double)simulation_servo_rate)*1000000000./5.)));
gettimeofday(&t,NULL);
current_time = (double) t.tv_sec + ((double)t.tv_usec)/1.e6;
}
}
#endif
last_time = current_time;
// check limits
for (i=1; i<=n_dofs; ++i) {
// use Geyer limited rebound model for (set aux=1 to avoid)
k = controller_gain_th[i]*10.0;
kd = controller_gain_thd[i]*sqrt(10.0);
delta = joint_sim_state[i].th - joint_range[i][MIN_THETA];
if ( delta < 0 ){ // only print at 10Hz
if ((int)(((double)simulation_servo_calls)/
(((double)simulation_servo_rate)/10.))%10 == 0) {
printf("-%s",joint_names[i]);
fflush(stdout);
}
if (joint_sim_state[i].thd > max_vel)
aux = 0.0;
else
aux = 1.-joint_sim_state[i].thd/max_vel;
joint_sim_state[i].u += - delta * k * aux - joint_sim_state[i].thd * kd * aux;
}
delta = joint_range[i][MAX_THETA] - joint_sim_state[i].th;
if (delta < 0){ // only print at 10Hz
if ((int)(((double)simulation_servo_calls)/
(((double)simulation_servo_rate)/10.))%10 == 0) {
printf("+%s",joint_names[i]);
fflush(stdout);
}
if (joint_sim_state[i].thd < -max_vel)
aux = 0.0;
else
aux = 1.-joint_sim_state[i].thd/(-max_vel);
joint_sim_state[i].u += delta * k * aux - joint_sim_state[i].thd * kd * aux;
}
}
// general numerical integration: integration runs at higher rate
dt = 1./(double)(simulation_servo_rate)/(double)n_integration;
for (i=1; i<=n_integration; ++i) {
// integrate the simulation
switch (integrate_method) {
case INTEGRATE_RK:
SL_IntegrateRK(joint_sim_state, &base_state,
&base_orient, ucontact, endeff,dt,n_dofs);
break;
case INTEGRATE_EULER:
SL_IntegrateEuler(joint_sim_state, &base_state,
&base_orient, ucontact, endeff,dt,n_dofs,TRUE);
break;
default:
printf("invalid integration method\n");
}
}
// compute miscellenous sensors
run_user_simulation();
// run user specific simulations
runUserSimulation();
// data collection
writeToBuffer();
last_simulation_servo_time = simulation_servo_time;
return TRUE;
}
