wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-09 11:31:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

add CoM manipulability tracking example + update robot for manipulability (by L. Rozo and N. Jaquier)

Browse Source
This commit is contained in:
Brian Delhaisse
2019-06-18 15:32:46 +02:00
parent e73af66d40
commit 521b9d2677
4 changed files with 222 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/manipulability/README.md
+8
View File
@@ -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
examples/manipulability/com_manipulability_tracking.py
+131
View File
@@ -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)
pyrobolearn/robots/robot.py
+80 -29
View File
@@ -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):
"""
pyrobolearn/utils/manifold_utils.py
+3 -3
View File
@@ -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: