Skip to content

Commit 84013d0

Browse files
committed
Jm now accepts axes
1 parent 08f2e59 commit 84013d0

File tree

3 files changed

+23
-7
lines changed

3 files changed

+23
-7
lines changed

roboticstoolbox/models/URDF/UR5.py

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -29,13 +29,14 @@ class UR5(ERobot):
2929

3030
def __init__(self):
3131

32-
args = super().urdf_to_ets_args(
32+
elinks, name = super().urdf_to_ets_args(
3333
"ur_description/urdf/ur5_joint_limited_robot.urdf.xacro")
3434

3535
super().__init__(
36-
args[0],
37-
name=args[1],
38-
manufacturer='Universal Robotics'
36+
elinks,
37+
name=name,
38+
manufacturer='Universal Robotics',
39+
gripper_links=elinks[7]
3940
)
4041

4142
self.addconfiguration("qz", np.array([0, 0, 0, 0, 0, 0]))

roboticstoolbox/robot/ERobot.py

Lines changed: 15 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1591,7 +1591,7 @@ def cross(a, b):
15911591

15921592
return H
15931593

1594-
def jacobm(self, q=None, J=None, H=None, end=None, start=None):
1594+
def jacobm(self, q=None, J=None, H=None, end=None, start=None, axes='all'):
15951595
"""
15961596
Calculates the manipulability Jacobian. This measure relates the rate
15971597
of change of the manipulability to the joint velocities of the robot.
@@ -1621,6 +1621,15 @@ def jacobm(self, q=None, J=None, H=None, end=None, start=None):
16211621
end, start, _ = self._get_limit_links(end, start)
16221622
path, n = self.get_path(end, start)
16231623

1624+
if axes == 'all':
1625+
axes = [True, True, True, True, True, True]
1626+
elif axes.startswith('trans'):
1627+
axes = [True, True, True, False, False, False]
1628+
elif axes.startswith('rot'):
1629+
axes = [False, False, False, True, True, True]
1630+
else:
1631+
raise ValueError('axes must be all, trans or rot')
1632+
16241633
if J is None:
16251634
if q is None:
16261635
q = np.copy(self.q)
@@ -1637,7 +1646,11 @@ def jacobm(self, q=None, J=None, H=None, end=None, start=None):
16371646
verifymatrix(H, (6, n, n))
16381647

16391648
manipulability = self.manipulability(
1640-
q, J=J, start=start, end=end)
1649+
q, J=J, start=start, end=end, axes=axes)
1650+
1651+
J = J[axes, :]
1652+
H = H[axes, :, :]
1653+
16411654
b = np.linalg.inv(J @ np.transpose(J))
16421655
Jm = np.zeros((n, 1))
16431656

roboticstoolbox/robot/Robot.py

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -383,7 +383,9 @@ def manipulability(
383383
- Robotics, Vision & Control, Chap 8, P. Corke, Springer 2011.
384384
385385
"""
386-
if axes == 'all':
386+
if isinstance(axes, list) and len(axes) == 6:
387+
pass
388+
elif axes == 'all':
387389
axes = [True, True, True, True, True, True]
388390
elif axes.startswith('trans'):
389391
axes = [True, True, True, False, False, False]

0 commit comments

Comments
 (0)