コード例 #1
0
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;

}
コード例 #2
0
TEST(TestSuite, oneDofRevoluteX)
{
  const char* ffJointName = "base_footprint_joint";
  jrl::dynamics::urdf::Parser parser;
  CjrlHumanoidDynamicRobot* robot = parser.parse
    (TEST_MODEL_DIRECTORY "/one_dof_revolute_x.urdf",
     ffJointName);

  CjrlJoint* root = robot->rootJoint ();
  ASSERT_TRUE (root);
  ASSERT_EQ (1, root->countChildJoints());
  ASSERT_EQ (ffJointName, root->getName());

  CjrlJoint* joint = root->childJoint (0);
  ASSERT_TRUE (joint);
  ASSERT_EQ ("joint1", joint->getName());

  // Create robot configuration.
  const int ndofs = 7;
  vectorN q (ndofs);
  vectorN dq (ndofs);
  vectorN ddq (ndofs);

  for (unsigned i = 0; i < ndofs; ++i)
    q (i) = dq (i) = ddq (i) = 0.;

  // Check configuration at zero.
  std::cout << q << std::endl;
  ASSERT_TRUE (robot->currentConfiguration(q));
  ASSERT_TRUE (robot->currentVelocity(dq));
  ASSERT_TRUE (robot->currentAcceleration(ddq));
  ASSERT_TRUE (robot->computeForwardKinematics());

  matrix4d jointPosition;

  jointPosition.setIdentity ();
  for (unsigned i = 0; i < 4; ++i)
    for (unsigned j = 0; j < 4; ++j)
      ASSERT_EQ(jointPosition (i, j),
		root->currentTransformation () (i, j));

  jointPosition.setIdentity ();
  jointPosition (1, 3) = 1.;

  // std::cout << "current value:" << std::endl;
  // std::cout << joint->currentTransformation () << std::endl;
  // std::cout << "expected value:" << std::endl;
  // std::cout << jointPosition << std::endl;

  for (unsigned i = 0; i < 4; ++i)
    for (unsigned j = 0; j < 4; ++j)
      ASSERT_NEAR (jointPosition (i, j),
		   joint->currentTransformation () (i, j),
		   1e-9);

  // Check configuration at pi/2.
  q (6) = M_PI / 2.;
  std::cout << q << std::endl;
  ASSERT_TRUE (robot->currentConfiguration(q));
  ASSERT_TRUE (robot->currentVelocity(dq));
  ASSERT_TRUE (robot->currentAcceleration(ddq));
  ASSERT_TRUE (robot->computeForwardKinematics());

  jointPosition.setIdentity ();
  jointPosition (1, 1) = 0.;
  jointPosition (1, 2) = -1.;
  jointPosition (2, 1) = 1.;
  jointPosition (2, 2) = 0.;
  jointPosition (1, 3) = 1.;

  std::cout << "current value:" << std::endl;
  std::cout << joint->currentTransformation () << std::endl;
  std::cout << "expected value:" << std::endl;
  std::cout << jointPosition << std::endl;

  for (unsigned i = 0; i < 4; ++i)
    for (unsigned j = 0; j < 4; ++j)
      ASSERT_NEAR (jointPosition (i, j),
		   joint->currentTransformation () (i, j),
		   1e-9);
}