The getTransformVector seems to only add up the positions.
If the joints are rotated does this still work (aren't the positions relative?) ?
bool UrdfGeometryParser::getTransformVector(const std::string& joint_name, const std::string& parent_link_name
, urdf::Vector3 &transform_vector)
{
if(model_)
{
urdf::JointConstSharedPtr joint(model_->getJoint(joint_name));
if (!joint)
{
ROS_ERROR_STREAM(joint_name
<< " couldn't be retrieved from model description");
return false;
}
transform_vector = joint->parent_to_joint_origin_transform.position;
while(joint->parent_link_name != parent_link_name)
{
urdf::LinkConstSharedPtr link_parent(model_->getLink(joint->parent_link_name));
if (!link_parent || !link_parent->parent_joint)
{
ROS_ERROR_STREAM(joint->parent_link_name
<< " couldn't be retrieved from model description or his parent joint");
return false;
}
joint = link_parent->parent_joint;
transform_vector = transform_vector + joint->parent_to_joint_origin_transform.position;
}
return true;
}
else
return false;
}
The
getTransformVectorseems to only add up the positions.If the joints are rotated does this still work (aren't the positions relative?) ?