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);
}