예제 #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 ();
    }
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);
}