From 521b9d2677b512936912d09bb5902e16e563d8b3 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Tue, 18 Jun 2019 15:32:46 +0200 Subject: [PATCH] add CoM manipulability tracking example + update robot for manipulability (by L. Rozo and N. Jaquier) --- examples/manipulability/README.md | 8 ++ .../com_manipulability_tracking.py | 131 ++++++++++++++++++ pyrobolearn/robots/robot.py | 109 +++++++++++---- pyrobolearn/utils/manifold_utils.py | 6 +- 4 files changed, 222 insertions(+), 32 deletions(-) create mode 100644 examples/manipulability/README.md create mode 100644 examples/manipulability/com_manipulability_tracking.py diff --git a/examples/manipulability/README.md b/examples/manipulability/README.md new file mode 100644 index 0000000..f4bfdc7 --- /dev/null +++ b/examples/manipulability/README.md @@ -0,0 +1,8 @@ +## Manipulability Ellipsoids + +We provide examples on how to use manipulability ellipsoids. + +References: +[1] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010 +[2] "Springer Handbook of Robotics", Siciliano et al., 2008 +[3] "Geometry-aware Tracking of Manipulability Ellipsoids", Jaquier et al., R:SS, 2018 diff --git a/examples/manipulability/com_manipulability_tracking.py b/examples/manipulability/com_manipulability_tracking.py new file mode 100644 index 0000000..dc4ec17 --- /dev/null +++ b/examples/manipulability/com_manipulability_tracking.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python +"""Center of mass manipulability tracking + +Track the velocity manipulability ellipsoid of the center of mass of a particular robot. In this example, the robot +base is fixed. + +See Also: + - `file.py`: short description + +References: + [1] "Robotics: Modelling, Planning and Control" (section 3.9), Siciliano et al., 2010 + [2] "Geometry-aware Tracking of Manipulability Ellipsoids", Jaquier et al., R:SS, 2018 +""" + +from itertools import count +import numpy as np + +from pyrobolearn.simulators import Bullet +from pyrobolearn.worlds import BasicWorld +from pyrobolearn.robots import Centauro, Cogimon, Nao, KukaIIWA + + +# select robot to use +robot_name = 'nao' # 'kuka_iiwa', 'cogimon', 'centauro' +dt = 0.01 + +# Create simulator and world +sim = Bullet() +world = BasicWorld(sim) + +# Define robot, velocity manipulability for CoM, and proportional gain +if robot_name == 'kuka_iiwa': + # load robot + robot = KukaIIWA(sim) + + # desired Velocity Manipulability for CoM + desired_velocity_manip = np.array([[0.02193921, -0.01192746, -0.0155832], + [-0.01192746, 0.04524443, 0.01892251], + [-0.0155832, 0.01892251, 0.02259072]]) + + # proportional gain + Km = 5 * np.eye(6) + +elif robot_name == 'nao': + # load robot + robot = Nao(sim, fixed_base=True) + + # # desired Velocity Manipulability for CoM + # desired_velocity_manip = np.array([[2.53907684e-03, 2.65373209e-04, 1.83354058e-04], + # [2.65373209e-04, 2.08082139e-03, -5.84361397e-05], + # [1.83354058e-04, -5.84361397e-05, 8.60170044e-04]]) + desired_velocity_manip = np.array([[1.0e-03, 0.0, 0.0], + [0.0, 3.0e-03, 0.0], + [0.0, 0.0, 1.0e-04]]) + + # proportional gain + Km = 500 * np.eye(6) + +elif robot_name == 'cogimon': + # load robot + robot = Cogimon(sim, fixed_base=True) + + # # desired Velocity Manipulability for CoM + # desired_velocity_manip = np.array([[0.02045482, 0.00356193, 0.00081623], + # [0.00356193, 0.04693186, 0.01798659], + # [0.00081623, 0.01798659, 0.01636723]]) + desired_velocity_manip = np.array([[0.06, 0.0, 0.0], + [0.0, 0.01, 0.0], + [0.0, 0.0, 0.005]]) + + # proportional gain + Km = 500 * np.eye(6) + +elif robot_name == 'centauro': + # load robot + robot = Centauro(sim, fixed_base=True) + + # desired Velocity Manipulability for CoM + desired_velocity_manip = np.array([[0.01, 0.0, 0.0], + [0.0, 0.04, 0.0], + [0.0, 0.0, 0.005]]) + + # proportional gain + Km = 200 * np.eye(6) + +else: + raise NotImplementedError("The given robot has not been implemented") + +# Initial conditions for visualization +# Display initial and desired manipulability ellipsoid +q0 = robot.get_joint_positions() +Jcom = robot.get_center_of_mass_jacobian(q0) +velocity_manip = robot.compute_velocity_manipulability_ellipsoid(Jcom) +robot.draw_velocity_manipulability_ellipsoid(link_id=-1, JJT=10*desired_velocity_manip, color=(0.1, 0.75, 0.1, 0.6)) +ellipsoid_id = robot.draw_velocity_manipulability_ellipsoid(link_id=-1, JJT=10 * velocity_manip[0:3, 0:3], + color=(0.75, 0.1, 0.1, 0.6)) + +# Run simulator +for i in count(): + robot.compute_and_draw_com_position(radius=0.03) + print("CoM: {}".format(robot.get_center_of_mass_position())) + + # get current joint position + qt = robot.get_joint_positions() + + # get center of mass jacobian + Jcom = robot.get_center_of_mass_jacobian(qt) + print("CoM velocity: {}".format(robot.get_center_of_mass_velocity())) + print("Jcom.dot(qt) = {}".format(Jcom.dot(robot.get_joint_velocities()))) + + # Get Inertia Matrix + # M = robot.get_mass_matrix(q=qt) + # print("M: {}".format(M)) + + # Tracking of CoM velocity manipulability + velocity_manip = robot.compute_velocity_manipulability_ellipsoid(Jcom) + print("Mv: {}".format(velocity_manip[0:3, 0:3])) + + # Plot current manipulability ellipsoid + if i % 10 == 0: + robot.update_manipulability_ellipsoid(link_id=-1, ellipsoid_id=ellipsoid_id, + ellipsoid=10 * velocity_manip[:3, :3], color=(0.75, 0.1, 0.1, 0.6)) + + # Obtaining joint velocity command + dq = robot.calculate_inverse_differential_kinematics_velocity_manipulability(Jcom, desired_velocity_manip, Km)[0] + + # Set joint position + q = qt + dq * dt + robot.set_joint_positions(q) + + world.step(sleep_dt=dt) diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py index 47dc0f4..72168d8 100644 --- a/pyrobolearn/robots/robot.py +++ b/pyrobolearn/robots/robot.py @@ -25,7 +25,8 @@ from pyrobolearn.robots.base import ControllableBody __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["Brian Delhaisse", "Leonel Rozo", "Songyan Xin"] +__credits__ = ["Brian Delhaisse (general)", "Leonel Rozo (manipulability)", + "Noemie Jaquier (manipulability)", "Songyan Xin (centroidal dynamics)"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" @@ -2022,7 +2023,7 @@ class Robot(ControllableBody): float: manipulability measure :math:`w(q)` References: - [1] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010, chap 3.5 and 3.9 + [1] "Robotics: Modelling, Planning and Control" (chap 3.5 and 3.9), Siciliano et al., 2010 """ return np.sqrt(np.linalg.det(self.get_JJT(jacobian))) @@ -2035,6 +2036,9 @@ class Robot(ControllableBody): Returns: np.array[D,D]: velocity manipulability + + References: + [1] "Robotics: Modelling, Planning and Control" (section 3.9), Siciliano et al., 2010 """ return self.get_JJT(jacobian) @@ -2047,6 +2051,9 @@ class Robot(ControllableBody): Returns: np.array[D,D]: force manipulability + + References: + [1] "Robotics: Modelling, Planning and Control" (section 3.9), Siciliano et al., 2010 """ return np.linalg.inv(self.get_JJT(jacobian)) @@ -2067,7 +2074,7 @@ class Robot(ControllableBody): bool: True if in a singular configuration References: - [1] "Robotics: Modelling, Planning, and Control" (book), Siciliano et al., 2010, chap 3.3 + [1] "Robotics: Modelling, Planning, and Control" (chap 3.3), Siciliano et al., 2010 """ # TODO: define close to singular configuration using SVD J = jacobian @@ -3782,54 +3789,98 @@ class Robot(ControllableBody): return orientation, scale - def draw_velocity_manipulability_ellipsoid(self, link_id, Jlin=None, JJT=None, color=(0, 1, 0, 0.7)): + def draw_ellipsoid_from_matrix(self, ellipsoid, position, color=(0, 1, 0, 0.7)): r""" - evecs of JJ^T = directions - singular values of JJ^T = dimensions + Draw the manipulability ellipsoid at the specified link position (provided by the link id); the directions of + the ellipsoid are given by the eigenvectors of the ellipsoid matrix :math:`evecs(E)` and the dimension scales + are given by the singular values of :math:`\sigma(E)` where :math:`E` is the ellipsoid matrix. + + Args: + ellipsoid (np.array): ellipsoid matrix (on which SVD will be performed to get the directions + position (np.array): cartesian world position to draw the ellipsoid + color (tuple of 4 float): RGBA color (each channel is between 0 and 1) + + Returns: + int: id of the visual ellipsoid + """ + orientation, scale = self.get_ellipsoid_orientation_and_scale(ellipsoid) + return self.draw3d_ellipsoid(position, orientation, scale=scale, color=color) + + def draw_velocity_manipulability_ellipsoid(self, link_id, linear_jacobian=None, JJT=None, color=(0, 1, 0, 0.7)): + r""" + Draw the velocity manipulability ellipsoid using the linear jacobian; the directions of the ellipsoid are + given by the eigenvectors :math:`evecs(JJ^T)` and the dimension scales are given by the singular values of + :math:`JJ^T` where :math:`J` is the linear jacobian. Args: link_id (int): link id. This will be used to check where to draw the ellipsoid. - J (np.array[3,N], None): linear Jacobian matrix. It doesn't need to be provided if `JJT` is given. + linear_jacobian (np.array[3,N], None): linear Jacobian matrix. It doesn't need to be provided if `JJT` is + given. JJT (np.array[3,3], None): if None, it will compute it using the provided linear Jacobian matrix. + color (tuple of 4 float): RGBA color (each channel is between 0 and 1) + + Returns: + int: id of the visual ellipsoid + + References: + [1] "Robotics: Modelling, Planning and Control" (section 3.9), Siciliano et al., 2010 + """ + if JJT is None: + if linear_jacobian is None: + raise ValueError("Please provide the linear Jacobian matrix") + JJT = self.get_JJT(linear_jacobian) + + position = self.get_link_world_positions(link_id) + return self.draw_ellipsoid_from_matrix(JJT, position=position, color=color) + + def draw_force_manipulability_ellipsoid(self, link_id, linear_jacobian=None, JJT=None, color=(0, 0, 1, 0.7)): + r""" + Draw the force manipulability ellipsoid using the linear jacobian; the directions of the ellipsoid are + given by the eigenvectors :math:`evecs((JJ^T)^{-1})` and the dimension scales are given by the singular values + of :math:`(JJ^T)^{-1}` where :math:`J` is the linear jacobian. + + Kineto-statics duality: direction with good velocity manipulability is obtained a direction along which poor + force manipulability is obtained. + + Args: + link_id (int): link id. This will be used to check where to draw the ellipsoid. + linear_jacobian (np.array[3,N], None): linear Jacobian matrix. It doesn't need to be provided if `JJT` is + given. + JJT (np.array[3,3], None): if None, it will compute it using the provided linear Jacobian matrix. + color (tuple of 4 float): RGBA color (each channel is between 0 and 1) Returns: int: id of the visual ellipsoid """ if JJT is None: - if Jlin is None: + if linear_jacobian is None: raise ValueError("Please provide the linear Jacobian matrix") - JJT = self.get_JJT(Jlin) + JJT = self.get_JJT(linear_jacobian) - orientation, scale = self.get_ellipsoid_orientation_and_scale(JJT) - - # load ellipsoid position = self.get_link_world_positions(link_id) - self.draw3d_ellipsoid(position, orientation, scale=scale, color=color) + return self.draw_ellipsoid_from_matrix(np.linalg.inv(JJT), position=position, color=color) - def draw_force_manipulability_ellipsoid(self, link_id, J=None, JJT=None): - r""" - Kineto-statics duality: direction with good velocity manipulability is obtained a direction along which poor - force manipulability is obtained. - - ..math:: evecs((JJ^T)^{-1}) - - Args: - link_id: - J: - JJT: - """ - pass - - def update_manipulability_ellipsoid(self, link_id, ellipsoid_id): + def update_manipulability_ellipsoid(self, link_id, ellipsoid_id, ellipsoid, color=(0, 1, 0, 0.7)): """ Update the position, orientation, and scaling of the given manipulability ellipsoid. Warnings: currently, the bullet simulator do not allow to update the scale, only the position and orientation. + + Args: + link_id (int): link id. This will be used to check where to draw the ellipsoid. + ellipsoid_id (int): id of the ellipsoid to update. + ellipsoid (np.array): manipulability ellipsoid matrix + color (tuple of 4 float): RGBA color (each channel is between 0 and 1) + + Returns: + int: id of the new visual ellipsoid """ - pass # orientation, scale = self.get_ellipsoid_orientation_and_scale(ellipsoid) # position = self.get_link_world_positions(link_id) # self.sim.reset_base_pose(ellipsoid_id, position, orientation) + self.remove_manipulability_ellipsoid(ellipsoid_id) + position = self.get_link_world_positions(link_id) + return self.draw_ellipsoid_from_matrix(ellipsoid, position=position, color=color) def remove_manipulability_ellipsoid(self, ellipsoid_id): """ diff --git a/pyrobolearn/utils/manifold_utils.py b/pyrobolearn/utils/manifold_utils.py index 4484d38..d3b5bed 100644 --- a/pyrobolearn/utils/manifold_utils.py +++ b/pyrobolearn/utils/manifold_utils.py @@ -6,9 +6,9 @@ import numpy as np import scipy.linalg -__author__ = "Leonel Rozo" +__author__ = ["Leonel Rozo", "Noemie Jaquier"] __copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["Leonel Rozo"] +__credits__ = ["Leonel Rozo", "Noemie Jaquier"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" @@ -152,7 +152,7 @@ def distance_spd(S1, S2): def parallel_transport(S1, S2): - """ + r""" Parallel transport operation. Args: