Пример #1
0
    Value RetrieveJointNames::doExecute ()
    {
      RosJointState& entity = static_cast<RosJointState&> (owner ());

      std::vector<Value> values = getParameterValues ();
      std::string name = values[0].value ();

      if (!dynamicgraph::PoolStorage::getInstance ()->existEntity (name))
	{
	  std::cerr << "invalid entity name" << std::endl;
	  return Value ();
	}

      dynamicgraph::sot::Dynamic* dynamic =
	dynamic_cast<dynamicgraph::sot::Dynamic*>
	(&dynamicgraph::PoolStorage::getInstance ()->getEntity (name));
      if (!dynamic)
	{
	  std::cerr << "entity is not a Dynamic entity" << std::endl;
	  return Value ();
	}

      CjrlHumanoidDynamicRobot* robot = dynamic->m_HDR;
      if (!robot)
	{
	  std::cerr << "no robot in the dynamic entity" << std::endl;
	  return Value ();
	}

      entity.jointState ().name.resize (robot->numberDof());
      buildJointNames (entity.jointState (), robot->rootJoint());
      return Value ();
    }
int main(int argc, char *argv[])
{
  string aPath;
  string aName;
  string JointToRank;
  string aSpecificitiesFileName;

  int InitialPosition = POSITION_STILL;

  if (argc!=5)
    {
      aPath="./";
      aName="sample.wrl";
      aSpecificitiesFileName = "sampleSpecificities.xml";
      JointToRank = "sampleLinkJointRank.xml";
    }
  else
    {
      aPath=argv[1];
      aName=argv[2];
      JointToRank = argv[4];
      aSpecificitiesFileName = argv[3];
    }

  int VerboseMode = 0;
  // Read the first humanoid.
  CjrlHumanoidDynamicRobot * aHDR = robotDynamicsObjectConstructor.createHumanoidDynamicRobot();
  string RobotFileName = aPath+aName;
  dynamicsJRLJapan::parseOpenHRPVRMLFile(*aHDR,RobotFileName,JointToRank, aSpecificitiesFileName);

  // The second humanoid is constructed through the abstract interface
  //
  CjrlHumanoidDynamicRobot* a2HDR = robotDynamicsObjectConstructor.createHumanoidDynamicRobot();

  // The third humanoid is also constructed through the abstract interface
  //
  CjrlHumanoidDynamicRobot* a3HDR = robotDynamicsObjectConstructor.createHumanoidDynamicRobot();

  // Test the new humanoid structure.
  double dInitPos[40] = {
    0.0, 0.0, -26.0, 50.0, -24.0, 0.0, 0.0, 0.0, -26.0, 50.0, -24.0, 0.0,  // legs

    0.0, 0.0, 0.0, 0.0, // chest and head

    15.0, -10.0, 0.0, -30.0, 0.0, 0.0, 10.0, // right arm
    15.0,  10.0, 0.0, -30.0, 0.0, 0.0, 10.0, // left arm

    -20.0, 20.0, -20.0, 20.0, -20.0, // right hand
    -10.0, 10.0, -10.0, 10.0, -10.0  // left hand
  };


  // This is mandatory for this implementation of computeForwardKinematics
  // to compute the derivative of the momentum.
  {
    string inProperty[4]={"TimeStep","ComputeAcceleration",
			  "ComputeBackwardDynamics", "ComputeZMP"};
    string inValue[4]={"0.005","false","false","true"};
    for(unsigned int i=0;i<4;i++)
      {
	aHDR->setProperty(inProperty[i],inValue[i]);
	a2HDR->setProperty(inProperty[i],inValue[i]);
	a3HDR->setProperty(inProperty[i],inValue[i]);
      }
  }

  int NbOfDofs = aHDR->numberDof();

  MAL_VECTOR_DIM(aCurrentConf,double,NbOfDofs);
  MAL_VECTOR_DIM(aCurrentVel,double,NbOfDofs);
  MAL_VECTOR_FILL(aCurrentVel,0.0);
  MAL_VECTOR_DIM(aCurrentAcc,double,NbOfDofs);
  MAL_VECTOR_FILL(aCurrentAcc,0.0);

  int lindex=0;
  for(int i=0;i<6;i++)
    aCurrentConf[lindex++] = 0.0;

  for(int i=0;i<(NbOfDofs-6 < 40 ? NbOfDofs-6 : 40) ;i++)
    {
      if (InitialPosition==POSITION_STILL)
	aCurrentConf[lindex++] = 0.0;
      else if (InitialPosition==POSITION_HALF_SITTING)
	aCurrentConf[lindex++] = dInitPos[i]*M_PI/180.0;
    }

  aHDR->currentConfiguration(aCurrentConf);
  aHDR->currentVelocity(aCurrentVel);
  aHDR->currentAcceleration(aCurrentAcc);
  aHDR->computeForwardKinematics();

  HumanoidCopy aHumanoidCopier;
  aHumanoidCopier.PerformCopyFromJointsTree(aHDR, a2HDR);

  aHumanoidCopier.PerformCopyFromJointsTree(a2HDR, a3HDR);

  NbOfDofs = a2HDR->numberDof();
  if (VerboseMode>2)
    std::cout << "NbOfDofs :" << NbOfDofs << std::endl;

  a2HDR->currentConfiguration(aCurrentConf);
  a2HDR->currentVelocity(aCurrentVel);
  a2HDR->currentAcceleration(aCurrentAcc);
  a2HDR->computeForwardKinematics();

  a3HDR->currentConfiguration(aCurrentConf);
  a3HDR->currentVelocity(aCurrentVel);
  a3HDR->currentAcceleration(aCurrentAcc);
  a3HDR->computeForwardKinematics();

  // Initial Humanoid
  ofstream initialhumanoid;
  initialhumanoid.open("initialhumanoid.output");
  if (initialhumanoid.is_open())
    {
      DisplayHumanoid(aHDR,initialhumanoid);
      initialhumanoid.close();
    }

  // Copied Humanoid
  ofstream copiedhumanoid;
  copiedhumanoid.open("copiedhumanoid.output");
  if (copiedhumanoid.is_open())
    {
      DisplayHumanoid(a2HDR,copiedhumanoid);
      copiedhumanoid.close();
    }
  // 2nd copied humanoid
  ofstream copied2ndhumanoid;
  copied2ndhumanoid.open("copied2ndhumanoid.output");
  if (copied2ndhumanoid.is_open())
    {
      DisplayHumanoid(a3HDR,copied2ndhumanoid);
      copied2ndhumanoid.close();
    }

  MAL_S3_VECTOR(ZMPval,double);

  for(int i=0;i<4;i++)
    {
      aHDR->currentVelocity(aCurrentVel);
      aHDR->currentAcceleration(aCurrentAcc);
      aHDR->computeForwardKinematics();
      ZMPval = aHDR->zeroMomentumPoint();
      if (VerboseMode>4)
	{
	  cout << i << "-th value of ZMP : " << ZMPval <<endl;
	  cout << "Should be equal to the CoM: " << aHDR->positionCenterOfMass() << endl;
	}

      a2HDR->currentVelocity(aCurrentVel);
      a2HDR->currentAcceleration(aCurrentAcc);
      a2HDR->computeForwardKinematics();
      ZMPval = a2HDR->zeroMomentumPoint();
      if(VerboseMode>4)
	{
	  cout << i << "-th value of ZMP : " << ZMPval <<endl;
	  cout << "Should be equal to the CoM: " << aHDR->positionCenterOfMass() << endl;
	}

    }


  delete aHDR;
  delete a2HDR;
  delete a3HDR;

  string iho("copiedhumanoid.output");
  string cho("copied2ndhumanoid.output");
  string rho("reportcopy.output");
  if (CompareTwoFiles(iho.c_str(),
		      cho.c_str(),
		      rho.c_str()))
    {
      return 0;
    }
  return -1;

}
int main(int argc, char *argv[])
{
  string aSpecificitiesFileName;
  string aPath;
  string aName;
  string aMapFromCjrlJointToRank;

  string RefLogFile;
  string ActualLogFile;
  if (argc!=5)
    {
      const char *envrobotpath="ROBOTPATH";
      char *robotpath = 0;
      const char *envrobotname="ROBOT";
      char *robotname = 0;
      robotpath = getenv(envrobotpath);
      robotname = getenv(envrobotname);

      if ((robotpath==0) || (robotname==0))
	{
	  cerr << " This program takes 6 arguments: " << endl;
	  cerr << "./TestHumanoidDynamicRobot PATH_TO_VRML_FILE VRML_FILE_NAME "<< endl;
	  cerr << " PATH_TO_SPECIFICITIES_XML PATH PATH_TO_MAP_JOINT_2_RANK" << endl;
	  cerr << " ReferenceLogFile ActualLogFile" << endl;
	  exit(-1);
	}
      else
	{
	  aPath=robotpath;
	  aName=robotname;
	  aName+="main.wrl";
	  aSpecificitiesFileName = robotpath;
	  aSpecificitiesFileName +="/../etc/";
	  aSpecificitiesFileName += robotname;
	  aSpecificitiesFileName += "Specificities.xml";
	  aMapFromCjrlJointToRank = robotpath;
	  aMapFromCjrlJointToRank += "/../etc/";
	  aMapFromCjrlJointToRank += robotname;
	  aMapFromCjrlJointToRank += "JointRank.xml";

	}

      if (argc==3)
	{
	  RefLogFile = argv[1];
	  ActualLogFile = argv[2];
	}
    }
  else
    {
      aPath=argv[1];
      aName=argv[2];
      aSpecificitiesFileName = argv[3];
      aMapFromCjrlJointToRank = argv[4];
    }

  dynamicsJRLJapan::ObjectFactory dynFactory;
  CjrlHumanoidDynamicRobot * aHDR  = dynFactory.createHumanoidDynamicRobot();
  CjrlHumanoidDynamicRobot * aHDR2 = dynFactory.createHumanoidDynamicRobot();

  if (aHDR==0)
    {
      cerr<< "Dynamic cast on HDR failed " << endl;
      exit(-1);
  }
  cout << "Robot's model file:" << aPath << aName << endl;
  cout << "Specificities file:" << aSpecificitiesFileName << endl;
  cout << "Map from joint to rank:" << aMapFromCjrlJointToRank << endl;
  string RobotFileName = aPath + aName;
  parseOpenHRPVRMLFile(*aHDR,RobotFileName,
		       aMapFromCjrlJointToRank,aSpecificitiesFileName);
  parseOpenHRPVRMLFile(*aHDR2,RobotFileName,
		       aMapFromCjrlJointToRank,aSpecificitiesFileName);

  // Display tree of the joints.

  int NbOfDofs = aHDR->numberDof();
  std::cout << "NbOfDofs :" << NbOfDofs << std::endl;
  MAL_VECTOR_DIM(aCurrentConf,double,NbOfDofs);
  int lindex=0;
  for(int i=0;i<6;i++)
    aCurrentConf[lindex++] = 0.0;

  for(int i=0;i<(NbOfDofs-6 < 41 ? NbOfDofs-6 : 40) ;i++)
    aCurrentConf[lindex++] = 0.0;

  aHDR->currentConfiguration(aCurrentConf);

  aHDR2->currentConfiguration(aCurrentConf);

  const CjrlJoint * LeftFoot = aHDR->leftFoot()->associatedAnkle();
  const CjrlJoint * RightFoot = aHDR->rightFoot()->associatedAnkle();

  const CjrlJoint * LeftFoot2 = aHDR2->leftFoot()->associatedAnkle();
  const CjrlJoint * RightFoot2 = aHDR2->rightFoot()->associatedAnkle();

  CjrlJoint * Waist2 = aHDR2->waist();

  // Read the data file.
  ifstream ActualStateFile;
  ActualStateFile.open(ActualLogFile.c_str(),ifstream::in);
  if (!ActualStateFile.is_open())
    {
      cerr << "Unable to open actual state file: " <<
	ActualLogFile << endl;
      exit(-1);
    }

  ifstream RefStateFile;
  RefStateFile.open(RefLogFile.c_str(),ifstream::in);
  if (!RefStateFile.is_open())
    {
      cerr << "Unable to open reference state file: " <<
	RefLogFile << endl;
      exit(-1);
    }

  ofstream RebuildZMP;
  RebuildZMP.open("RebuildZMP.dat",ofstream::out);
  RebuildZMP.close();

  ofstream RebuildForces;
  RebuildForces.open("RebuildForces.dat",ofstream::out);
  RebuildForces.close();

  ofstream RebuildWaist;
  RebuildWaist.open("RebuildWaist.dat",ofstream::out);
  RebuildWaist.close();

  // Set properties for the first model.
  {
    string inProperty[4]={"TimeStep","ComputeAcceleration",
			  "ComputeBackwardDynamics", "ComputeZMP"};
    string inValue[4]={"0.005","true","true","true"};
    for(unsigned int i=0;i<4;i++)
      aHDR->setProperty(inProperty[i],inValue[i]);
  }
  // Set properties for the second model.
  {
    string inProperty[4]={"TimeStep","ComputeAcceleration",
			  "ComputeBackwardDynamics", "ComputeZMP"};
    string inValue[4]={"0.005","false","false","false"};
    for(unsigned int i=0;i<4;i++)
      aHDR2->setProperty(inProperty[i],inValue[i]);

  }

  // Read the first line of actual state:
  ofstream ASD("ActualStateDescription.dat");
  if (ASD.is_open())
    {
      for(unsigned int i=0;i<126;i++)
	{
	  string tmp;
	  ActualStateFile>> tmp;
	  ASD  << i << " : " << tmp << endl;
	}
      ASD.close();
    }

  ofstream RSD("ReferenceStateDescription.dat");
  if (RSD.is_open())
    {
      for(unsigned int i=0;i<95;i++)
	{
	  string tmp;
	  RefStateFile>> tmp;
	  RSD  << i << " : " << tmp << endl;
	}
      RSD.close();
    }

  unsigned long int NbIt=0;
  double WaistFromRef[6];
  double WaistFromActual[6];
  matrix4d AbsSupportFootPos;
  // We set the first support foot as being the left one.
  int PreviousSupportFoot=-1;

  while(!ActualStateFile.eof())
    {
      double NormalForces[2]={0.0,0.0};
      double ActualData[131];
      double RefData[100];

      double RotationFreeFlyer[9];

      for(unsigned int i=0;i<131;i++)
	{
	  ActualStateFile >> ActualData[i];

	  if (i<40)
	    aCurrentConf[i+6] = ActualData[i];
	  if (i==82)
	    NormalForces[1]=ActualData[i];

	  if (i==88)
	    NormalForces[0]=ActualData[i];
	}

      ExtractRefWaist(RefStateFile,WaistFromRef,RotationFreeFlyer,RefData);

      for(unsigned int i=0;i<6;i++)
	aCurrentConf[i]=WaistFromRef[i];

      for(unsigned int i=0;i<40;i++)
	aCurrentConf[i+6]=RefData[i];

      aHDR2->currentConfiguration(aCurrentConf);
      aHDR2->computeForwardKinematics();

      ExtractActualWaist(LeftFoot2,
			 RightFoot2,
			 Waist2,
			 AbsSupportFootPos,
			 WaistFromRef,
			 RotationFreeFlyer,
			 NbIt,
			 WaistFromActual,
			 PreviousSupportFoot);

      SaveWaistPositions(WaistFromRef, WaistFromActual);

      if (1)
	{
	  for(unsigned int i=0;i<6;i++)
	    aCurrentConf[i] = WaistFromActual[i];
	}
      else
	{
	   for(unsigned int i=0;i<6;i++)
	     aCurrentConf[i] = WaistFromRef[i];
	}

      aHDR->currentConfiguration(aCurrentConf);
      aHDR->computeForwardKinematics();

      vector3d ZMPval;
      ZMPval = aHDR->zeroMomentumPoint();

      matrix4d TrLF = LeftFoot->currentTransformation();
      matrix4d TrRF = RightFoot->currentTransformation();
      matrix4d TrLF2 = LeftFoot2->currentTransformation();
      matrix4d TrRF2 = RightFoot2->currentTransformation();

      double lnorm  = NormalForces[0] + NormalForces[1];

      RebuildZMP.open("RebuildZMP.dat",ofstream::app);
      RebuildZMP << (MAL_S4x4_MATRIX_ACCESS_I_J(TrLF,0,3) * NormalForces[0] +
		     MAL_S4x4_MATRIX_ACCESS_I_J(TrRF,0,3) * NormalForces[1])/ lnorm  << " "
		 << (MAL_S4x4_MATRIX_ACCESS_I_J(TrLF,1,3) * NormalForces[0] +
		     MAL_S4x4_MATRIX_ACCESS_I_J(TrRF,1,3) * NormalForces[1])/ lnorm  << " "
		 << ZMPval(0) << " " << ZMPval(1) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(TrLF,0,3) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(TrLF,1,3) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(TrLF,2,3) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(TrRF,0,3) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(TrRF,1,3) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(TrRF,2,3) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(AbsSupportFootPos,0,3) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(AbsSupportFootPos,1,3) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(AbsSupportFootPos,2,3) << " "
		 << PreviousSupportFoot << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(TrLF2,0,3) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(TrLF2,1,3) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(TrLF2,2,3) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(TrRF2,0,3) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(TrRF2,1,3) << " "
		 << MAL_S4x4_MATRIX_ACCESS_I_J(TrRF2,2,3) << endl;
      //92
      RebuildZMP.close();


      NbIt++;
    }

  ActualStateFile.close();

  delete aHDR;

}