ros-controls / urdf_geometry_parser

6 stars 8 forks source link

Does getTransformVector work for rotated joints? #11

Open AlexReimann opened 6 years ago

AlexReimann commented 6 years ago

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