Ejemplo n.º 1
0
// this is the method used in starlight aurora vsx, quite handy actually
void beginBlobs(vsx_module_engine_info* engine)
{
	glEnable(GL_TEXTURE_2D);
	GLfloat tmpMat[16];


  glGetFloatv(GL_MODELVIEW_MATRIX, blobMat);


  glMatrixMode(GL_PROJECTION);

	glPushMatrix();


  glGetFloatv(GL_PROJECTION_MATRIX, tmpMat);

	tmpMat[3] = 0;
	tmpMat[7] = 0;
	tmpMat[11] = 0;
	tmpMat[12] = 0;
	tmpMat[13] = 0;
	tmpMat[14] = 0;
	tmpMat[15] = 1;

	v_norm(tmpMat);
	v_norm(tmpMat + 4);
	v_norm(tmpMat + 8);

  glLoadIdentity();

  glMultMatrixf(tmpMat);

	blobMat[3] = 0;
	blobMat[7] = 0;
	blobMat[11] = 0;
	blobMat[12] = 0;
	blobMat[13] = 0;
	blobMat[14] = 0;
	blobMat[15] = 1;

	v_norm(blobMat);
	v_norm(blobMat + 4);
	v_norm(blobMat + 8);

  glMultMatrixf(blobMat);

  glGetFloatv(GL_PROJECTION_MATRIX, blobMat);

	glPopMatrix();

	glMatrixMode(GL_MODELVIEW);

	//inv_mat(blobMat, blobMatInv);

	GLfloat upLeft[] = {-0.5f, 0.5f, 0.0f, 1.0f};
	GLfloat upRight[] = {0.5f, 0.5f, 0.0f, 1.0f};

	mat_vec_mult(blobMat, upLeft, blobVec0);
	mat_vec_mult(blobMat, upRight, blobVec1);
}
Ejemplo n.º 2
0
/*!*****************************************************************************
 *******************************************************************************
\note  w2i
\date  Jul 2000, Tom
   
\remarks 

       transfer coordinate from the world to the images

 *******************************************************************************
 Function Parameters: [in]=input,[out]=output

 \param[in]     pos   :  3D position of an object
 \param[out]    xy_l :  projected 2D position of the object for the left eye
 \param[out]    xy_r :  projected 2D position of the object for the right eye

 ******************************************************************************/
int
w2i (Vector pos, Vector xy_l, Vector xy_r)
{
  Vector tmp;

  tmp = my_vector (1, 3);

  mat_vec_mult (Pl, pos, tmp);
  if (tmp[3] > 1.0e-4) {
    xy_l[1] = tmp[1]/tmp[3];
    xy_l[2] = tmp[2]/tmp[3];
  } else {
    printf ("numerical problem during calculation of a optical projection\n");
    return FALSE;
  }

  mat_vec_mult (Pr, pos, tmp);
  if (tmp[3] > 1.0e-4) {
    xy_r[1] = tmp[1]/tmp[3];
    xy_r[2] = tmp[2]/tmp[3];
  } else {
    printf ("numerical problem during calculation of a optical projection\n");
    return FALSE;
  }

  my_free_vector (tmp, 1,3);

  return TRUE;
}
Ejemplo n.º 3
0
Archivo: layout.c Proyecto: ekg/mars
static double* inv_mul_full(double* a, double* x, int n, mat u, mat v, mat q_t, mat r)
{
    int i;
    double* a_x = inv_mul_ax(a,x,n); //A^{-1}x
    double* tmp1 = mat_vec_mult(v,a_x); //VA^{-1}x
    double* tmp2 = mat_vec_mult(q_t,tmp1); //Q^TVA^{-1]x
    upper_tri_solve(r,tmp2); //R^{-1}Q^TVA^{-1}x
    free(tmp1);
    tmp1 = mat_vec_mult(u,tmp2); //UR^{-1}Q^TVA^{-1}x
    free(tmp2);
    tmp2 = inv_mul_ax(a,tmp1,n); //A^{-1}UR^{-1}Q^TVA^{-1}x
    for(i = 0; i < n; i++) { //A^{-1}x-A^{-1}UR^{-1}Q^TVA^{-1}x
        a_x[i] = a_x[i] - tmp2[i];  
    }
    free(tmp1);
    free(tmp2);
    return a_x;
}
void BaseStateEstimation::transformImuStateToBase()
{
	  MY_MATRIX(S_angrate, 1,3,1,3);
	  vectorToSkewMatrix(O_angrate_I_, S_angrate);
	  MY_MATRIX(S_angacc, 1,3,1,3);
	  vectorToSkewMatrix(O_angacc_I_, S_angacc);
	  MY_MATRIX(S_helper, 1,3,1,3);
	  mat_mult(S_angrate, S_angrate, S_helper);
	  mat_add(S_angacc, S_helper, S_helper);

	  mat_mult(O_rot_I_, I_rot_B_, O_rot_B_);

	  mat_vec_mult(O_rot_I_, I_linpos_B_, O_linpos_B_);
	  vec_add(O_linpos_I_, O_linpos_B_, O_linpos_B_);

	  mat_vec_mult(O_rot_I_, I_linpos_B_, O_linvel_B_);
	  mat_vec_mult(S_angrate, O_linvel_B_, O_linvel_B_);
	  vec_add(O_linvel_I_, O_linvel_B_, O_linvel_B_);

	  mat_vec_mult(O_rot_I_, I_linpos_B_, O_linacc_B_);
	  mat_vec_mult(S_helper, O_linacc_B_, O_linacc_B_);
	  vec_add(O_linacc_I_, O_linacc_B_, O_linacc_B_);
}
Ejemplo n.º 5
0
/* Adds correlated noise to a vector. Supplied with a square root of covariance matrix
 * TODO: supply covariance only
 *
 */
void add_noise(Vector vec, Matrix std) {

	int nr = std[0][0]; //number of rows
	//printf("Vector length: %d\n",nr);
	Vector noise = my_vector(1,nr);
	int i;

	for (i = 1; i <= nr; i++) {
		noise[i] = gaussian(noise[i],1.0);
	}

	mat_vec_mult(std,noise,noise);

	for (i = 1; i <= nr; i++) {
		vec[i] += noise[i];
	}

	return;

}
Ejemplo n.º 6
0
std::vector<double> MATHNS::rotate_vector(std::vector<double> X, std::string axis, double angle)
{
    std::vector<double> rotated;
    double **rotMatrix;
    rotMatrix = mmemory->new_2d<double>(3,3);
    if (axis=="x") {
        rotMatrix[0][0] = 1;  rotMatrix[0][1] = 0;              rotMatrix[0][2] =  0;
        rotMatrix[1][0] = 0;  rotMatrix[1][1] = cos(angle*D2R); rotMatrix[1][2] = -sin(angle*D2R);
        rotMatrix[2][0] = 0;  rotMatrix[2][1] = sin(angle*D2R); rotMatrix[2][2] =  cos(angle*D2R);
    } else if (axis=="y") {
        rotMatrix[0][0] = cos(angle*D2R);  rotMatrix[0][1] = 0; rotMatrix[0][2] = sin(angle*D2R);
        rotMatrix[1][0] = 0;               rotMatrix[1][1] = 1; rotMatrix[1][2] = 0;
        rotMatrix[2][0] = -sin(angle*D2R); rotMatrix[2][1] = 0; rotMatrix[2][2] = cos(angle*D2R);
    } else if (axis=="z") {
        rotMatrix[0][0] = cos(angle*D2R);  rotMatrix[0][1] = -sin(angle*D2R); rotMatrix[0][2] = 0;
        rotMatrix[1][0] = sin(angle*D2R);  rotMatrix[1][1] =  cos(angle*D2R); rotMatrix[1][2] = 0;
        rotMatrix[2][0] = 0;               rotMatrix[2][1] =  0;              rotMatrix[2][2] = 1;
    }

    rotated = mat_vec_mult(3,rotMatrix,X);

    mmemory->destroy(rotMatrix);
    return rotated;
}
Ejemplo n.º 7
0
int steepest_solver (
	int n,
	double ** A,
	double * b,
	double * x
) {

	int row, col;
	double **AT, *ATb, **ATA;
	double *residual;

	AT = (double**) malloc (n*sizeof(double*));
	ATb = (double*) malloc (n*sizeof(double));
	ATA = (double**) malloc (n*sizeof(double*));
	residual = (double*) malloc (n*sizeof(double));
	
	// allocate
	for (row=0; row<n; row++) {
		AT[row] = (double*) malloc(n*sizeof(double));
		ATA[row] = (double*) malloc(n*sizeof(double));
	}

	// aux variables
	transpose (n, A, AT);
	mat_mat_mult (n, AT, A, ATA);
	mat_vec_mult (n, AT, b, ATb);

	// x is the vector that minimizes f(x) = 0.5(x^T)ATAx - ((ATb)^T)x
	// initialize
	int step = 0;
	for (row=0; row<n; row++) {
		residual[row] = ATb[row];
		for (col=0; col<n; col++)
			residual[row] -= ATA[row][col]*x[col]; // in HCDC, x elements are non constant all but row number of terms are constant
	}

	// delta = rTr;
	double delta = vec_vec_mult (n,residual,residual);
	double delta_0 = delta;

	do {
		double alpha = delta / vec_mat_vec_mult (n, residual, ATA);// + DBL_EPSILON;
		for (row=0; row<n; row++)
			x[row] += alpha * residual[row];
		for (row=0; row<n; row++) {
			residual[row] = ATb[row];
			for (col=0; col<n; col++)
				residual[row] -= ATA[row][col]*x[col]; // in HCDC, x elements are non constant all but row number of terms are constant
		}
		delta = vec_vec_mult (n,residual,residual);
		step++;
	} while (
		delta>DBL_EPSILON*delta_0
		&& step<(1<<20)
	);

	for (row=0; row<n; row++) {
		free(AT[row]);
		free(ATA[row]);
	}
	free (AT);
	free (ATA);
	free (ATb);
	free (residual);

	return step;

}
Ejemplo n.º 8
0
/*!*****************************************************************************
 *******************************************************************************
\note  compute_force_sensors
\date  March 2003
   
\remarks 

      This function computes simulated force sensor information for both
      feet. The results are force and torque values for in link coordinates
      of the last ankle joint. This informatin can be extracted from the
      contact force computation, which has world coordinates. Thus, only
      a simple coordinate transformatino from world to local link coordinates
      is needed. A small correction is need to transform the force/torque
      information to the exact position of the load cell center.

 *******************************************************************************
 Function Parameters: [in]=input,[out]=output

     All results can be computed based on global variables. Results are
     assigned to the misc_sensor structures.


 ******************************************************************************/
static void
compute_force_sensors(void)

{
  int i,j;
  static int firsttime = TRUE;
  static Matrix M;
  static Vector v;
  static Vector vv;
  static Vector r;

  if (firsttime) {
    firsttime = FALSE;
    M = my_matrix(1,N_CART,1,N_CART);
    v = my_vector(1,N_CART);
    vv= my_vector(1,N_CART);
    r = my_vector(1,N_CART);
  }

  // rotation matrix from world to L_AAA coordinates 
  mat_trans_size(Adof_sim[L_AAA],N_CART,N_CART,M);

  // transform forces
  for (i=1; i<=N_CART; ++i)
    v[i] = ucontact[L_AAA].f[i] - uext_sim[L_AAA].f[i];
  mat_vec_mult(M,v,vv);

  misc_sim_sensor[L_CFx] = vv[_X_];
  misc_sim_sensor[L_CFy] = vv[_Y_];
  misc_sim_sensor[L_CFz] = vv[_Z_];

  // transform torques
  for (i=1; i<=N_CART; ++i)
    v[i] = ucontact[L_AAA].t[i] - uext_sim[L_AAA].t[i];
  mat_vec_mult(M,v,v);

  // correct for load cell offset (need to use local coordinates)
  r[_X_] = FTA_Z_OFF;
  r[_Y_] = FTA_X_OFF;
  r[_Z_] = -FTA_Y_OFF;
  vec_mult_outer(vv,r,vv);

  misc_sim_sensor[L_CTa] = v[_A_] + vv[_A_];
  misc_sim_sensor[L_CTb] = v[_B_] + vv[_B_];
  misc_sim_sensor[L_CTg] = v[_G_] + vv[_G_];

  // rotation matrix from world to R_AAA coordinates
  mat_trans_size(Adof_sim[R_AAA],N_CART,N_CART,M);

  // transform forces
  for (i=1; i<=N_CART; ++i)
    v[i] = ucontact[R_AAA].f[i] - uext_sim[R_AAA].f[i];
  mat_vec_mult(M,v,vv);

  misc_sim_sensor[R_CFx] = vv[_X_];
  misc_sim_sensor[R_CFy] = vv[_Y_];
  misc_sim_sensor[R_CFz] = vv[_Z_];

  // transform torques
  for (i=1; i<=N_CART; ++i)
    v[i] = ucontact[R_AAA].t[i] - uext_sim[R_AAA].t[i];
  mat_vec_mult(M,v,v);

  // correct for load cell offset (need to use local coordinates)
  r[_X_] = FTA_Z_OFF;
  r[_Y_] = FTA_X_OFF;
  r[_Z_] = FTA_Y_OFF;
  vec_mult_outer(vv,r,vv);

  misc_sim_sensor[R_CTa] = v[_A_] + vv[_X_];
  misc_sim_sensor[R_CTb] = v[_B_] + vv[_Y_];
  misc_sim_sensor[R_CTg] = v[_G_] + vv[_Z_];

}
Ejemplo n.º 9
0
/*!*****************************************************************************
 *******************************************************************************
\note  compute_inertial
\date  Oct 2005
   
\remarks 

      This functions computes simulated inertial signals, i.e., body 
      quaternion, angular velocity, and linear acceleration, and 
      linear acceleration at the feet

 *******************************************************************************
 Function Parameters: [in]=input,[out]=output

     All results can be computed based on global variables. Results are
     assigned to the misc_sensor structures.


 ******************************************************************************/
static void
compute_inertial(void)

{
  int i,j;
  static int firsttime = TRUE;
  static Matrix R;
  static Matrix RT;
  static Vector ad;
  static Vector xdd;
  static Vector r;
  static Vector r0;
  static Vector t1;
  static Vector t2;
  static Vector t3;
  static Vector lv;
  static Vector rv;
  static Vector lv_last;
  static Vector rv_last;
  static Vector lacc;
  static Vector racc;


  if (firsttime) {
    R   = my_matrix(1,N_CART,1,N_CART);
    RT  = my_matrix(1,N_CART,1,N_CART);
    ad  = my_vector(1,N_CART);
    xdd = my_vector(1,N_CART);
    r   = my_vector(1,N_CART);
    r0  = my_vector(1,N_CART);
    t1  = my_vector(1,N_CART);
    t2  = my_vector(1,N_CART);
    t3  = my_vector(1,N_CART);
    lv  = my_vector(1,N_CART);
    rv  = my_vector(1,N_CART);
    lacc= my_vector(1,N_CART);
    racc= my_vector(1,N_CART);
    lv_last  = my_vector(1,N_CART);
    rv_last  = my_vector(1,N_CART);
    firsttime = FALSE;
  }

  // copy quaternion from base coordinates (perfect data)
  misc_sim_sensor[B_Q0_IMU] = base_orient.q[_Q0_];
  misc_sim_sensor[B_Q1_IMU] = base_orient.q[_Q1_];
  misc_sim_sensor[B_Q2_IMU] = base_orient.q[_Q2_];
  misc_sim_sensor[B_Q3_IMU] = base_orient.q[_Q3_];


  // the angular vel. and translatory acc are first computed in world coordinates
  // and then transformed to base coordinates.

  // need the base coordinate rotatin matrix, which transforms world to base
  quatToRotMat(&base_orient,R);
  mat_trans(R,RT);

  // offset is IMU_X_OFFSET and -IMU_Y_OFFSET
  r[_X_] = IMU_X_OFFSET;
  r[_Y_] = -IMU_Y_OFFSET;
  r[_Z_] = 0.0;

  // get offset vector in global coordinates
  mat_vec_mult(RT,r,r0);

  // w x r0
  vec_mult_outer_size(base_orient.ad,r0,N_CART,t1);

  // w x (w x r0) = w x t1 (which is one term of xdd)
  vec_mult_outer_size(base_orient.ad,t1,N_CART,t2);

  // wd x r0
  vec_mult_outer_size(base_orient.add,r0,N_CART,t1);  

  // add to xdd
  vec_add(t1,t2,t1);

  // and add the base acceleration
  vec_add_size(t1,base_state.xdd,N_CART,t1);

  // add gravity to t1
  t1[_Z_] -= gravity;

  // rotate all into base coordinates
  mat_vec_mult_size(R,N_CART,N_CART,base_orient.ad,N_CART,t3);
  mat_vec_mult_size(R,N_CART,N_CART,t1,N_CART,t2);

  // and rotate this information into IMU coordinates
  // IMU_X = -Z_BASE; IMU_Y = -X_BASE; IMU_Z = Y_BASE

  t1[_A_] = -PI/2.;
  t1[_B_] = 0.0;
  t1[_G_] = PI/2.;
  eulerToRotMat(t1,R);
  mat_vec_mult(R,t2,xdd);
  mat_vec_mult(R,t3,ad);

  // the final result
  misc_sim_sensor[B_AD_A_IMU] = ad[_A_];
  misc_sim_sensor[B_AD_B_IMU] = ad[_B_];
  misc_sim_sensor[B_AD_G_IMU] = ad[_G_];

  misc_sim_sensor[B_XACC_IMU] = xdd[_X_];
  misc_sim_sensor[B_YACC_IMU] = xdd[_Y_];
  misc_sim_sensor[B_ZACC_IMU] = xdd[_Z_];


  // now get the foot accelerations in WORLD coordinate, computed at the L_FOOT position,
  // which is not exactly the same as the load cell offset, but very close.
  computeLinkVelocity(L_FOOT, link_pos_sim, joint_origin_pos_sim, 
		      joint_axis_pos_sim, joint_sim_state, lv);

  computeLinkVelocity(R_FOOT, link_pos_sim, joint_origin_pos_sim, 
		      joint_axis_pos_sim, joint_sim_state, rv);

  for (i=1; i<=N_CART; ++i) {
    lacc[i] = (lv[i]-lv_last[i])*(double)simulation_servo_rate;
    lv_last[i] = lv[i];
    racc[i] = (rv[i]-rv_last[i])*(double)simulation_servo_rate;
    rv_last[i] = rv[i];
  }

  // transform to local foot coordinates after adding gravity
  lacc[_Z_] -= gravity;
  racc[_Z_] -= gravity;

  // rotation matrix from world to L_AAA coordinates 
  mat_trans_size(Alink_sim[L_FOOT],N_CART,N_CART,R);
  mat_vec_mult(R,lacc,t1);

  // rotation matrix from world to R_AAA coordinates 
  mat_trans_size(Alink_sim[R_FOOT],N_CART,N_CART,R);
  mat_vec_mult(R,racc,t2);

#ifdef HAS_LOWER_BODY
  // the final result
  misc_sim_sensor[L_FOOT_XACC] = t1[_X_];
  misc_sim_sensor[L_FOOT_YACC] = t1[_Y_];
  misc_sim_sensor[L_FOOT_ZACC] = t1[_Z_];

  misc_sim_sensor[R_FOOT_XACC] = t2[_X_];
  misc_sim_sensor[R_FOOT_YACC] = t2[_Y_];
  misc_sim_sensor[R_FOOT_ZACC] = t2[_Z_];
#endif
}
Ejemplo n.º 10
0
/*!*****************************************************************************
 *******************************************************************************
\note  parm_opt
\date  10/20/91

\remarks 
		this is the major optimzation program

 *******************************************************************************
 Function Parameters: [in]=input,[out]=output

 \param[in]     tol      : error tolernance to be achieved
 \param[in]     n_parm   : number of parameters to be optimzed
 \param[in]     n_con    : number of contraints to be taken into account
 \param[in]     f_dLda   : function which calculates the derivative of the 
                    optimziation criterion with respect to the parameters;
		    must return vector
 \param[in]     f_dMda   : function which calculates the derivate of the 
                    constraints with respect to parameters
		    must return matrix
 \param[in]     f_M      : constraints function, must always be formulted to
                    return 0 for properly fulfilled constraints
 \param[in]     f_L      : function to calculate simple cost (i.e., constraint
	            cost NOT included), the constraint costs are added
		    by this program automatically, the function returns
		    a double scalar
 \param[in]     f_dLdada : second derivative of L with respect to the parameters, 
                    must be a matrix of dim n_parm x n_parm
 \param[in]     f_dMdada : second derivative of M with respect to parameters,
                    must be a matrix n_con*n_parm x n_parm
 \param[in]     use_newton: TRUE or FALSE to indicate that second derivatives
                    are given and can be used for Newton algorithm
 \param[in,out] a        : initial setting of parameters and also return of
                    optimal value (must be a vector, even if scalar)
 \param[out]    final_cost: the final cost
 \param[out]    err       : the sqrt of the squared error of all constraints
      				
      NOTE: - program returns TRUE if everything correct, otherwise FALSE
      			- always minimizes the cost!!!
      			-	algorithms come from Dyer McReynolds
      			
      NOTE: besides the possiblity of a bug, the Newton method seems to
      			sacrifice the validity of the constraint a little up to
      			quite a bit and should be used prudently

 ******************************************************************************/
int
parm_opt(double *a,int n_parm, int n_con, double *tol, void (*f_dLda)(), 
	void (*f_dMda)(), void (*f_M)(), double (*f_L)(), void (*f_dMdada)(),
	void (*f_dLdada)(), int use_newton, double *final_cost, double *err)

{
  
  register int i,j,n;
  double cost= 999.e30;
  double last_cost = 0.0;
  double *mult=NULL, *new_mult=NULL; /* this is the vector of Lagrange mulitplier */
  double **dMda=NULL, **dMda_t=NULL;
  double *dLda;
  double *K=NULL; /* the error in the constraints */
  double eps = 0.025; /* the learning rate */
  double **aux_mat=NULL; /* needed for inversion of matrix */
  double *aux_vec=NULL;
  double *new_a;
  double **dMdada=NULL;
  double **dLdada=NULL;
  double **A=NULL; /* big matrix, a combination of several other matrices */
  double *B=NULL;  /* a big vector */
  double **A_inv=NULL;
  int    rc=TRUE;
  long   count = 0;
  int    last_sign = 1;
  int    pending1 = FALSE, pending2 = FALSE;
  int    firsttime = TRUE;
  int    newton_active = FALSE;
  
  dLda    = my_vector(1,n_parm);
  new_a   = my_vector(1,n_parm);
  if (n_con > 0) {
    mult    = my_vector(1,n_con);
    dMda    = my_matrix(1,n_con,1,n_parm);
    dMda_t  = my_matrix(1,n_parm,1,n_con);
    K       = my_vector(1,n_con);
    aux_mat = my_matrix(1,n_con,1,n_con);
    aux_vec = my_vector(1,n_con);
  }

  
  if (use_newton) {
    
    dLdada   = my_matrix(1,n_parm,1,n_parm);
    A        = my_matrix(1,n_parm+n_con,1,n_parm+n_con);
    A_inv    = my_matrix(1,n_parm+n_con,1,n_parm+n_con);
    B        = my_vector(1,n_parm+n_con);

    if (n_con > 0) {
      dMdada   = my_matrix(1,n_con*n_parm,1,n_parm);
      new_mult = my_vector(1,n_con);
    }
    
    for (i=1+n_parm; i<=n_con+n_parm; ++i) {
      for (j=1+n_parm; j<=n_con+n_parm; ++j) {
	A[i][j] = 0.0;
      }
    }
    
  }
  
  
  while (fabs(cost-last_cost) > *tol) {
    
    ++count;
    pending1 = FALSE;
    pending2 = FALSE;
    
  AGAIN:
    
    /* calculate the current Lagrange multipliers */
    
    if (n_con > 0) {
      (*f_M)(a,K);       /* takes the parameters, returns residuals */
      (*f_dMda)(a,dMda); /* takes the parameters, returns the Jacobian */
    }
    (*f_dLda)(a,dLda); /* takes the parameters, returns the gradient */
    
    if (n_con > 0) {
      mat_trans(dMda,dMda_t);
    }
    
    
    if (newton_active) {
      if (n_con > 0) {
	(*f_dMdada)(a,dMdada);
      }
      (*f_dLdada)(a,dLdada);
    }
    
    
    /* the first step is always a gradient step */
    
    if (newton_active) {
      
      if (firsttime) {
	firsttime = FALSE;
	eps = 0.1; 
      }
      
      
      /* build the A matrix */
      
      for (i=1; i<=n_parm; ++i) {
	for (j=1; j<=n_parm; ++j) {
	  
	  A[i][j] = dLdada[i][j];
	  
	  for (n=1; n<=n_con; ++n) {
	    A[i][j] += mult[n]*dMdada[n+(i-1)*n_con][j];
	  }
	  
	}
      }
      
      
      for (i=1+n_parm; i<=n_con+n_parm; ++i) {
	for (j=1; j<=n_parm; ++j) {
	  A[j][i] = A[i][j] = dMda[i-n_parm][j];
	}
      }
      
      
      /* build the B vector */
      
      if (n_con > 0) {
	mat_vec_mult(dMda_t,mult,B);
      }
      
      
      for (i=1; i<=n_con; ++i) {
	B[i+n_parm] = K[i];
      }
      
      /* invert the A matrix */
      
      if (!my_inv_ludcmp(A, n_con+n_parm, A_inv)) {
	rc = FALSE;
	break;
      }
      
      mat_vec_mult(A_inv,B,B);
      vec_mult_scalar(B,eps,B);
      
      for (i=1; i<=n_parm; ++i) {
	new_a[i] = a[i] + B[i];
      }
      
      for (i=1; i<=n_con; ++i) {
	new_mult[i] = mult[i] + B[n_parm+i];
      }
      
      
      
    } else {
      
      
      if (n_con > 0) {

	/* the mulitpliers are updated according:
	   mult = (dMda dMda_t)^(-1) (K/esp - dMda dLda_t)       */

	mat_mult(dMda,dMda_t,aux_mat);
      
	if (!my_inv_ludcmp(aux_mat, n_con, aux_mat)) {
	  rc = FALSE;
	  break;
	}
      
	mat_vec_mult(dMda,dLda,aux_vec);
      
	vec_mult_scalar(K,1./eps,K);
      
	vec_sub(K,aux_vec,aux_vec);
      
	mat_vec_mult(aux_mat,aux_vec,mult);

      }
      
      
      /* the update step looks the following:
	 a_new = a - eps * (dLda + mult_t * dMda)_t     */
      
      if (n_con > 0) {

	vec_mat_mult(mult,dMda,new_a);
      
	vec_add(dLda,new_a,new_a);

      } else {

	vec_equal(dLda,new_a);

      }
      
      vec_mult_scalar(new_a,eps,new_a);
      
      vec_sub(a,new_a,new_a);
      
    }
    
    
    if (count == 1 && !pending1) {
      
      last_cost  = (*f_L)(a);

      if (n_con > 0) {
	(*f_M)(a,K);
	last_cost += vec_mult_inner(K,mult);
      }
      
    } else {
    
      last_cost = cost;

    }
    
    /* calculate the updated cost */
    
    
    cost = (*f_L)(new_a);
    /*printf("   %f\n",cost);*/

    if (n_con > 0) {

      (*f_M)(new_a,K);
      
      if (newton_active) {
	cost += vec_mult_inner(K,new_mult);
      } else {
	cost += vec_mult_inner(K,mult);
      }

    }
    
    /* printf("last=%f new=%f\n",last_cost,cost); */
    
    
    /* check out whether we reduced the cost */
    
    if (cost > last_cost && fabs(cost-last_cost) > *tol) {
      
      /* reduce the gradient climbing rate: sometimes a reduction of eps
	 causes an increase in cost, thus leave an option to increase
	 eps */
      
      cost = last_cost; /* reset last_cost */
      
      
      if (pending1 && pending2) {
	
	/* this means that either increase nor decrease
	   of eps helps, ==> leave the program */
	
	rc = TRUE;
	break;
	
      } else if (pending1) {
	
	eps *= 4.0;  /* the last cutting by half did not help, thus
			multiply by 2 to get to previous value, and
			one more time by 2 to get new value */
	pending2 = TRUE;
	
      } else {
	
	eps /= 2.0;
	pending1 = TRUE;
	
      }
      
      goto AGAIN;
      
    } else {
      
      vec_equal(new_a,a);
      if (newton_active && n_con > 0) {
	vec_equal(new_mult,mult);
      }
      if (use_newton && fabs(cost-last_cost) < NEWTON_THRESHOLD) 
	newton_active = TRUE;
      
    }
    
    
  }
  
  my_free_vector(dLda,1,n_parm);
  my_free_vector(new_a,1,n_parm);
  if (n_con > 0) {
    my_free_vector(mult,1,n_con);
    my_free_matrix(dMda,1,n_con,1,n_parm);
    my_free_matrix(dMda_t,1,n_parm,1,n_con);
    my_free_vector(K,1,n_con);
    my_free_matrix(aux_mat,1,n_con,1,n_con);
    my_free_vector(aux_vec,1,n_con);
  }
  
  if (use_newton) {
    
    my_free_matrix(dLdada,1,n_parm,1,n_parm);
    my_free_matrix(A,1,n_parm+n_con,1,n_parm+n_con);
    my_free_matrix(A_inv,1,n_parm+n_con,1,n_parm+n_con);
    my_free_vector(B,1,n_parm+n_con);
    if (n_con > 0) {
      my_free_matrix(dMdada,1,n_con*n_parm,1,n_parm);
      my_free_vector(new_mult,1,n_con);
    }
    
  }
  *final_cost = cost;
  *tol = fabs(cost-last_cost);
  if (n_con > 0) {
    *err = sqrt(vec_mult_inner(K,K));
  } else {
    *err = 0.0;
  }
/*  
  printf("count=%ld  rc=%d\n",count,rc);
*/ 
  return rc;
  
}
Ejemplo n.º 11
0
int lob_scaat(SCAATState *out,
		STORAGE_TYPE z, STORAGE_TYPE s_x, STORAGE_TYPE s_y, STORAGE_TYPE dt,
		SCAATState *in,
		STORAGE_TYPE q, STORAGE_TYPE R){

	STORAGE_TYPE **A;
	STORAGE_TYPE **Q;
	STORAGE_TYPE **Q1; // temporary matrix
	STORAGE_TYPE opt_z=0.0;
	STORAGE_TYPE *H;
	STORAGE_TYPE *K; //Kalman gain
	STORAGE_TYPE **I; //unit matrix

	STORAGE_TYPE **v; // for the eigenvectors
	int    *nrot; // for the Jacobi method of eigenvalue computation
	STORAGE_TYPE *d;

	SCAATState pred; // prediction

	int i, j;
	int add_noise = 0;

	// section for temporary variables
	STORAGE_TYPE *t_v1; // vector

	STORAGE_TYPE **t_m1; // matrix
	STORAGE_TYPE **t_m2; // matrix
	STORAGE_TYPE **t_m3; // matrix

	STORAGE_TYPE t_s; //temporary scalar value
	STORAGE_TYPE temp_sum = 0.0;
	STORAGE_TYPE min_d = 0.0;


	// program start
	//A=[1 0 dt  0 dt*dt/2       0
	//   0 1  0 dt       0 dt*dt/2
	//   0 0  1  0      dt       0
	//   0 0  0  1       0      dt
	//   0 0  0  0       1       0
	//   0 0  0  0       0       1];

	pred.x = VECTOR(1, STATE_NUM);
	//pred.P = MATRIX(1, STATE_NUM, 1, STATE_NUM);
	pred.P = dmatrix_alloc(STATE_NUM,STATE_NUM);

	t_v1 = VECTOR(1, STATE_NUM);
	//t_m1 = MATRIX(1, STATE_NUM, 1, STATE_NUM);
	//t_m2 = MATRIX(1, STATE_NUM, 1, STATE_NUM);
	//t_m3 = MATRIX(1, STATE_NUM, 1, STATE_NUM);

	t_m1 = dmatrix_alloc(STATE_NUM,STATE_NUM);
	t_m2 = dmatrix_alloc(STATE_NUM,STATE_NUM);
	t_m3 = dmatrix_alloc(STATE_NUM,STATE_NUM);

	//v = MATRIX(1, STATE_NUM, 1, STATE_NUM);
	v  = dmatrix_alloc(STATE_NUM,STATE_NUM);
	nrot = ivector(1, STATE_NUM);
	d = VECTOR(1, STATE_NUM);

	//A = MATRIX(1, STATE_NUM, 1, STATE_NUM);
	//Q = MATRIX(1, STATE_NUM, 1, STATE_NUM);
	//Q1 = MATRIX(1, STATE_NUM, 1, STATE_NUM);
	A = dmatrix_alloc(STATE_NUM,STATE_NUM);
	Q = dmatrix_alloc(STATE_NUM,STATE_NUM);
	Q1 = dmatrix_alloc(STATE_NUM,STATE_NUM);

	H = VECTOR(1, STATE_NUM);
	K = VECTOR(1, STATE_NUM);
	//I = MATRIX(1, STATE_NUM, 1, STATE_NUM);
	I = dmatrix_alloc(STATE_NUM,STATE_NUM);


	// Initialize matrices A and Q
	for (i=1;i<=STATE_NUM;i++){
		K[i] = 0.0;
		for (j=1;j<=STATE_NUM;j++){
			A[i][i] = 0;
			Q[i][i] = 0;
			Q1[i][i] = 0;
			if (i==j)
				I[i][j]= 1.0;
			else
				I[i][j]= 0.0;

		}
	}

	for (i=1;i<=STATE_NUM;i++){
		A[i][i] = 1;
		if (i+2<=STATE_NUM)
			A[i][i+2] = dt;
		if (i+4<=STATE_NUM)
			A[i][i+4] = dt*dt/2.0;
	}



//% The state error covariance matrix is a function of the driving
//% error which is assumed to only hit the accelaration directly.
//% Indirectly this noise affects the velocity and position estimate
//% through the dynamics model. 

//Q=q*[(dt^5)/20         0 (dt^4)/8        0  (dt^3)/6         0
//            0 (dt^5)/20        0 (dt^4)/8         0  (dt^3)/6   
//      (dt^4)/8         0 (dt^3)/3        0  (dt^2)/2         0
//             0  (dt^4)/8        0 (dt^3)/3         0  (dt^2)/2
//     (dt^3)/6         0  (dt^2)/2       0        dt         0
//             0  (dt^3)/6         0  (dt^2)/2       0        dt];


	Q[1][1] = q*pow(dt, 5)/20.0;
	Q[1][3] = q*pow(dt, 4)/8.0;
	Q[1][5] = q*pow(dt, 3)/6.0;

	Q[2][2] = q*pow(dt, 5)/20.0;
	Q[2][4] = q*pow(dt, 4)/8.0;
	Q[2][6] = q*pow(dt, 3)/6.0;

	Q[3][1] = q*pow(dt, 4)/8.0;
	Q[3][3] = q*pow(dt, 3)/3.0;
	Q[3][5] = q*pow(dt, 2)/2.0;

	Q[4][2] = q*pow(dt, 4)/8.0;
	Q[4][4] = q*pow(dt, 3)/3.0;
	Q[4][6] = q*pow(dt, 2)/2.0;

	Q[5][1] = q*pow(dt, 3)/6.0;
	Q[5][3] = q*pow(dt, 2)/2.0;
	Q[5][5] = q*dt;

	Q[6][2] = q*pow(dt, 3)/6.0;
	Q[6][4] = q*pow(dt, 2)/2.0;
	Q[6][6] = q*dt;

	/*for (i=1;i<=STATE_NUM;i++) {
	for (j=1;j<=STATE_NUM;j++)
		printf("%f ",Q[i][j]);
	 printf("\n");
	}*/
	//%step b
	//pred_x=A*pre_x;  %6 x 1
	mat_vec_mult(pred.x, A, in->x, STATE_NUM, 0);
	//pred_P=A*pre_P*(A')+Q;  %6 x 6
	// A is transpose
	mat_mat_mult(t_m1, in->P, A, STATE_NUM, 0, 1);
	mat_mat_mult(t_m2, A, t_m1, STATE_NUM, 0, 0);
	mat_add(pred.P, t_m2, Q, STATE_NUM, 0);

	// %step c
	// % assume that 0<=z<2*pi 
	// opt_z = 2*pi + atan2((pred_x(2)-s_y), (pred_x(1)-s_x)); %1 x 1
	// if (opt_z>2*pi)
	//     opt_z = opt_z - 2*pi;
	// end
	// %H is 1 x 6
	// % measurement function is nonlinear 
	// % 
	// % z = atan(x(2) - s_y,  x(1) - s_x);
	// % In order to linearize the problem we find the jacobian
	// % noticing that 
	// %
	// % d(atan(x))/dx = 1/(1+x^2)
	// %
	// H=[ -(pred_x(2)-s_y)/(( (pred_x(1)-s_x)^2+(pred_x(2)-s_y)^2 )), (pred_x(1)-s_x)/(( (pred_x(1)-s_x)^2+(pred_x(2)-s_y)^2 )), 0, 0, 0, 0];

	opt_z = 2*PI + atan2((pred.x[2]-s_y), (pred.x[1]-s_x));
	
	H[1] = -(pred.x[2]-s_y)/ ( (pred.x[1]-s_x)*(pred.x[1]-s_x)+(pred.x[2]-s_y)*(pred.x[2]-s_y) );
	H[2] = (pred.x[1]-s_x) / ( (pred.x[1]-s_x)*(pred.x[1]-s_x)+(pred.x[2]-s_y)*(pred.x[2]-s_y) );
	H[3] = 0.0; H[4] = 0.0; H[5] = 0.0; H[6] = 0.0;


	//test=find(isnan(H)==1);
	//if (~isempty(test))
	//	disp('H goes to infinity due to node position is the same as the predicted value')
	//	return;
	//end
	if (isnan(H[1]) || isnan(H[1])){
			fprintf(stderr, "H goes to infinity due to node position is the same as the predicted value\n");
			return(-1);
	}


	//%step d
	//%also test if P is still a positive definite matrix.  If not, add some small noise to it to
	//%make it still positive.
	//n1=1;
	//Q1=n1*eye(6);
	//Q1(1,1)=0.1;
	//Q1(2,2)=0.1;
	//Q1(3,3)=5;
	//Q1(4,4)=5;

	Q1[1][1] = 0.1;
	Q1[2][2] = 0.1;
	Q1[3][3] = 5.0;
	Q1[4][4] = 5.0;
	Q1[5][5] = 1.0;
	Q1[6][6] = 1.0;

	//if isempty(find(eig(pred_P)<0.001))
	//   K=pred_P*H'*inv(H*pred_P*H' + R);   %6 x 1
	//else
	//   pred_P=pred_P+Q1;
	//    K=pred_P*H'*inv(H*pred_P*H' + R);   %6 x 1
	//end

	add_noise=0;
	// jacobi destroys the input vector
	mat_copy(t_m2, pred.P, STATE_NUM, 0);
	jacobi(t_m2, STATE_NUM, d, v, nrot);
	for(i=1;i<=STATE_NUM;i++){
		if (d[i]< 0.001)
			add_noise=1;
	}

	if (add_noise){
		mat_add(t_m1, pred.P, Q1, STATE_NUM, 0);
		mat_copy(pred.P, t_m1, STATE_NUM, 0);
	}

	mat_vec_mult(t_v1, pred.P, H, STATE_NUM, 1);
	inner_prod(&t_s, t_v1, H, STATE_NUM);
	t_s+= R;
	scal_vec_mult(K, 1.0/t_s, t_v1, STATE_NUM);

	//%step e and f
	//I=eye(6);

	//opt_x=pred_x+K*(z-opt_z);   %6 x 1
	scal_vec_mult(t_v1, z - opt_z, K, STATE_NUM);
	vec_add(out->x, pred.x, t_v1, STATE_NUM);
	// H: 1 x 6, K: 6 x 1
	//%Joseph form to ward off round off problem
	//opt_P=(I-K*H)*pred_P*(I-K*H)'+K*R*K';       %6 x 6
	scal_vec_mult(t_v1, R, K, STATE_NUM); // R*K'
	outer_prod(t_m1, K, t_v1, STATE_NUM); // K*(R*K')

	outer_prod(t_m2, K, H, STATE_NUM); // K*H
	mat_add(t_m3, I, t_m2, STATE_NUM, 1); // I-K*H
	mat_mat_mult(t_m2, pred.P, t_m3, STATE_NUM, 0, 1);
	mat_mat_mult(out->P, t_m3, t_m2, STATE_NUM, 0, 0);
	mat_add(t_m3, out->P, t_m1, STATE_NUM, 0);
	mat_copy(out->P, t_m3, STATE_NUM, 0);

	//% PRECAUTIONARY APPROACH.  EVEN THOUGH IT HASN'T HAPPEN IN THE SIMULATION SO FAR
	//% also test if opt_P is still a positive definite matrix.  If not, add some small noise to it to
	//% make it still positive.
	//% d: a diagonal matrix of eigenvalues
	//% v: matrix whose vectors are the eigenvectors
	//% X*V = V*D
	//if isempty(find(eig(opt_P)<0.001))
	//else
	//[v d]=eig(opt_P);
	//c=find(d==min(diag(d)));
	//d(c)=0.001;
	//opt_P=v*d*v';
	//end

	add_noise=0;
	// jacobi destroys the input vector
	mat_copy(t_m1, out->P, STATE_NUM, 0);
	jacobi(t_m1, STATE_NUM, d, v, nrot);

	min_d = d[1] ;
	for(i=1;i<=STATE_NUM;i++){
		if ( min_d > d[i])
			min_d = d[i];

		if (d[i]< 0.001)
			add_noise=1;
	}

	// CAUTION - maybe the 2nd minimum is quite far from 0.001
	if (add_noise){
		// change diagonal matrix d
		for (i=1;i<STATE_NUM;i++){
			if (d[i] == min_d)
				d[i] = 0.001;
		}

		// opt_P=v*d*v';
		mat_copy(t_m1, v, STATE_NUM, 1);
		convert_vec_diag(t_m2, d, STATE_NUM);
		mat_mat_mult(t_m3, t_m2, t_m1, STATE_NUM, 0, 0);
		mat_mat_mult(out->P, v, t_m3, STATE_NUM, 0, 0);
	}

	//printf("The pred  are x = %f y = %f\n",pred.x[1],pred.x[2]);


	// Free all MALLOCed matrices
	FREE_VECTOR_FUNCTION(pred.x, 1, STATE_NUM);
	FREE_MATRIX_FUNCTION(pred.P, 1, STATE_NUM, 1, STATE_NUM);

	FREE_VECTOR_FUNCTION(t_v1,   1, STATE_NUM);
	FREE_MATRIX_FUNCTION(t_m1,   1, STATE_NUM, 1, STATE_NUM);
	FREE_MATRIX_FUNCTION(t_m2,   1, STATE_NUM, 1, STATE_NUM);
	FREE_MATRIX_FUNCTION(t_m3,   1, STATE_NUM, 1, STATE_NUM);

	FREE_MATRIX_FUNCTION(v,      1, STATE_NUM, 1, STATE_NUM);
	free_ivector(nrot,           1, STATE_NUM);
	FREE_VECTOR_FUNCTION(d,      1, STATE_NUM);

	FREE_MATRIX_FUNCTION(A,      1, STATE_NUM, 1, STATE_NUM);
	FREE_MATRIX_FUNCTION(Q,      1, STATE_NUM, 1, STATE_NUM);
	FREE_MATRIX_FUNCTION(Q1,     1, STATE_NUM, 1, STATE_NUM);
	FREE_VECTOR_FUNCTION(H,      1, STATE_NUM);
	FREE_VECTOR_FUNCTION(K,      1, STATE_NUM);
	FREE_MATRIX_FUNCTION(I,      1, STATE_NUM, 1, STATE_NUM);

	return(0);
}
Ejemplo n.º 12
0
/*
 * This is the constraint that makes sure we hit the ball
 */
void kinematics_eq_constr(unsigned m, double *result, unsigned n, const double *x, double *grad, void *data) {

	static double ballPred[CART];
	static double racketDesVel[CART];
	static double racketDesNormal[CART];
	static Matrix  link_pos_des;
	static Matrix  joint_cog_mpos_des;
	static Matrix  joint_origin_pos_des;
	static Matrix  joint_axis_pos_des;
	static Matrix  Alink_des[N_LINKS+1];
	static Matrix  racketTransform;
	static Matrix  Jacobi;
	static Vector  qfdot;
	static Vector  xfdot;
	static Vector  normal;
	static int     firsttime = TRUE;

	double T = x[2*DOF];
	int N = T/TSTEP;
	int i;

	/* initialization of static variables */
	if (firsttime) {
		firsttime = FALSE;

		link_pos_des         = my_matrix(1,N_LINKS,1,3);
		joint_cog_mpos_des   = my_matrix(1,N_DOFS,1,3);
		joint_origin_pos_des = my_matrix(1,N_DOFS,1,3);
		joint_axis_pos_des   = my_matrix(1,N_DOFS,1,3);
		Jacobi               = my_matrix(1,2*CART,1,N_DOFS);
		racketTransform      = my_matrix(1,4,1,4);
		qfdot                = my_vector(1,DOF);
		xfdot                = my_vector(1,2*CART);
		normal               = my_vector(1,CART);

		for (i = 0; i <= N_LINKS; ++i)
			Alink_des[i] = my_matrix(1,4,1,4);

		// initialize the base variables
		//taken from ParameterPool.cf
		bzero((void *) &base_state, sizeof(base_state));
		bzero((void *) &base_orient, sizeof(base_orient));
		base_orient.q[_Q1_] = 1.0;

		// the the default endeffector parameters
		setDefaultEndeffector();

		// homogeneous transform instead of using quaternions
		racketTransform[1][1] = 1;
		racketTransform[2][3] = 1;
		racketTransform[3][2] = -1;
		racketTransform[4][4] = 1;
	}

	if (grad) {
		// compute gradient of kinematics = jacobian
		//TODO:
		grad[0] = 0.0;
		grad[1] = 0.0;
		grad[2] = 0.0;
	}

	// interpolate at time T to get the desired racket parameters
	first_order_hold(ballPred,racketDesVel,racketDesNormal,T);

	// extract state information from array to joint_des_state structure
	for (i = 1; i <= DOF; i++) {
		joint_des_state[i].th = x[i-1];
		joint_des_state[i].thd = qfdot[i] = x[i-1+DOF];
	}

	/* compute the desired link positions */
	kinematics(joint_des_state, &base_state, &base_orient, endeff,
			           joint_cog_mpos_des, joint_axis_pos_des, joint_origin_pos_des,
			           link_pos_des, Alink_des);


	/* compute the racket normal */
	mat_mult(Alink_des[6],racketTransform,Alink_des[6]);
	for (i = 1; i <= CART; i++) {
		normal[i] = Alink_des[6][i][3];
	}

	/* compute the jacobian */
	jacobian(link_pos_des, joint_origin_pos_des, joint_axis_pos_des, Jacobi);
	mat_vec_mult(Jacobi, qfdot, xfdot);

	/* deviations from the desired racket frame */
	for (i = 1; i <= CART; i++) {
		//printf("xfdot[%d] = %.4f, racketDesVel[%d] = %.4f\n",i,xfdot[i],i,racketDesVel[i-1]);
		//printf("normal[%d] = %.4f, racketDesNormal[%d] = %.4f\n",i,normal[i],i,racketDesNormal[i-1]);
		result[i-1] = link_pos_des[6][i] - ballPred[i-1];
		result[i-1 + CART] = xfdot[i] - racketDesVel[i-1];
		result[i-1 + 2*CART] = normal[i] - racketDesNormal[i-1];
	}

}
void BaseStateEstimation::update(SL_quat& base_orientation, SL_Cstate& base_position)
{
	SL_quat orient;
	memcpy(&(orient.q[1]), imu_quaternion_, 4*sizeof(double));
	MY_MATRIX(quat_to_rot_helper, 1,3,1,3);
	quatToRotMat(&orient, quat_to_rot_helper);
	for(int r=1; r<=3; r++)
	  for(int c=1; c<=3; c++)
	      O_rot_I_[r][c] = quat_to_rot_helper[c][r];

	memcpy(&(O_angrate_I_[1]), imu_angrate_, 3*sizeof(double));
//	memcpy(&(O_linvel_I_[1]), imu_linvel_, 3*sizeof(double));
	memcpy(&(O_linacc_I_[1]), imu_linacc_, 3*sizeof(double));

	// compensate for gravity
//	for(int i=1; i<=3; i++)
//	  O_linacc_I_[i] -= gravity_[i];

	// do numerical integration
	for(int i=1; i<=3; i++)
	{
		//integrate
		O_angacc_I_[i] = (double)update_rate_*(O_angrate_I_[i] - prev_O_angrate_I_[i]);
//		O_linpos_I_[i] += (1.0/(double)update_rate_)*O_linvel_I_[i];

		//apply leakage term
//		O_linvel_I_[i] *= leakage_factor_;
//		O_linvel_I_[i] += (1.0/(double)update_rate_)*O_linacc_I_[i];
	}

	// do coordinate transformation
	MY_MATRIX(S_angrate, 1,3,1,3);
	vectorToSkewMatrix(O_angrate_I_, S_angrate);
	MY_MATRIX(S_angacc, 1,3,1,3);
	vectorToSkewMatrix(O_angacc_I_, S_angacc);
	MY_MATRIX(S_helper, 1,3,1,3);
	mat_mult(S_angrate, S_angrate, S_helper);
	mat_add(S_angacc, S_helper, S_helper);

	mat_mult(O_rot_I_, I_rot_B_, O_rot_B_);

//	mat_vec_mult(O_rot_I_, I_linpos_B_, O_linpos_B_);
//	vec_add(O_linpos_I_, O_linpos_B_, O_linpos_B_);
//
//	mat_vec_mult(O_rot_I_, I_linpos_B_, O_linvel_B_);
//	mat_vec_mult(S_angrate, O_linvel_B_, O_linvel_B_);
//	vec_add(O_linvel_I_, O_linvel_B_, O_linvel_B_);

	mat_vec_mult(O_rot_I_, I_linpos_B_, O_linacc_B_);
	mat_vec_mult(S_helper, O_linacc_B_, O_linacc_B_);
	vec_add(O_linacc_I_, O_linacc_B_, O_linacc_B_);

	// integrate base state
	for(int i=1; i<=3; i++)
	{
		//integrate
		O_linpos_B_[i] += (1.0/(double)update_rate_)*O_linvel_B_[i];

		//apply leakage term
		O_linvel_B_[i] *= leakage_factor_;
		O_linvel_B_[i] += (1.0/(double)update_rate_)*O_linacc_B_[i];
	}

	// write data back
	linkQuat(O_rot_B_, &O_orient_B_);
	memcpy(&(O_orient_B_.ad[1]), &(O_angrate_I_[1]), 3*sizeof(double));
	memcpy(&(O_orient_B_.add[1]), &(O_angacc_I_[1]), 3*sizeof(double));
	memcpy(&(O_trans_B_.x[1]), &(O_linpos_B_[1]),3*sizeof(double));
	memcpy(&(O_trans_B_.xd[1]), &(O_linvel_B_[1]),3*sizeof(double));
	memcpy(&(O_trans_B_.xdd[1]), &(O_linacc_B_[1]),3*sizeof(double));

//	linkQuat(O_rot_I_, &O_orient_B_);
//	memcpy(&(O_orient_B_.ad[1]), &(O_angrate_I_[1]), 3*sizeof(double));
//	memcpy(&(O_orient_B_.add[1]), &(O_angacc_I_[1]), 3*sizeof(double));
//	memcpy(&(O_trans_B_.x[1]), &(O_linpos_I_[1]),3*sizeof(double));
//	memcpy(&(O_trans_B_.xd[1]), &(O_linvel_I_[1]),3*sizeof(double));
//	memcpy(&(O_trans_B_.xdd[1]), &(O_linacc_I_[1]),3*sizeof(double));

//	quatDerivatives(&O_orient_B_);
	base_orientation = O_orient_B_;
	base_position = O_trans_B_;

	// update buffers
	memcpy(&(prev_O_angrate_I_[1]), &(O_angrate_I_[1]), 3*sizeof(double));
}
Ejemplo n.º 14
0
void
test_constraint(void)
{
  int i,j,n,m;
  static Matrix Jbig;
  static Matrix dJbigdt;
  static Vector dsbigdt;
  static Matrix JbigMinvJbigt;
  static Matrix JbigMinvJbigtinv;
  static Matrix Minv;
  static Matrix MinvJbigt;
  static Matrix Jbigt;
  static Vector dJbigdtdsbigdt;
  static Matrix Jbart;
  static Matrix Jbar;
  static Vector lv;
  static Vector lambda;
  static Vector f;
  static Matrix R;
  static Vector v;
  static int firsttime = TRUE;
  int    debug_print = TRUE;

  if (firsttime) {
    firsttime = FALSE;
    Jbig             = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6);
    Jbigt            = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6);
    dJbigdt          = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6);
    JbigMinvJbigt    = my_matrix(1,N_ENDEFFS*6,1,N_ENDEFFS*6);
    JbigMinvJbigtinv = my_matrix(1,N_ENDEFFS*6,1,N_ENDEFFS*6);
    Minv             = my_matrix(1,N_DOFS+6,1,N_DOFS+6);
    MinvJbigt        = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6);
    dsbigdt          = my_vector(1,N_DOFS+6);
    dJbigdtdsbigdt   = my_vector(1,N_ENDEFFS*6);
    Jbar             = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6);
    Jbart            = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6);
    lv               = my_vector(1,N_ENDEFFS*6);
    lambda           = my_vector(1,N_ENDEFFS*6);
    f                = my_vector(1,N_DOFS+6);
    R                = my_matrix(1,N_CART,1,N_CART);
    v                = my_vector(1,N_CART);

    // the base part of Jbig is just the identity matrix for both constraints
    for (i=1; i<=6; ++i)
      Jbig[i][N_DOFS+i] = 1.0;

    for (i=1; i<=6; ++i)
      Jbig[i+6][N_DOFS+i] = 1.0;

  }

  // build the Jacobian including the base -- just copy J into Jbig
  mat_equal_size(J,N_ENDEFFS*6,N_DOFS,Jbig);
  mat_trans(Jbig,Jbigt);

  // build the Jacobian derivative
  mat_equal_size(dJdt,N_ENDEFFS*6,N_DOFS,dJbigdt);

  // build the augmented state derivate vector
  for (i=1; i<=N_DOFS; ++i)
    dsbigdt[i] = joint_state[i].thd;

  for (i=1; i<=N_CART; ++i)
    dsbigdt[N_DOFS+i] = base_state.xd[i];

  for (i=1; i<=N_CART; ++i)
    dsbigdt[N_DOFS+N_CART+i] = base_orient.ad[i];


  // compute J-bar transpose
  my_inv_ludcmp(rbdInertiaMatrix,N_DOFS+6, Minv);
  mat_mult(Minv,Jbigt,MinvJbigt);
  mat_mult(Jbig,MinvJbigt,JbigMinvJbigt);
  my_inv_ludcmp(JbigMinvJbigt,N_ENDEFFS*6,JbigMinvJbigtinv);
  mat_mult(MinvJbigt,JbigMinvJbigtinv,Jbar);
  mat_trans(Jbar,Jbart);

  // compute C+G-u
  for (i=1; i<=N_DOFS; ++i)
    f[i] = -joint_state[i].u;
  
  for (i=1; i<=6; ++i)
    f[N_DOFS+i] = 0.0;

  vec_add(f,rbdCplusGVector,f);

  // compute first part of lambda
  mat_vec_mult(Jbart,f,lambda);

  // compute second part of lambda
  mat_vec_mult(dJbigdt,dsbigdt,dJbigdtdsbigdt);
  mat_vec_mult(JbigMinvJbigtinv,dJbigdtdsbigdt,lv);

  // compute final lambda
  vec_sub(lambda,lv,lambda);

  // ----------------------------------------------------------------------
  // express lambda in the same coordinate at the foot 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 */

  mat_trans_size(Alink[L_IN_HEEL],N_CART,N_CART,R);

  // transform forces
  for (i=1; i<=N_CART; ++i)
    v[i] = lambda[N_CART*2+i];
  mat_vec_mult(R,v,v);

  if (debug_print) {
    printf("LEFT Force: lx=% 7.5f   sx=% 7.5f\n",v[1],misc_sim_sensor[L_CFx]);
    printf("LEFT Force: ly=% 7.5f   sy=% 7.5f\n",v[2],misc_sim_sensor[L_CFy]);
    printf("LEFT Force: lz=% 7.5f   sz=% 7.5f\n",v[3],misc_sim_sensor[L_CFz]);
  }

  misc_sensor[L_CFx] = v[1];
  misc_sensor[L_CFy] = v[2];
  misc_sensor[L_CFz] = v[3];

  // transform torques
  for (i=1; i<=N_CART; ++i)
    v[i] = lambda[N_CART*2+N_CART+i];
  mat_vec_mult(R,v,v);

  if (debug_print) {
    printf("LEFT Torque: lx=% 7.5f   sx=% 7.5f\n",v[1],misc_sim_sensor[L_CTa]);
    printf("LEFT Torque: ly=% 7.5f   sy=% 7.5f\n",v[2],misc_sim_sensor[L_CTb]);
    printf("LEFT Torque: lz=% 7.5f   sz=% 7.5f\n",v[3],misc_sim_sensor[L_CTg]);
  }

  misc_sensor[L_CTa] = v[1];
  misc_sensor[L_CTb] = v[2];
  misc_sensor[L_CTg] = v[3];

  /* 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 */
  mat_trans_size(Alink[R_IN_HEEL],N_CART,N_CART,R);

  // transform forces
  for (i=1; i<=N_CART; ++i)
    v[i] = lambda[i];
  mat_vec_mult(R,v,v);

  if (debug_print) {
    printf("RIGHT Force: lx=% 7.5f   sx=% 7.5f\n",v[1],misc_sim_sensor[R_CFx]);
    printf("RIGHT Force: ly=% 7.5f   sy=% 7.5f\n",v[2],misc_sim_sensor[R_CFy]);
    printf("RIGHT Force: lz=% 7.5f   sz=% 7.5f\n",v[3],misc_sim_sensor[R_CFz]);
  }
    
  misc_sensor[R_CFx] = v[1];
  misc_sensor[R_CFy] = v[2];
  misc_sensor[R_CFz] = v[3];

  // transform torques
  for (i=1; i<=N_CART; ++i)
    v[i] = lambda[N_CART+i];
  mat_vec_mult(R,v,v);

  if (debug_print) {
    printf("RIGHT Torque: lx=% 7.5f   sx=% 7.5f\n",v[1],misc_sim_sensor[R_CTa]);
    printf("RIGHT Torque: ly=% 7.5f   sy=% 7.5f\n",v[2],misc_sim_sensor[R_CTb]);
    printf("RIGHT Torque: lz=% 7.5f   sz=% 7.5f\n",v[3],misc_sim_sensor[R_CTg]);
  }

  misc_sensor[R_CTa] = v[1];
  misc_sensor[R_CTb] = v[2];
  misc_sensor[R_CTg] = v[3];

  if (debug_print)
    getchar();
  
}
Ejemplo n.º 15
0
/*****************************************************************************
******************************************************************************
  Function Name	: run_gravityComp_task
  Date		: Dec. 1997

  Remarks:

  run the task from the task servo: REAL TIME requirements!

******************************************************************************
  Paramters:  (i/o = input/output)

  none

 *****************************************************************************/
static int 
run_gravityComp_task(void)
{
  int j,i;

  // Damping Gains (index starts at 1)
  //double Kdamp[N_DOFS+1] = {0.0, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.1, 0.1, 0.1}; 
  double Kdamp[N_DOFS+1] = {0.0, 0.07, 0.07, 0.07, 0.07, 0.07, 0.07, 0.07, 0.1, 0.1, 0.1}; 
  // double Kdamp[N_DOFS+1] = {0.0, 0.2, 0.2, 0.5, 0.5, 1.0, 0.5, 0.5, 1, 1, 1}; 

  // Coriolus Compensation Gains 
  double Kcori[N_DOFS+1] = {0.0, 1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 0.00, 0.00, 0.00};
  // double Kcori[N_DOFS+1] = {0.0, 0.30, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.00, 0.00, 0.00};
  // double Kcori[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};

  // Inertia Compensation Gains 
  double Kiner[N_DOFS+1] = {0.0, 0.40, 0.40, 0.30, 0.35, 0.00, 0.4, 0.4, 0.00, 0.00, 0.00};
  // double Kiner[N_DOFS+1] = {0.0, 1.00, 1.00, 1.00, 1.00, 0.50, 1.00, 1.00, 0.00, 0.00, 0.00}; 
  //double Kiner[N_DOFS+1] = {0.0, 0.20, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.00, 0.00, 0.00};
  // double Kiner[N_DOFS+1] = {0.0, 0.50, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.00, 0.00, 0.00};
  // double Kiner[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0, 0.0, 0.00, 0.00, 0.00};
 
  // Integer gains (how fast desired state becomes actual state)  
  //double Kint[N_DOFS+1]  = {0.0, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.001, 0.001, 0.001};
  double Kint[N_DOFS+1]  = {0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.001, 0.001, 0.001};
  //double Kint[N_DOFS+1]  = {0.0, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.001, 0.001, 0.001};

  // Dead Zone Threshold 
  double thres[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.05, 0.05, 0.05};
  
  // To shut off Inertia,Coriolus Compensation
  //double Kcori[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};
  //double Kiner[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};
  
  for (i=1; i<=N_DOFS; ++i) {
    joint_filt_state[i] = filter(i);
  }
  
  if (joint_state[8].th < 0.08) {  // Hold down thumb to trigger Grav. Comp.

    // Gravity Compensation
    for (i=1; i<=N_DOFS; ++i) {
      if (fabs(joint_state[i].th-joint_des_state[i].th) > thres[i]){
        joint_des_state[i].th += (joint_state[i].th-joint_des_state[i].th)*Kint[i];
      }
    }

    // add compensation relative to the current state of the robot
    for (i=1; i<=N_DOFS; ++i) {
      // Gravity Compensation
	 target[i].th   = joint_state[i].th;
      // Coriolus Compensation
	 target[i].thd  = Kcori[i]*joint_filt_state[i].thd;
      // Inertia Compensation
	 target[i].thdd = Kiner[i]*joint_filt_state[i].thdd;
      
	 // Damping (to be premultiplied by inertia matrix)
         //  if (i != 5 ) {
         //    target[i].thdd += (0.0 - joint_state[i].thd ) * Kdamp[i] * DampScale * controller_gain_thd[i];
	 //  }
      
	 target[i].uff  = 0.0;
    }
      
    SL_InvDynNE(NULL,target,endeff,&base_state,&base_orient);
 
    // assign compensation torques
    for (i=1; i<=N_DOFS; ++i) {
      if (i==1 || i==2 || i==3 || i==4 || i==6 || i==7 || i==5) { 
	// Use my own damping (not premultiplied by the inertia matrix)
	joint_des_state[i].uff = 
	  target[i].uff + (0.0 - joint_filt_state[i].thd) * Kdamp[i] * DampScale * controller_gain_thd[i];
	
        // Cancel damping in the motor servo
	joint_des_state[i].thd = joint_state[i].thd;  
      }
      else {
        // Do not use my own damping
	joint_des_state[i].uff   = target[i].uff;
      }
    }

    fieldtorque = 0.0;
    /* FORCE FIELDS GO HERE */
    switch (fieldon) {
      case 1:
        joint_des_state[4].uff += joint_filt_state[2].thd * fieldGain + joint_filt_state[1].thd * fieldGain;
        break;
      case 2:
        if (joint_state[1].thd > 0.0) {
          joint_des_state[5].uff += joint_state[1].thd * -0.5;
        }
        break;
      case 3:
        joint_des_state[2].uff += joint_state[3].thd * 1.0;
        break;
      case 4:
        joint_des_state[1].uff += joint_state[4].thd * 1.0;
        break;
      case 5:
        joint_des_state[4].uff += joint_state[3].thd * -1.0;
        break;
      case 6:
        if (joint_state[1].thd > 0.0) {
          joint_des_state[3].uff += joint_state[1].thd * 1.0;
        }
        break;
      case 7: //end-effector:
        for (i=1;i<=7;i++) {
	  for (j=1;j<=3;j++) {
	    J7T[i][j] = J[j][i];
	  }
	}
	for (i=1;i<=3;i++) {
	  xd[i] = cart_state[1].xd[i];
	}
	if (!mat_mult(J7T,B, J7T))
	  return FALSE;
	if (!mat_vec_mult(J7T, xd, u_field))
	  return FALSE;
        for (i=1;i<=7;i++) {
	  joint_des_state[i].uff += fieldGain*u_field[i];
	}
	fieldtorque = fieldGain*u_field[1];
        break;    
    }
  }

  return TRUE;
}