torydebra
commented
4 years ago Found out how moveit parse the mimics. If I understood well, first loop all joints to see if they are mimic (i.e. they have mimic tag). If so, mimic params (multiplier, offset and father joint) are stored along with the joint name. After, it loops all the mimics and, for each one, it goes in the "father" joint. From "father" (if this is mimic) it goes to its father... and so on until the actuated "ancestor" joint. Then Multipliers, offset and ancestor name are updated considering the whole "family chain" (such that each mimic joint point to the real actuated joint with right params, in general a mimic joint can mimic another mimic joint).
void moveit::core::RobotModel::buildMimic(const urdf::ModelInterface& urdf_model)
{
// compute mimic joints
for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
{
const urdf::Joint* jm = urdf_model.getJoint(joint_model_vector_[i]->getName()).get();
if (jm)
if (jm->mimic)
{
JointModelMap::const_iterator jit = joint_model_map_.find(jm->mimic->joint_name);
if (jit != joint_model_map_.end())
{
if (joint_model_vector_[i]->getVariableCount() == jit->second->getVariableCount())
joint_model_vector_[i]->setMimic(jit->second, jm->mimic->multiplier, jm->mimic->offset);
else
logError("Join '%s' cannot mimic joint '%s' because they have different number of DOF",
joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
}
else
logError("Joint '%s' cannot mimic unknown joint '%s'", joint_model_vector_[i]->getName().c_str(),
jm->mimic->joint_name.c_str());
}
}
// in case we have a joint that mimics a joint that already mimics another joint, we can simplify things:
bool change = true;
while (change)
{
change = false;
for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
if (joint_model_vector_[i]->getMimic())
{
if (joint_model_vector_[i]->getMimic()->getMimic())
{
joint_model_vector_[i]->setMimic(
joint_model_vector_[i]->getMimic()->getMimic(),
joint_model_vector_[i]->getMimicFactor() * joint_model_vector_[i]->getMimic()->getMimicFactor(),
joint_model_vector_[i]->getMimicOffset() +
joint_model_vector_[i]->getMimicFactor() * joint_model_vector_[i]->getMimic()->getMimicOffset());
change = true;
}
if (joint_model_vector_[i] == joint_model_vector_[i]->getMimic())
{
logError("Cycle found in joint that mimic each other. Ignoring all mimic joints.");
for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
joint_model_vector_[i]->setMimic(NULL, 0.0, 0.0);
change = false;
break;
}
}
}
// build mimic requests
for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
if (joint_model_vector_[i]->getMimic())
{
const_cast<JointModel*>(joint_model_vector_[i]->getMimic())->addMimicRequest(joint_model_vector_[i]);
mimic_joints_.push_back(joint_model_vector_[i]);
}
}
liesrock
We need to parse mimic joint to be aware of them. We do not only need to know that a joint is mimic, but also find the joint that it is mimicking, and store this info in someway. Related to #16
Related comments from another issue:
I think that orocos kdl parser can not deal with mimic joints, thus we can not parse them with it. I found no info about this on Internet and neither on kdl api doc.
MoveIt (which uses kdl) probably developed some additional code to parse them, as I suppose from https://github.com/ros-planning/moveit/pull/1517
@liesrock, are you aware of someone who uses orocos kdl for parsing urdf with mimic tags?
Originally posted by @torydebra in https://github.com/ADVRHumanoids/ROSEndEffector/issues/23#issuecomment-605962607
no unfortunately I am not aware of anyone using orocos_kdl for this: the feature is currently missing as you mentioned.
Can we extend it or use another parser only for mimic joints ?
Originally posted by @liesrock in https://github.com/ADVRHumanoids/ROSEndEffector/issues/23#issuecomment-606014301
As you know I used moveit functions for parsing. In Parser, we could parse urdf also with moveit, then, for each mimic joint, delete it from the finger_jointmap and __urdf_jointmap, and decrease the jointsnum counter.
But at this point is not easier to parse the model directly with moveit structures? So we do not have to "loop" twice.
However, these problems are related to the issue about passive joints ( #16 )
Originally posted by @torydebra in https://github.com/ADVRHumanoids/ROSEndEffector/issues/23#issuecomment-606024010