Example #1
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 */

  // 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);

  // use the simulated base state if required
  if (use_simulated_base_state)
    read_simulated_base();

  /*//sending
  char buf[BUFLEN];

  sprintf(buf, 'to je paket %d\n', 5);
  mainSend(buf);
  printf("send\n");*/
  return TRUE;
}
/**
 * NO MODE SWITCHES, NO RT REQUIREMENTS
 */
void imuDataToSLQuat(double* accel, double* angrate, double* orientation, SL_quat& sl_orient,
		SL_Cstate& sl_pos, double* unstab_accel, SL_Cstate& sl_pos_unstab)
{
  MY_MATRIX(sl_rot_mat,1,3,1,3);

  for(unsigned int i=0; i<9;i++)
  {
	// raw data is given in M1,1; M1,2; M1,3; M2,1; ...
	// we transpose to get it in world frame
    const unsigned int c = i/3;
    const unsigned int r = i%3;
    sl_rot_mat[r+1][c+1] = orientation[i];
  }

  // imu world needs to be transformed into SL world
	double rot_scales[4] = {0.0, -1.0, 1.0, -1.0};
	for(int r=1; r<=3; r++)
	  for(int c=1; c<=3; c++)
		  sl_rot_mat[r][c] *= rot_scales[r];

  // avoid using linkQuat
  {
		Eigen::Matrix3d eig_rot_mat;
    for(int r=1; r<=3; r++)
			for(int c=1; c<=3; c++)
				eig_rot_mat(r-1,c-1) = sl_rot_mat[r][c];
		Eigen::Quaterniond eig_orient(eig_rot_mat);
    sl_orient.q[_QW_] = eig_orient.w();
    sl_orient.q[_QX_] = eig_orient.x();
    sl_orient.q[_QY_] = eig_orient.y();
    sl_orient.q[_QZ_] = eig_orient.z();
	}
  //linkQuat(sl_rot_mat, &sl_orient);

  //transfrom angular rate and acceleration into world frame
  MY_VECTOR(sl_angrate, 1,3);
  MY_VECTOR(sl_accel, 1,3);
  MY_VECTOR(sl_accel_unstab, 1,3);
//  MY_VECTOR(sl_linvel, 1,3);
  for(unsigned int i=1; i<=3; i++)
  {
	  sl_angrate[i] = sl_rot_mat[i][1]*angrate[0] +
	    sl_rot_mat[i][2]*angrate[1] + sl_rot_mat[i][3]*angrate[2];

	  sl_accel[i] = sl_rot_mat[i][1]*accel[0] +
	    sl_rot_mat[i][2]*accel[1] + sl_rot_mat[i][3]*accel[2];

          sl_accel_unstab[i] = sl_rot_mat[i][0]*unstab_accel[0] +
              sl_rot_mat[i][1]*unstab_accel[1] + sl_rot_mat[i][2]*unstab_accel[2];

//	  sl_linvel[i] = sl_rot_mat[i][1]*lin_vel[0] +
//	    sl_rot_mat[i][2]*lin_vel[1] + sl_rot_mat[i][3]*lin_vel[2];
  }

  for(unsigned int i=1; i<=3; i++)
  {
    sl_orient.ad[i] = sl_angrate[i];
    sl_pos.xdd[i] = sl_accel[i];
    sl_pos_unstab.xdd[i] = sl_accel_unstab[i];
//    sl_pos.xd[i] = sl_linvel[i];
  }

//  quatDerivatives(&sl_orient);
}
Example #3
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;

}