Add jacobian and manipulability

rofunc/utils/robolab/kinematics/__init__.py

@@ -1,4 +1,5 @@
 from .utils import check_urdf
-from .fk import fk
-from .ik import ik
+from .fk import get_fk_from_model
+from .jacobian import get_jacobian_from_model
+from .ik import get_ik_from_model
 from .ik_dual import ik_dual

rofunc/utils/robolab/kinematics/fk.py

@@ -1,8 +1,16 @@
 from urdfpy import URDF
 
 
-def fk(urdf_path, joint_name, joint_value, export_link='panda_left_link7'):
-    robot = URDF.load(urdf_path)
+def get_fk_from_model(file_path, joint_name, joint_value, export_link='panda_left_link7'):
+    """
+    Get the forward kinematics of the robot from the 3D model
+    :param file_path:
+    :param joint_name:
+    :param joint_value:
+    :param export_link:
+    :return:
+    """
+    robot = URDF.load(file_path)
     link_name = []
     for link in robot.links:
         str = link.name

rofunc/utils/robolab/kinematics/ik.py

@@ -10,7 +10,7 @@
 damp = 1e-12
 
 
-def ik(model, POSE, ORI, JOINT_ID):
+def get_ik_from_model(model, POSE, ORI, JOINT_ID):
     data = model.createData()
 
     oMdes = pinocchio.SE3(ORI, np.array(POSE))

rofunc/utils/robolab/kinematics/jacobian.py

@@ -0,0 +1,31 @@
+import numpy as np
+
+
+def get_jacobian_from_model(file_path, end_link_name, joint_values):
+    """
+    Get the Jacobian matrix J of the robot from the 3D model, \dot x= J \dot q
+    :param file_path:
+    :param joint_values:
+    :return:
+    """
+    import kinpy as kp
+    if file_path.endswith('.urdf'):
+        chain = kp.build_serial_chain_from_urdf(open(file_path).read(), end_link_name=end_link_name)
+    elif file_path.endswith('.xml'):
+        chain = kp.build_serial_chain_from_mjcf(open(file_path).read(), end_link_name=end_link_name)
+
+    J = chain.jacobian(joint_values)
+
+    ret = chain.forward_kinematics(joint_values, end_only=False)
+
+    # viz = kp.Visualizer()
+    # viz.add_robot(ret, chain.visuals_map(), mesh_file_path="/home/ubuntu/Github/Manipulation/kinpy/examples/kuka_iiwa/", axes=True)
+    # viz.spin()
+
+    return J
+
+
+if __name__ == '__main__':
+    urdf_path = "/home/ubuntu/Github/Manipulation/kinpy/examples/kuka_iiwa/model.urdf"
+    joint_values = [0.0, -np.pi / 4.0, 0.0, np.pi / 2.0, 0.0, np.pi / 4.0, -np.pi / 4.0]
+    get_jacobian_from_model(urdf_path, end_link_name='lbr_iiwa_link_7', joint_values=joint_values)

rofunc/utils/robolab/manipulability/__init__.py

@@ -0,0 +1 @@
+from .mpb import get_mpb_from_urdf

rofunc/utils/robolab/manipulability/mpb.py

@@ -0,0 +1,37 @@
+import numpy as np
+
+from rofunc.utils.robolab.kinematics.jacobian import get_jacobian_from_model
+from rofunc.utils.visualab.ellipsoid import ellipsoid_plot3d
+
+
+def get_mpb_from_model(file_path, end_link_name, joint_values, show=False):
+    """
+    Get the manipulability of the robot from the 3D model
+    :param file_path: path to the robot model file
+    :return: the manipulability of the robot
+    """
+    J = get_jacobian_from_model(file_path, end_link_name, joint_values)
+
+    # calculate the velocity manipulability
+    A = J @ J.T
+    vel_eig_values, vel_eig_vectors = np.linalg.eig(A)
+    vel_M = np.sqrt(np.linalg.det(A))
+
+    # calculate the force manipulability
+    force_eig_values = np.reciprocal(vel_eig_values)
+    force_eig_vectors = vel_eig_vectors
+    A_inv = np.linalg.inv(A)
+    # force_eig_values, force_eig_vectors = np.linalg.eig(A_inv)
+    force_M = np.sqrt(np.linalg.det(A_inv))
+
+    if show:
+        ellipsoid_plot3d(np.array([vel_eig_values[:3], force_eig_values[:3]]), mode='given',
+                         Rs=np.array([vel_eig_vectors[:3, :3], vel_eig_vectors[:3, :3]]))
+
+    return vel_M, vel_eig_values, vel_eig_vectors, force_M, force_eig_values, force_eig_vectors
+
+
+if __name__ == '__main__':
+    urdf_path = "/home/ubuntu/Github/Manipulation/kinpy/examples/kuka_iiwa/model.urdf"
+    joint_values = [0, -np.pi / 4.0, -np.pi / 4.0, np.pi / 2.0, 0.0, np.pi / 4.0, -np.pi / 4.0]
+    get_mpb_from_model(urdf_path, end_link_name='lbr_iiwa_link_7', joint_values=joint_values, show=True)

rofunc/utils/visualab/ellipsoid.py

@@ -8,13 +8,16 @@
 import nestle
 
 
-def ellipsoid_plot3d(ellipsoids):
+def ellipsoid_plot3d(ellipsoids, mode='quaternion', Rs=None):
     """
     Plot the ellipsoids in 3d
-    Args:
-        ellipsoids: list or array including several 7-dim pose (3 for center, 4 for (w, x, y, z), )
-
+    :param ellipsoids: list of ellipsoids to plot
+    :param mode: 'quaternion' or 'euler' or 'given'
+    :param Rs: rotation matrices
+    :return: None
     """
+    if mode == 'given':
+        assert Rs is not None, "Rotation matrices are not given"
     fig = plt.figure(figsize=(8, 8))
     ax = fig.add_subplot(111, projection='3d')
 
@@ -27,25 +30,48 @@ def ellipsoid_plot3d(ellipsoids):
     # compute each and plot each ellipsoid iteratively
     for index in range(n_ellip):
         # your ellipsoid and center in matrix form
-        center = ellipsoids[index, :3]
-        R = rf.coord.quaternion_matrix(ellipsoids[index, 3:7])
-
-        # find the rotation matrix and radii of the axes
-        U, s, rotation = linalg.svd(R)
-        radii = 1.0 / np.sqrt(s) * 0.3  # reduce radii by factor 0.3
-
-        # calculate cartesian coordinates for the ellipsoid surface
-        u = np.linspace(0.0, 2.0 * np.pi, 60)
-        v = np.linspace(0.0, np.pi, 60)
-        x = radii[0] * np.outer(np.cos(u), np.sin(v))
-        y = radii[1] * np.outer(np.sin(u), np.sin(v))
-        z = radii[2] * np.outer(np.ones_like(u), np.cos(v))
+        if mode in ['quaternion', 'euler']:
+            center = ellipsoids[index, :3]
+
+            if mode == 'quaternion':
+                R = rf.robolab.coord.quaternion_matrix(ellipsoids[index, 3:7])
+            elif mode == 'euler':
+                R = rf.robolab.coord.euler_matrix(ellipsoids[index, 3], ellipsoids[index, 4], ellipsoids[index, 5],
+                                                  'sxyz')
+
+            # find the rotation matrix and radii of the axes
+            U, s, rotation = linalg.svd(R)
+            radii = 1.0 / np.sqrt(s) * 0.3  # reduce radii by factor 0.3
+
+            # calculate cartesian coordinates for the ellipsoid surface
+            u = np.linspace(0.0, 2.0 * np.pi, 60)
+            v = np.linspace(0.0, np.pi, 60)
+            x = radii[0] * np.outer(np.cos(u), np.sin(v))
+            y = radii[1] * np.outer(np.sin(u), np.sin(v))
+            z = radii[2] * np.outer(np.ones_like(u), np.cos(v))
+
+        elif mode == 'given':
+            center = np.zeros(3)
+
+            rotation = Rs[index]
+            radii = ellipsoids[index]
+            # calculate cartesian coordinates for the ellipsoid surface
+            u = np.linspace(0.0, 2.0 * np.pi, 60)
+            v = np.linspace(0.0, np.pi, 60)
+            x = radii[0] * np.outer(np.cos(u), np.sin(v))
+            y = radii[1] * np.outer(np.sin(u), np.sin(v))
+            z = radii[2] * np.outer(np.ones_like(u), np.cos(v))
+
+        else:
+            raise ValueError("Unknown mode")
 
         for i in range(len(x)):
             for j in range(len(x)):
                 [x[i, j], y[i, j], z[i, j]] = np.dot([x[i, j], y[i, j], z[i, j]], rotation) + center
 
-        ax.plot_surface(x, y, z, rstride=3, cstride=3, color=m.to_rgba(index), linewidth=0.1, alpha=0.5, shade=True)
+        ax.plot_surface(x, y, z, rstride=3, cstride=3, color=m.to_rgba(index), linewidth=0.1, alpha=0.2, shade=True)
+
+    # rf.visualab.set_axis(ax)
     plt.show()
 
 
@@ -109,8 +135,7 @@ def plot_ellipsoid_3d(ell, ax, color, alpha):
     # transform points to ellipsoid
     for i in range(len(x)):
         for j in range(len(x)):
-            x[i, j], y[i, j], z[i, j] = ell.ctr + np.dot(ell.axes,
-                                                         [x[i, j], y[i, j], z[i, j]])
+            x[i, j], y[i, j], z[i, j] = ell.ctr + np.dot(ell.axes, [x[i, j], y[i, j], z[i, j]])
 
     ax.plot_surface(x, y, z, rstride=4, cstride=4, color=color, alpha=alpha)