예제 #1
0
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;
}
예제 #2
0
/*!*****************************************************************************
*******************************************************************************
\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;
}
예제 #3
0
/*!*****************************************************************************
*******************************************************************************
\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;
}
예제 #4
0
/*!*****************************************************************************
 *******************************************************************************
\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;
}
예제 #5
0
/*!*****************************************************************************
*******************************************************************************
\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);

}
예제 #7
0
/*!*****************************************************************************
 *******************************************************************************
\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;
  
}
예제 #8
0
/*!*****************************************************************************
*******************************************************************************
\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;
}
예제 #9
0
파일: SL_go_cart_task.c 프로젝트: kralf/sl
/*!*****************************************************************************
 *******************************************************************************
\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;

}
예제 #10
0
/*!*****************************************************************************
 *******************************************************************************
\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;
}
예제 #11
0
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;
}
예제 #12
0
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;
}
예제 #13
0
/*!*****************************************************************************
*******************************************************************************
\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;
}
예제 #14
0
/*!*****************************************************************************
 *******************************************************************************
\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;
}
예제 #15
0
/*!*****************************************************************************
 *******************************************************************************
 \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;

}
예제 #16
0
파일: SL_dbvision.c 프로젝트: Riba7/hoap-sl
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;
}
예제 #18
0
/*!*****************************************************************************
 *******************************************************************************
\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);

}
예제 #19
0
/*!*****************************************************************************
 *******************************************************************************
\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;
}
예제 #20
0
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);
  
}
예제 #21
0
/*!*****************************************************************************
 *******************************************************************************
\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;

}