Skip to content

Commit

Permalink
[RQT-JTC] limits from jtc controlled joints (#1146)
Browse files Browse the repository at this point in the history
(cherry picked from commit 0ae3e42)
  • Loading branch information
delihus authored and mergify[bot] committed Aug 26, 2024
1 parent c2d8cb4 commit 6f6ed60
Show file tree
Hide file tree
Showing 2 changed files with 50 additions and 47 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ def subscribe_to_robot_description(node, key="robot_description"):
node.create_subscription(String, key, callback, qos_profile)


def get_joint_limits(node, use_smallest_joint_limits=True):
def get_joint_limits(node, joints_names, use_smallest_joint_limits=True):
global description
use_small = use_smallest_joint_limits
use_mimic = True
Expand All @@ -71,54 +71,56 @@ def get_joint_limits(node, use_smallest_joint_limits=True):
if jtype == "fixed":
continue
name = child.getAttribute("name")
try:
limit = child.getElementsByTagName("limit")[0]

if name in joints_names:
try:
minval = float(limit.getAttribute("lower"))
maxval = float(limit.getAttribute("upper"))
except ValueError:
if jtype == "continuous":
minval = -pi
maxval = pi
else:
limit = child.getElementsByTagName("limit")[0]
try:
minval = float(limit.getAttribute("lower"))
maxval = float(limit.getAttribute("upper"))
except ValueError:
if jtype == "continuous":
minval = -pi
maxval = pi
else:
raise Exception(
f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!"
)
try:
maxvel = float(limit.getAttribute("velocity"))
except ValueError:
raise Exception(
f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!"
f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!"
)
try:
maxvel = float(limit.getAttribute("velocity"))
except ValueError:
except IndexError:
raise Exception(
f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!"
f"Missing limits tag for the joint : {name} in the robot_description!"
)
except IndexError:
raise Exception(
f"Missing limits tag for the joint : {name} in the robot_description!"
)
safety_tags = child.getElementsByTagName("safety_controller")
if use_small and len(safety_tags) == 1:
tag = safety_tags[0]
if tag.hasAttribute("soft_lower_limit"):
minval = max(minval, float(tag.getAttribute("soft_lower_limit")))
if tag.hasAttribute("soft_upper_limit"):
maxval = min(maxval, float(tag.getAttribute("soft_upper_limit")))

mimic_tags = child.getElementsByTagName("mimic")
if use_mimic and len(mimic_tags) == 1:
tag = mimic_tags[0]
entry = {"parent": tag.getAttribute("joint")}
if tag.hasAttribute("multiplier"):
entry["factor"] = float(tag.getAttribute("multiplier"))
if tag.hasAttribute("offset"):
entry["offset"] = float(tag.getAttribute("offset"))

dependent_joints[name] = entry
continue

if name in dependent_joints:
continue

joint = {"min_position": minval, "max_position": maxval}
joint["has_position_limits"] = jtype != "continuous"
joint["max_velocity"] = maxvel
free_joints[name] = joint
safety_tags = child.getElementsByTagName("safety_controller")
if use_small and len(safety_tags) == 1:
tag = safety_tags[0]
if tag.hasAttribute("soft_lower_limit"):
minval = max(minval, float(tag.getAttribute("soft_lower_limit")))
if tag.hasAttribute("soft_upper_limit"):
maxval = min(maxval, float(tag.getAttribute("soft_upper_limit")))

mimic_tags = child.getElementsByTagName("mimic")
if use_mimic and len(mimic_tags) == 1:
tag = mimic_tags[0]
entry = {"parent": tag.getAttribute("joint")}
if tag.hasAttribute("multiplier"):
entry["factor"] = float(tag.getAttribute("multiplier"))
if tag.hasAttribute("offset"):
entry["offset"] = float(tag.getAttribute("offset"))

dependent_joints[name] = entry
continue

if name in dependent_joints:
continue

joint = {"min_position": minval, "max_position": maxval}
joint["has_position_limits"] = jtype != "continuous"
joint["max_velocity"] = maxvel
free_joints[name] = joint
return free_joints
Original file line number Diff line number Diff line change
Expand Up @@ -238,7 +238,8 @@ def _update_jtc_list(self):
# for _all_ their joints
running_jtc = self._running_jtc_info()
if running_jtc and not self._robot_joint_limits:
self._robot_joint_limits = get_joint_limits(self._node) # Lazy evaluation
for jtc_info in running_jtc:
self._robot_joint_limits = get_joint_limits(self._node, _jtc_joint_names(jtc_info))
valid_jtc = []
if self._robot_joint_limits:
for jtc_info in running_jtc:
Expand Down

0 comments on commit 6f6ed60

Please sign in to comment.