コード例 #1
0
ファイル: kmeans.cpp プロジェクト: ivanzamanov/kmeans
void computeCentroid(vector& centroid, const list<vector*> &vectors) {
	if(vectors.size() == 0)
		return;
	list<iter*> iters;
	for(int i=0; i<vectors.size(); i++) {
		iter *it = new iter;
		it->size = vectors.get(i)->size;
		iters.add(it);
	}

	int stop = 0;
	while(!stop) {
		int min_key = 1 << 30;
		stop = 1;
		for(int i=0; i<iters.size(); i++) {
			iter& it = *(iters.get(i));
			int key = vectors.get(i)->keys[it.index];
			if(key < min_key && it.index < it.size) {
				stop = 0;
				min_key = key;
			}
		}
		if(stop)
			break;

		expandVector(centroid);
		centroid.keys[centroid.size - 1] = min_key;

		double value = 0;
		for(int i=0; i<iters.size(); i++) {
			iter& it = *(iters.get(i));
			int key = vectors.get(i)->keys[it.index];
			double val = vectors.get(i)->values[it.index];
			if(key == min_key && it.index < it.size) {
				value += val;
				it.index++;
			}
		}

		centroid.values[centroid.size - 1] = value;
	}

	for(int i=0; i<centroid.size; i++)
		centroid.values[i] /= vectors.size();
	for(int i=0; i<iters.size(); i++)
		delete iters.get(i);
}
コード例 #2
0
ファイル: ControlLaw.cpp プロジェクト: radarsat1/siconos
SICONOS_EXPORT void controlLaw(double * t, double * q, double * qdot, int * NDOF, int * NCONT, double * torques)
{
  int ndof;
  int contacts;
  char lagrangianModelName[20] = "";

  getLagrangianModelName(lagrangianModelName);
  getActiveDOFsize(&ndof);
  vector<int> activeDOF(ndof);
  if (ndof > 0)
    getActiveDOF(&(activeDOF[0]));
  else
    ndof = *NDOF;

  ///////////////////////////////////////////
  // Computation of the Lagrangian model
  ///////////////////////////////////////////


  matrix<double, column_major> M(*NDOF, *NDOF);
  vector<double> N(*NDOF);

  if (strcmp(lagrangianModelName, "Human36") == 0)
  {
    double L[31];
    double addl[84];
    double mass;
    GetModelMass(&mass);
    GetAnatomicalLengths(L);
    GetTag2JointLengths(addl);
    InertiaH36(&(M(0, 0)), q, L, addl, mass);
    NLEffectsH36(&(N[0]), q, qdot, L, addl, mass);
  }
  else
  {
    Inertia(&(M(0, 0)), q);
    NLEffects(&(N[0]), q, qdot);
  }

  //////////////////////////////////////////////////
  //Additionnal forces
  /////////////////////////////////////////////////

  vector<double> fAdditionnal(*NDOF);
  if (strcmp(lagrangianModelName, "PA10") == 0)
    Friction(&(fAdditionnal[0]), q, qdot);
  else if (strcmp(lagrangianModelName, "RX90") == 0)
    SpringForce(&(fAdditionnal[0]), q);
  else
    fAdditionnal.clear();

  ///////////////////////////////////////////////////
  // Limitation to the selected degrees of freedom
  ///////////////////////////////////////////////////
  reduceMatrix(M, activeDOF, activeDOF);
  N = reduceVector(N, activeDOF);
  fAdditionnal = reduceVector(fAdditionnal, activeDOF);

  //////////////////////////////////////////////////
  // Proportionnel-derivee in the task space
  //////////////////////////////////////////////////

  vector<double> sDesired(*NDOF);
  vector<double> sdotDesired(*NDOF);
  matrix<double, column_major> sddotDesiredMATRIX(*NDOF, 1);
  matrix_column<matrix<double, column_major> > sddotDesired(sddotDesiredMATRIX, 1);

  //vector<double> sddotDesired(*NDOF);



  trajectory(t, &(sDesired[0]), &(sdotDesired[0]), &(sddotDesired[0]), &contacts);

  //v = Kp*(s_desiree-TaskFunction(q))+Kv*(sdot_desiree-H*qdot)-h+sddot_desiree;
  vector<double> TF(*NDOF);
  vector<double> h(*NDOF);
  matrix<double, column_major> H(*NDOF, *NDOF);

  TaskFunction(&(TF[0]), q);
  TaskJacobian(&(H(0, 0)), q);
  TaskNLEffects(&(h[0]), q, qdot);

  double Kp = 100.0;
  double Kv = 30.0;
  vector<double, array_adaptor<double> > tmp_cast(*NDOF, array_adaptor<double> (*NDOF, qdot));
  sddotDesired += Kp * (sDesired - TF) + Kv * (sdotDesired - prod(H, tmp_cast)) - h;

  /////////////////////////////////////////////////////////////
  // Generalized Torques to make the realisation of the command
  /////////////////////////////////////////////////////////////

  //qddot = inv(H)*v;
  vector<int> ipiv(*NDOF);
  // lapack::getrf(H, ipiv);
  // sddotDesired = prod(H, sddotDesired);

  lapack::gesv(H, ipiv, sddotDesiredMATRIX);

  //D = M*qddot(ActiveDOF) + N + FAdditionnal;
  sddotDesired = reduceVector(sddotDesired, activeDOF);

  N += prod(M, sddotDesired) + fAdditionnal;

  //nmot = sum(ActiveDOF<=size(q, 1)); en fait ici nmot = ndof
  //Torques = zeros(q);
  //Torques(ActiveDOF(1:nmot)) = D(1:nmot);
  for (int i = 0; i < *NDOF; i++)
    torques[i] = 0;
  expandVector(torques, N, activeDOF);

}