Skip to content

Commit fed8889

Browse files
LemonPiLemonPi
authored
Merge pull request #53 from patricknaughton01/master
Extracting velocity and effort limits from URDF
2 parents c478735 + 5e1b6c9 commit fed8889

File tree

6 files changed

+207
-10
lines changed

6 files changed

+207
-10
lines changed

‎src/pytorch_kinematics/chain.py‎

Lines changed: 19 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -393,17 +393,28 @@ def clamp(self, th):
393393
return torch.clamp(th, self.low, self.high)
394394

395395
def get_joint_limits(self):
396+
return self._get_joint_limits("limits")
397+
398+
def get_joint_velocity_limits(self):
399+
return self._get_joint_limits("velocity_limits")
400+
401+
def get_joint_effort_limits(self):
402+
return self._get_joint_limits("effort_limits")
403+
404+
def _get_joint_limits(self, param_name):
396405
low = []
397406
high = []
398-
for joint_name in self.get_joint_parameter_names(exclude_fixed=True):
399-
joint = self.find_joint(joint_name)
400-
if joint.limits is None:
401-
low.append(-np.pi)
402-
high.append(np.pi)
407+
for joint in self.get_joints():
408+
val = getattr(joint, param_name)
409+
if val is None:
410+
# NOTE: This changes the previous default behavior of returning
411+
# +/- np.pi for joint limits to be more natural for both
412+
# revolute and prismatic joints
413+
low.append(-np.inf)
414+
high.append(np.inf)
403415
else:
404-
low.append(joint.limits[0])
405-
high.append(joint.limits[1])
406-
416+
low.append(val[0])
417+
high.append(val[1])
407418
return low, high
408419

409420
@staticmethod

‎src/pytorch_kinematics/frame.py‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,8 @@ class Joint(object):
4545
TYPES = ['fixed', 'revolute', 'prismatic']
4646

4747
def __init__(self, name=None, offset=None, joint_type='fixed', axis=(0.0, 0.0, 1.0),
48-
dtype=torch.float32, device="cpu", limits=None):
48+
dtype=torch.float32, device="cpu", limits=None,
49+
velocity_limits=None, effort_limits=None):
4950
if offset is None:
5051
self.offset = None
5152
else:
@@ -65,6 +66,8 @@ def __init__(self, name=None, offset=None, joint_type='fixed', axis=(0.0, 0.0, 1
6566
self.axis = self.axis / self.axis.norm()
6667

6768
self.limits = limits
69+
self.velocity_limits = velocity_limits
70+
self.effort_limits = effort_limits
6871

6972
def to(self, *args, **kwargs):
7073
self.axis = self.axis.to(*args, **kwargs)

‎src/pytorch_kinematics/urdf.py‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -49,9 +49,19 @@ def _build_chain_recurse(root_frame, lmap, joints):
4949
limits = (j.limit.lower, j.limit.upper)
5050
except AttributeError:
5151
limits = None
52+
# URDF assumes symmetric velocity and effort limits
53+
try:
54+
velocity_limits = (-j.limit.velocity, j.limit.velocity)
55+
except AttributeError:
56+
velocity_limits = None
57+
try:
58+
effort_limits = (-j.limit.effort, j.limit.effort)
59+
except AttributeError:
60+
effort_limits = None
5261
child_frame = frame.Frame(j.child)
5362
child_frame.joint = frame.Joint(j.name, offset=_convert_transform(j.origin),
54-
joint_type=JOINT_TYPE_MAP[j.type], axis=j.axis, limits=limits)
63+
joint_type=JOINT_TYPE_MAP[j.type], axis=j.axis, limits=limits,
64+
velocity_limits=velocity_limits, effort_limits=effort_limits)
5565
link = lmap[j.child]
5666
child_frame.link = frame.Link(link.name, offset=_convert_transform(link.origin),
5767
visuals=[_convert_visual(link.visual)])

‎tests/joint_limit_robot.urdf‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
<?xml version="1.0"?>
2+
<robot name="joint_limit_robot">
3+
<!--
4+
This robot is designed just to test that the velocity and effort limits
5+
are captured properly. It consists of revolute, prismatic, and fixed
6+
joints, is not a serial chain, and algorithmically defines joint,
7+
velocity and effort limits as a function of the joint number
8+
(low = j + 8, high = j + 9, v = j, e = j + 4) for easy validation.
9+
-->
10+
<link name="link1" />
11+
<link name="link2" />
12+
<link name="link3" />
13+
<link name="link4" />
14+
<link name="link5" />
15+
<link name="link6" />
16+
17+
<joint name="joint1" type="revolute">
18+
<origin xyz="0.0 0.0 1.0"/>
19+
<axis xyz="0 0 1"/>
20+
<limit lower="9" upper="10" effort="5" velocity="1"/>
21+
<parent link="link1"/>
22+
<child link="link2"/>
23+
</joint>
24+
<joint name="joint2" type="prismatic">
25+
<origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
26+
<axis xyz="1 0 0"/>
27+
<limit lower="10" upper="11" effort="6" velocity="2"/>
28+
<parent link="link2"/>
29+
<child link="link3"/>
30+
</joint>
31+
<joint name="joint3" type="revolute">
32+
<origin xyz="0.0 0.0 0.0" rpy="0 0 1.57079632679"/>
33+
<axis xyz="0 1 0"/>
34+
<limit lower="11" upper="12" effort="7" velocity="3"/>
35+
<parent link="link3"/>
36+
<child link="link4"/>
37+
</joint>
38+
<joint name="joint4" type="revolute">
39+
<origin xyz="0.0 0.0 0.0" rpy="0 0 -1.57079632679"/>
40+
<axis xyz="0 1 0"/>
41+
<limit lower="12" upper="13" effort="8" velocity="4"/>
42+
<parent link="link3"/>
43+
<child link="link5"/>
44+
</joint>
45+
<joint name="joint5" type="fixed">
46+
<parent link="link5"/>
47+
<child link="link6"/>
48+
<origin xyz="0.3 0.0 0.0"/>
49+
</joint>
50+
51+
</robot>

‎tests/joint_no_limit_robot.urdf‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
<?xml version="1.0"?>
2+
<robot name="joint_limit_robot">
3+
<!--
4+
This robot is designed just to test that the velocity and effort limits
5+
are captured properly. It consists of revolute, prismatic, and fixed
6+
joints, is not a serial chain, and algorithmically defines
7+
velocity and effort limits as a function of the joint number
8+
(v = j, e = j + 4) for easy validation. All joint limits are
9+
unspecified.
10+
-->
11+
<link name="link1" />
12+
<link name="link2" />
13+
<link name="link3" />
14+
<link name="link4" />
15+
<link name="link5" />
16+
<link name="link6" />
17+
18+
<joint name="joint1" type="revolute">
19+
<origin xyz="0.0 0.0 1.0"/>
20+
<axis xyz="0 0 1"/>
21+
<limit effort="5" velocity="1"/>
22+
<parent link="link1"/>
23+
<child link="link2"/>
24+
</joint>
25+
<joint name="joint2" type="prismatic">
26+
<origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
27+
<axis xyz="1 0 0"/>
28+
<limit effort="6" velocity="2"/>
29+
<parent link="link2"/>
30+
<child link="link3"/>
31+
</joint>
32+
<joint name="joint3" type="revolute">
33+
<origin xyz="0.0 0.0 0.0" rpy="0 0 1.57079632679"/>
34+
<axis xyz="0 1 0"/>
35+
<limit effort="7" velocity="3"/>
36+
<parent link="link3"/>
37+
<child link="link4"/>
38+
</joint>
39+
<joint name="joint4" type="revolute">
40+
<origin xyz="0.0 0.0 0.0" rpy="0 0 -1.57079632679"/>
41+
<axis xyz="0 1 0"/>
42+
<limit effort="8" velocity="4"/>
43+
<parent link="link3"/>
44+
<child link="link5"/>
45+
</joint>
46+
<joint name="joint5" type="fixed">
47+
<parent link="link5"/>
48+
<child link="link6"/>
49+
<origin xyz="0.3 0.0 0.0"/>
50+
</joint>
51+
52+
</robot>

‎tests/test_attributes.py‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
import os
2+
3+
import pytorch_kinematics as pk
4+
5+
TEST_DIR = os.path.dirname(__file__)
6+
7+
def test_limits():
8+
chain = pk.build_serial_chain_from_urdf(open(os.path.join(TEST_DIR, "kuka_iiwa.urdf")).read(), "lbr_iiwa_link_7")
9+
iiwa_low_individual = []
10+
iiwa_high_individual = []
11+
for joint in chain.get_joints():
12+
# Default velocity and effort limits for the iiwa arm
13+
assert joint.velocity_limits == (-10, 10)
14+
assert joint.effort_limits == (-300, 300)
15+
iiwa_low_individual.append(joint.limits[0])
16+
iiwa_high_individual.append(joint.limits[1])
17+
iiwa_low, iiwa_high = chain.get_joint_limits()
18+
assert iiwa_low == iiwa_low_individual
19+
assert iiwa_high == iiwa_high_individual
20+
chain = pk.build_chain_from_urdf(open(os.path.join(TEST_DIR, "joint_limit_robot.urdf")).read())
21+
nums = []
22+
for joint in chain.get_joints():
23+
# Slice off the "joint" prefix to get just the number of the joint
24+
num = int(joint.name[5])
25+
nums.append(num)
26+
# This robot is defined specifically to test joint limits. For joint
27+
# number `num`, it sets lower, upper, velocity, and effort limits to
28+
# `num+8`, `num+9`, `num`, and `num+4` respectively
29+
assert joint.limits == (num + 8, num + 9)
30+
assert joint.velocity_limits == (-num, num)
31+
assert joint.effort_limits == (-(num + 4), num + 4)
32+
low, high = chain.get_joint_limits()
33+
v_low, v_high = chain.get_joint_velocity_limits()
34+
e_low, e_high = chain.get_joint_effort_limits()
35+
assert low == [x + 8 for x in nums]
36+
assert high == [x + 9 for x in nums]
37+
assert v_low == [-x for x in nums]
38+
assert v_high == [x for x in nums]
39+
assert e_low == [-(x + 4) for x in nums]
40+
assert e_high == [x + 4 for x in nums]
41+
42+
43+
def test_empty_limits():
44+
chain = pk.build_chain_from_urdf(open(os.path.join(TEST_DIR, "joint_no_limit_robot.urdf")).read())
45+
nums = []
46+
for joint in chain.get_joints():
47+
# Slice off the "joint" prefix to get just the number of the joint
48+
num = int(joint.name[5])
49+
nums.append(num)
50+
# This robot is defined specifically to test joint limits. For joint
51+
# number `num`, it sets velocity, and effort limits to
52+
# `num`, and `num+4` respectively, and leaves the lower and upper
53+
# limits undefined
54+
assert joint.limits == (0, 0)
55+
assert joint.velocity_limits == (-num, num)
56+
assert joint.effort_limits == (-(num + 4), num + 4)
57+
low, high = chain.get_joint_limits()
58+
v_low, v_high = chain.get_joint_velocity_limits()
59+
e_low, e_high = chain.get_joint_effort_limits()
60+
assert low == [0] * len(nums)
61+
assert high == [0] * len(nums)
62+
assert v_low == [-x for x in nums]
63+
assert v_high == [x for x in nums]
64+
assert e_low == [-(x + 4) for x in nums]
65+
assert e_high == [x + 4 for x in nums]
66+
67+
68+
if __name__ == "__main__":
69+
test_limits()
70+
test_empty_limits()

0 commit comments

Comments
 (0)