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

update/refactor utils

Browse Source
This commit is contained in:
Brian Delhaisse
2019-04-29 04:09:43 +02:00
parent d136069115
commit 1ff346ffec
18 changed files with 255 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/robots/base.py
+1 -1
View File
@@ -10,7 +10,7 @@ import numpy as np
# import quaternion
from pyrobolearn.simulators import Simulator
from pyrobolearn.utils.orientation import get_rpy_from_quaternion, get_matrix_from_quaternion
from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_matrix_from_quaternion
__author__ = "Brian Delhaisse"
pyrobolearn/robots/cartpole.py
+1 -1
View File
@@ -9,7 +9,7 @@ import sympy
import sympy.physics.mechanics as mechanics
from pyrobolearn.robots.robot import Robot
from pyrobolearn.utils.orientation import get_symbolic_matrix_from_axis_angle, get_matrix_from_quaternion
from pyrobolearn.utils.transformation import get_symbolic_matrix_from_axis_angle, get_matrix_from_quaternion
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
pyrobolearn/robots/cubli.py
+1 -1
View File
@@ -5,7 +5,7 @@
import os
from pyrobolearn.robots.robot import Robot
from pyrobolearn.utils.orientation import get_rpy_from_quaternion
from pyrobolearn.utils.transformation import get_rpy_from_quaternion
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
pyrobolearn/robots/legged_robot.py
+11 -4
View File
@@ -138,7 +138,7 @@ class LeggedRobot(Robot):
raise TypeError("Expecting foot_id to be a list of int, or an int. Instead got: "
"{}".format(type(foot_id)))
def center_of_pressure(self, use_simulator=False):
def center_of_pressure(self, floor_id=None):
r"""
Center of Pressure (CoP).
@@ -164,9 +164,16 @@ class LeggedRobot(Robot):
[1] "Ground Reference Points in Legged Locomotion: Definitions, Biological Trajectories and Control
Implications", Popovic et al., 2005
"""
# self.sim.get_contact_points(self.id, foot_id) # use simulator
# use F/T sensor to get CoP
pass
if floor_id is not None:
# get contact points between the robot's links and the floor
points = self.sim.get_contact_points(body1=self.id, body2=floor_id)
positions = np.array([point[6] for point in points]) # contact positions in world frame
forces = np.array([point[9] for point in points]).reshape(-1, 1) # normal force at contact points
cop = forces * positions / np.sum(forces)
return cop
# check if there are force/pressure sensors at the links/joints
raise NotImplementedError
def zero_moment_point(self, update_com=False, use_simulator=False):
r"""
pyrobolearn/robots/quadcopter.py
+1 -1
View File
@@ -6,7 +6,7 @@ import os
import numpy as np
from pyrobolearn.robots.uav import RotaryWingUAV
from pyrobolearn.utils.orientation import get_matrix_from_quaternion
from pyrobolearn.utils.transformation import get_matrix_from_quaternion
from pyrobolearn.utils.units import inches_to_meters, rpm_to_rad_per_second
__author__ = "Brian Delhaisse"
pyrobolearn/robots/robot.py
+2 -2
View File
@@ -16,7 +16,7 @@ import numpy as np
import collections
import os
from pyrobolearn.utils.orientation import *
from pyrobolearn.utils.transformation import *
from pyrobolearn.robots.base import ControllableBody
@@ -3195,7 +3195,7 @@ class Robot(ControllableBody):
# evals, evecs = np.linalg.eigh(X)
# evals, evecs = evals[::-1], evecs[:,::-1]
# #S, orientation = np.sqrt(evals), self.angular_converter.convertFrom(quaternion.from_rotation_matrix(evecs.T))
# #S, orientation = np.sqrt(evals), self.angular_converter.convert_from(quaternion.from_rotation_matrix(evecs.T))
#
# print(V[0])
# print(V[1])
pyrobolearn/robots/sensors/camera.py
+1 -1
View File
@@ -6,7 +6,7 @@ Cameras have one of the most richest sensory inputs (i.e. visual).
import numpy as np
from pyrobolearn.utils.orientation import get_rpy_from_quaternion
from pyrobolearn.utils.transformation import get_rpy_from_quaternion
from pyrobolearn.robots.sensors.links import LinkSensor
__author__ = "Brian Delhaisse"
pyrobolearn/robots/sensors/links.py
+1 -1
View File
@@ -6,7 +6,7 @@ These include IMU, contact, Camera, and other sensors.
from abc import ABCMeta, abstractmethod
from pyrobolearn.utils.orientation import get_quaternion_product
from pyrobolearn.utils.transformation import get_quaternion_product
from pyrobolearn.robots.sensors.sensor import Sensor
pyrobolearn/robots/sensors/sensor.py
+1 -1
View File
@@ -11,7 +11,7 @@ add some noise to the returned sense value. The type of noise can also be select
from abc import ABCMeta, abstractmethod
import numpy as np
from pyrobolearn.utils.orientation import get_quaternion_product
from pyrobolearn.utils.transformation import get_quaternion_product
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
pyrobolearn/terminal_conditions/terminal_condition.py
+1 -1
View File
@@ -7,7 +7,7 @@ import numpy as np
from pyrobolearn.robots import Robot
from pyrobolearn.states import LinkState
from pyrobolearn.utils.orientation import *
from pyrobolearn.utils.transformation import *
__author__ = "Brian Delhaisse"
pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py
+1 -1
View File
@@ -377,7 +377,7 @@ class BridgeMouseKeyboardWorld(Bridge):
# plane
x_screen = np.array([self.interface.mouse_x, self.interface.mouse_y, self.depth, 1])
x_world = self.world_camera.screen_to_world(x_screen, Vp_inv, P_inv, V_inv)[:3]
point = self.plane.getIntersectionPoint(x_world)
point = self.plane.get_intersection_point(x_world)
# # draw some spheres on the plane
# if self.display_trajectories:
pyrobolearn/utils/converter.py
+50 -38
View File
@@ -1,4 +1,6 @@
# This file describes converter classes which allows to convert from one certain data type to another.
#!/usr/bin/env python
"""Provide converter classes which allows to convert from one certain data type to another.
"""
from abc import ABCMeta, abstractmethod
import numpy as np
@@ -6,6 +8,14 @@ import torch
import quaternion
import collections
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
def roll(lst, shift):
"""Roll elements of a list. This is similar to `np.roll()`"""
@@ -13,10 +23,12 @@ def roll(lst, shift):
def numpy_to_torch(tensor):
return torch.from_numpy(tensor)
"""Convert from numpy array to pytorch tensor."""
return torch.from_numpy(tensor).float()
def torch_to_numpy(tensor):
"""Convert from pytorch tensor to numpy array."""
if tensor.requires_grad:
return tensor.detach().numpy()
return tensor.numpy()
@@ -67,12 +79,12 @@ class TypeConverter(object):
self._to_type = to_type
@abstractmethod
def convertFrom(self, data):
def convert_from(self, data):
"""Convert to the 'from_type'"""
raise NotImplementedError
@abstractmethod
def convertTo(self, data):
def convert_to(self, data):
"""Convert to the 'to_type'"""
raise NotImplementedError
@@ -81,8 +93,8 @@ class TypeConverter(object):
Convert the data to the other type.
"""
if isinstance(data, self.from_type): # or self.from_type is None:
return self.convertTo(data)
return self.convertFrom(data)
return self.convert_to(data)
return self.convert_from(data)
def __call__(self, data):
"""
@@ -100,10 +112,10 @@ class IdentityConverter(TypeConverter):
def __init__(self):
super(IdentityConverter, self).__init__(None, None)
def convertFrom(self, data):
def convert_from(self, data):
return data
def convertTo(self, data):
def convert_to(self, data):
return data
@@ -129,7 +141,7 @@ class NumpyListConverter(TypeConverter):
raise ValueError("Expecting the convention to belong to {0,1,2}")
self.convention = convention
def convertFrom(self, data):
def convert_from(self, data):
"""Convert to list"""
if isinstance(data, self.from_type):
return list(data)
@@ -140,7 +152,7 @@ class NumpyListConverter(TypeConverter):
else:
raise TypeError("Type not known: {}".format(type(data)))
def convertTo(self, data):
def convert_to(self, data):
"""Convert to numpy array"""
if isinstance(data, self.to_type):
return data
@@ -159,13 +171,13 @@ class NumpyListConverter(TypeConverter):
def reshape(self, data, shape):
"""Reshape the data using the converter. Only valid if data is numpy array."""
if not isinstance(data, self.to_type):
data = self.convertTo(data)
data = self.convert_to(data)
return data.reshape(shape)
def transpose(self, data):
"""Transpose the data using the converter"""
if not isinstance(data, self.to_type):
data = self.convertTo(data)
data = self.convert_to(data)
return data.T
@@ -187,7 +199,7 @@ class QuaternionListConverter(TypeConverter):
raise TypeError("Expecting convention to be 0 or 1.")
self.convention = convention
def convertFrom(self, data):
def convert_from(self, data):
"""Convert to list"""
if isinstance(data, self.from_type):
return list(data)
@@ -196,7 +208,7 @@ class QuaternionListConverter(TypeConverter):
else:
raise TypeError("Type not known: {}".format(type(data)))
def convertTo(self, data):
def convert_to(self, data):
"""Convert to quaternion"""
if isinstance(data, self.to_type):
return data
@@ -224,7 +236,7 @@ class QuaternionNumpyConverter(TypeConverter):
raise TypeError("Expecting convention to be 0 or 1.")
self.convention = convention
def convertFrom(self, data):
def convert_from(self, data):
"""Convert to numpy array"""
if isinstance(data, self.from_type):
return data
@@ -233,7 +245,7 @@ class QuaternionNumpyConverter(TypeConverter):
else:
raise TypeError("Type not known: {}".format(type(data)))
def convertTo(self, data):
def convert_to(self, data):
"""Convert to quaternion"""
if isinstance(data, self.to_type):
return data
@@ -245,13 +257,13 @@ class QuaternionNumpyConverter(TypeConverter):
def reshape(self, data, shape):
"""Reshape the data using the converter. Only valid if data is numpy array."""
if not isinstance(data, self.from_type):
data = self.convertFrom(data)
data = self.convert_from(data)
return data.reshape(shape)
def transpose(self, data):
"""Transpose the data using the converter"""
if not isinstance(data, self.from_type):
data = self.convertFrom(data)
data = self.convert_from(data)
return data.T
@@ -274,7 +286,7 @@ class QuaternionPyTorchConverter(TypeConverter):
raise TypeError("Expecting convention to be 0 or 1.")
self.convention = convention
def convertFrom(self, data):
def convert_from(self, data):
"""Convert to pytorch tensor"""
if isinstance(data, self.from_type):
return data
@@ -283,7 +295,7 @@ class QuaternionPyTorchConverter(TypeConverter):
else:
raise TypeError("Type not known: {}".format(type(data)))
def convertTo(self, data):
def convert_to(self, data):
"""Convert to quaternion"""
if isinstance(data, self.to_type):
return data
@@ -295,13 +307,13 @@ class QuaternionPyTorchConverter(TypeConverter):
def reshape(self, data, shape):
"""Reshape the data using the converter. Only valid if data is numpy array."""
if not isinstance(data, self.from_type):
data = self.convertFrom(data)
data = self.convert_from(data)
return data.view(shape)
def transpose(self, data):
"""Transpose the data using the converter"""
if not isinstance(data, self.from_type):
data = self.convertFrom(data)
data = self.convert_from(data)
return data.t()
@@ -321,7 +333,7 @@ class NumpyNumberConverter(TypeConverter):
raise ValueError("The 'dim_array' argument should be 0 or 1.")
self.dim_array = dim_array
def convertFrom(self, data):
def convert_from(self, data):
"""Convert to a number"""
if isinstance(data, self.from_type):
return data
@@ -336,7 +348,7 @@ class NumpyNumberConverter(TypeConverter):
else:
raise TypeError("Type not known: {}".format(type(data)))
def convertTo(self, data):
def convert_to(self, data):
"""Convert to numpy array"""
if isinstance(data, self.to_type):
return data
@@ -370,7 +382,7 @@ class PyTorchListConverter(TypeConverter):
raise ValueError("Expecting the convention to belong to {0,1,2}")
self.convention = convention
def convertFrom(self, data):
def convert_from(self, data):
"""Convert to list"""
if isinstance(data, self.from_type):
return list(data)
@@ -382,7 +394,7 @@ class PyTorchListConverter(TypeConverter):
else:
raise TypeError("Type not known: {}".format(type(data)))
def convertTo(self, data):
def convert_to(self, data):
"""Convert to pytorch tensor"""
if isinstance(data, self.to_type):
return data
@@ -400,13 +412,13 @@ class PyTorchListConverter(TypeConverter):
def reshape(self, data, shape):
"""Reshape the data using the converter. Only valid if data is numpy array."""
if not isinstance(data, self.to_type):
data = self.convertTo(data)
data = self.convert_to(data)
return data.view(shape)
def transpose(self, data):
"""Transpose the data using the converter"""
if not isinstance(data, self.to_type):
data = self.convertTo(data)
data = self.convert_to(data)
return data.t()
@@ -419,7 +431,7 @@ class PyTorchNumpyConverter(TypeConverter):
def __init__(self):
super(PyTorchNumpyConverter, self).__init__(from_type=np.ndarray, to_type=torch.Tensor)
def convertFrom(self, data):
def convert_from(self, data):
"""Convert to numpy array"""
if isinstance(data, self.from_type):
return data
@@ -430,7 +442,7 @@ class PyTorchNumpyConverter(TypeConverter):
else:
raise TypeError("Type not known: {}".format(type(data)))
def convertTo(self, data):
def convert_to(self, data):
"""Convert to pytorch tensor"""
if isinstance(data, self.to_type):
return data
@@ -469,16 +481,16 @@ if __name__ == '__main__':
print("on np.array: a={} with type {}".format(a, type(a)))
b = converter(a)
print("converter(a) gives: {} with type {}".format(b, type(b)))
b = converter.convertFrom(a)
print("converter.convertFrom(a) gives: {} with type {}".format(b, type(b)))
b = converter.convertTo(a)
print("converter.convertTo(a) gives: {} with type {}".format(b, type(b)))
b = converter.convert_from(a)
print("converter.convert_from(a) gives: {} with type {}".format(b, type(b)))
b = converter.convert_to(a)
print("converter.convert_to(a) gives: {} with type {}".format(b, type(b)))
A = np.array(range(4)).reshape(2, 2)
print("on numpy matrix: \nA={} with type {}".format(A, type(A)))
b = converter(A)
print("converter(a) gives: {} with type {}".format(b, type(b)))
b = converter.convertFrom(A)
print("converter.convertFrom(a) gives: {} with type {}".format(b, type(b)))
b = converter.convertTo(A)
print("converter.convertTo(a) gives: \n{} with type {}".format(b, type(b)))
b = converter.convert_from(A)
print("converter.convert_from(a) gives: {} with type {}".format(b, type(b)))
b = converter.convert_to(A)
print("converter.convert_to(a) gives: \n{} with type {}".format(b, type(b)))
pyrobolearn/utils/decorator.py
+36
View File
@@ -0,0 +1,36 @@
#!/usr/bin/env python
"""Define the various decorators used in this framework.
"""
import numpy
import torch
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
def convert_numpy(f):
"""Decorator that converts the given numpy array to a torch tensor and return it back to a numpy array if
specified."""
def wrapper(self, x, to_numpy=False):
# convert to torch Tensor if numpy array
if not isinstance(x, np.ndarray):
x = torch.from_numpy(x).float()
# call inner function on the given argument
x = f(self, x)
# reconvert to numpy array if specified, and return it
if to_numpy:
return x.numpy()
# return torch Tensor
return x
return wrapper
pyrobolearn/utils/interpolator.py
+23 -9
View File
@@ -1,6 +1,18 @@
#!/usr/bin/env python
"""Provide some other interpolators that are not in `scipy`.
"""
import numpy as np
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class HermiteInterpolator(object):
r"""5th order Hermite interpolator
@@ -8,12 +20,14 @@ class HermiteInterpolator(object):
"""
def __init__(self, t, x):
"""Calculate the coefficients for the interpolation.
r"""Calculate the coefficients for the interpolation.
Assuming a trajectory x(t) is described by a fifth order polynomial such that:
.. math:: x(t) = a_5 t^5 + a_4 t^4 + a_3 t^3 + a_2 t^2 + a_1 t + a_0
then taking the derivatives with respect to time give us:
.. math::
\dot{x}(t) = 5 a_5 t^4 + 4 a_4 t^3 + 3 a_3 t^2 + 2 a_2 t + a_1
\ddot{x}(t) = 20 a_5 t^3 + 12 a_4 t^2 + 6 a_3 t + 2 a_2
@@ -47,7 +61,7 @@ class HermiteInterpolator(object):
b = np.array([x[-1], 0, 0, 0, 0, x[0]] + list(x[1:-1]))
else:
b = np.array([x[-1], 0, 0, 0, 0, x[0]])
#coeff = np.linalg.solve(A,b)[0]
# coeff = np.linalg.solve(A,b)[0]
self.coeff = np.linalg.lstsq(A, b, rcond=None)[0]
def __call__(self, t):
@@ -61,7 +75,7 @@ class HermiteInterpolator(object):
float, float[T]: velocity
float, float[T]: acceleration
"""
x = np.sum(self.coeff * np.array([[ti**i for i in range(5,-1,-1)] for ti in t]), axis=1)
x = np.sum(self.coeff * np.array([[ti**i for i in range(5, -1, -1)] for ti in t]), axis=1)
xd = np.sum(self.coeff[:-1] * np.array([[5*ti**4, 4*ti**3, 3*ti**2, 2*ti, 1] for ti in t]), axis=1)
xdd = np.sum(self.coeff[:-2] * np.array([[20*ti**3, 12*ti**2, 6*ti, 2] for ti in t]), axis=1)
return x, xd, xdd
@@ -82,19 +96,19 @@ if __name__ == '__main__':
# interpolate the data
t = np.linspace(0., 1., 100)
x,xd,xdd = x_interpolator(t)
y,yd,ydd = y_interpolator(t)
x, dx, ddx = x_interpolator(t)
y, dy, ddy = y_interpolator(t)
# plot figures
gs = gridspec.GridSpec(4,4)
gs = gridspec.GridSpec(4, 4)
plt.subplot(gs[0, 1:3])
plt.title('Hermite Interpolator')
plt.plot(x,y)
plt.plot(x, y)
plt.xlabel('x(t)')
plt.ylabel('y(t)')
y_labels = ['x(t)', 'y(t)', 'dx/dt', 'dy/dt', 'd^2x/dt^2', 'd^2y/dt^2']
for i, (x_traj, y_traj) in enumerate(zip([x, xd, xdd], [y, yd, ydd])):
for i, (x_traj, y_traj) in enumerate(zip([x, dx, ddx], [y, dy, ddy])):
plt.subplot(gs[i+1, :2])
plt.plot(t, x_traj)
plt.ylabel(y_labels[2*i])
@@ -107,4 +121,4 @@ if __name__ == '__main__':
plt.xlabel('t')
plt.tight_layout()
plt.show()
plt.show()
pyrobolearn/utils/math_utils.py
+25 -10
View File
@@ -1,21 +1,35 @@
# This file defines mathematical operations
#!/usr/bin/env python
"""Defines mathematical operations.
"""
import numpy as np
import copy
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
def exp(x):
if callable(x):
y = copy.copy(x)
def exp():
def exp_():
return np.exp(x())
y.__call__ = exp
y.__call__ = exp_
return y
else:
return np.exp(x)
class Plane(object):
"""Plane class.
r"""Plane class.
A plane is defined by its initial point and its normal vector.
.. math:: \pi \equiv \overline{n} \cdot (\overline{x} - \overline{x}_0) = 0
@@ -38,7 +52,8 @@ class Plane(object):
self.x0 = x0
self.normal = normal
def convertToArray(self, pt):
@staticmethod
def convert_to_array(pt):
if isinstance(pt, (tuple, list)):
pt = np.array(pt)
if not isinstance(pt, np.ndarray):
@@ -56,7 +71,7 @@ class Plane(object):
@x0.setter
def x0(self, x0):
self._x0 = self.convertToArray(x0)
self._x0 = self.convert_to_array(x0)
@property
def normal(self):
@@ -64,7 +79,7 @@ class Plane(object):
@normal.setter
def normal(self, normal):
normal = self.convertToArray(normal)
normal = self.convert_to_array(normal)
# normalize
norm = np.linalg.norm(normal)
if norm < self.threshold:
@@ -73,7 +88,7 @@ class Plane(object):
def __contains__(self, point):
"""Check if the given point is in the plane."""
point = self.convertToArray(point)
point = self.convert_to_array(point)
# scalar product between the normal and (point-x0) vectors
val = self.normal.T.dot(point - self.x0)
@@ -82,9 +97,9 @@ class Plane(object):
return True
return False
def getIntersectionPoint(self, point):
def get_intersection_point(self, point):
"""
Get the intersection of the plane with a line that starts at the given point and is parallel to the normal.
"""
point = self.convertToArray(point)
point = self.convert_to_array(point)
return point + self.normal.T.dot(self.x0 - point) * self.normal
pyrobolearn/utils/mesh.py
+95 -79
View File
@@ -1,13 +1,21 @@
#!/usr/bin/env python
"""Provide the code to create meshes using the `Mayavi` library.
Most of the meshes in the world such as the `cone`, `ellipsoid`, and others were created using the hereby code.
"""
import numpy as np
try:
from mayavi import mlab
except ImportError as e:
raise ImportError(repr(e) + '\nTry to install Mayavi: pip install mayavi')
try:
import gdal
except ImportError as e:
raise ImportError(repr(e) + '\nTry to install gdal: pip install gdal')
pass
# raise ImportError(repr(e) + '\nTry to install gdal: pip install gdal')
import subprocess
import fileinput
@@ -15,6 +23,15 @@ import sys
import os
import scipy.interpolate
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
def recenter(coords):
"""
@@ -37,10 +54,10 @@ def recenter(coords):
c_min, c_max = coords.min(), coords.max()
c_center = c_min + (c_max - c_min) / 2.
return (coords - c_center)
return coords - c_center
def createMesh(x, y, z, filename=None, show=False, center=True):
def create_mesh(x, y, z, filename=None, show=False, center=True):
"""
Create mesh from x,y,z arrays, and save it in the obj format.
@@ -61,32 +78,32 @@ def createMesh(x, y, z, filename=None, show=False, center=True):
x, y, z = a * np.cos(theta) * np.cos(phi), b * np.cos(theta) * np.sin(phi), c * np.sin(theta)
createMesh(x, y, z, show=True)
create_mesh(x, y, z, show=True)
"""
#if not (isinstance(x, np.ndarray) and isinstance(y, np.ndarray) and isinstance(z, np.ndarray)):
# raise TypeError("Expecting x, y, and z to be numpy arrays")
# if not (isinstance(x, np.ndarray) and isinstance(y, np.ndarray) and isinstance(z, np.ndarray)):
# raise TypeError("Expecting x, y, and z to be numpy arrays")
if isinstance(x, list) and isinstance(y, list) and isinstance(z, list):
# create several 3D mesh
for i,j,k in zip(x,y,z):
for i, j, k in zip(x, y, z):
# if we need to recenter
if center:
i,j,k = recenter([i,j,k])
mlab.mesh(i,j,k)
i, j, k = recenter([i, j, k])
mlab.mesh(i, j, k)
else:
# if we need to recenter the data
if center:
x,y,z = recenter([x,y,z])
x, y, z = recenter([x, y, z])
# create 3D mesh
mlab.mesh(x,y,z)
mlab.mesh(x, y, z)
# save mesh
if filename is not None:
if filename[-4:] == '.obj': # This is because the .obj saved by Mayavi is not correct (see in Meshlab)
if filename[-4:] == '.obj': # This is because the .obj saved by Mayavi is not correct (see in Meshlab)
x3dfile = filename[:-4] + '.x3d'
mlab.savefig(x3dfile)
convertX3dToObj(x3dfile, removeX3d=True)
convert_x3d_to_obj(x3dfile, removeX3d=True)
else:
mlab.savefig(filename)
@@ -97,8 +114,8 @@ def createMesh(x, y, z, filename=None, show=False, center=True):
mlab.close()
def createSurfMesh(surface, filename=None, show=False, subsample=None, interpolate_fct='multiquadric',
lower_bound=None, upper_bound=None, dtype=None):
def create_surf_mesh(surface, filename=None, show=False, subsample=None, interpolate_fct='multiquadric',
lower_bound=None, upper_bound=None, dtype=None):
"""
Create surface (heightmap) mesh, and save it in the obj format.
@@ -132,10 +149,10 @@ def createSurfMesh(surface, filename=None, show=False, subsample=None, interpola
import numpy as np
height = np.random.rand(100,100) # in meters
createSurfMesh(height, show=True)
create_surf_mesh(height, show=True)
"""
if isinstance(surface, str):
from utils.heightmap_generator import heightmap_gdal
from pyrobolearn.worlds.utils.heightmap_generator import heightmap_gdal
surface = heightmap_gdal(surface, subsample=subsample, interpolate_fct=interpolate_fct,
lower_bound=lower_bound, upper_bound=upper_bound, dtype=dtype)
@@ -152,7 +169,7 @@ def createSurfMesh(surface, filename=None, show=False, subsample=None, interpola
if filename[-4:] == '.obj': # This is because the .obj saved by Mayavi is not correct (see in Meshlab)
x3dfile = filename[:-4] + '.x3d'
mlab.savefig(x3dfile)
convertX3dToObj(x3dfile, removeX3d=True)
convert_x3d_to_obj(x3dfile, removeX3d=True)
else:
mlab.savefig(filename)
@@ -163,8 +180,8 @@ def createSurfMesh(surface, filename=None, show=False, subsample=None, interpola
mlab.close()
def create3DMesh(heightmap, x=None, y=None, depth_level=1., filename=None, show=False, subsample=None,
interpolate_fct='multiquadric', lower_bound=None, upper_bound=None, dtype=None, center=True):
def create_3d_mesh(heightmap, x=None, y=None, depth_level=1., filename=None, show=False, subsample=None,
interpolate_fct='multiquadric', lower_bound=None, upper_bound=None, dtype=None, center=True):
"""
Create 3D mesh from heightmap (which can be a 2D array or an image (.tif, .png, .jpg, etc), and save it in
the obj format.
@@ -206,7 +223,7 @@ def create3DMesh(heightmap, x=None, y=None, depth_level=1., filename=None, show=
import numpy as np
height = np.random.rand(100,100) # in meters
create3DMesh(height, show=True)
create_3d_mesh(height, show=True)
"""
if isinstance(heightmap, str):
# load data (raster)
@@ -221,8 +238,8 @@ def create3DMesh(heightmap, x=None, y=None, depth_level=1., filename=None, show=
# 4 = column rotation (typically zero)
# 5 = height of a pixel (typically negative)
# numpy array of shape: (channel, height, width)
#dem = data.ReadAsArray()
# # numpy array of shape: (channel, height, width)
# dem = data.ReadAsArray()
# get elevation values (i.e. height values) with shape (height, width)
band = data.GetRasterBand(1)
@@ -275,7 +292,7 @@ def create3DMesh(heightmap, x=None, y=None, depth_level=1., filename=None, show=
# center the coordinates if specified
if center:
x,y = recenter([x,y])
x, y = recenter([x, y])
# create lower plane
z0 = np.min(z) * np.ones(z.shape) - depth_level
@@ -287,16 +304,16 @@ def create3DMesh(heightmap, x=None, y=None, depth_level=1., filename=None, show=
c4 = (np.vstack((x[:, -1], x[:, -1])), np.vstack((y[:, -1], y[:, -1])), np.vstack((z0[:, -1], z[:, -1])))
c = [c1, c2, c3, c4]
# createMesh([x, x] + [i[0] for i in c], [y, y] + [i[1] for i in c], [z, z0] + [i[2] for i in c],
# create_mesh([x, x] + [i[0] for i in c], [y, y] + [i[1] for i in c], [z, z0] + [i[2] for i in c],
# filename=filename, show=show, center=False)
createMesh([x, x] + [i[0] for i in c], [y, y] + [i[1] for i in c], [z, z0] + [i[2] for i in c],
filename=filename, show=show, center=False)
create_mesh([x, x] + [i[0] for i in c], [y, y] + [i[1] for i in c], [z, z0] + [i[2] for i in c],
filename=filename, show=show, center=False)
def createURDFFromMesh(meshfile, filename, position=(0.,0.,0.), orientation=(0.,0.,0.), scale=(1.,1.,1.),
color=(1,1,1,1), texture=None, mass=0., inertia=(0.,0.,0.,0.,0.,0.),
lateral_friction=0.5, rolling_friction=0., spinning_friction=0., restitution=0.,
kp=None, kd=None): #, cfm=0., erf=0.):
def create_urdf_from_mesh(meshfile, filename, position=(0., 0., 0.), orientation=(0., 0., 0.), scale=(1., 1., 1.),
color=(1, 1, 1, 1), texture=None, mass=0., inertia=(0., 0., 0., 0., 0., 0.),
lateral_friction=0.5, rolling_friction=0., spinning_friction=0., restitution=0.,
kp=None, kd=None): # , cfm=0., erf=0.):
"""
Create a URDF file and insert the specified mesh inside.
@@ -314,6 +331,7 @@ def createURDFFromMesh(meshfile, filename, position=(0.,0.,0.), orientation=(0.,
lateral_friction (float): friction coefficient
rolling_friction (float): rolling friction coefficient orthogonal to contact normal
spinning_friction (float): spinning friction coefficient around contact normal
restitution (float): restitution coefficient
kp (float, None): contact stiffness (useful to make surfaces soft). Set it to None/-1 if not using it.
kd (float, None): contact damping (useful to make surfaces soft). Set it to None/-1 if not using it.
#cfm: constraint force mixing
@@ -328,13 +346,13 @@ def createURDFFromMesh(meshfile, filename, position=(0.,0.,0.), orientation=(0.,
- "Tutorial: Using a URDF in Gazebo": http://gazebosim.org/tutorials/?tut=ros_urdf
- SDF format: http://sdformat.org/spec
"""
def getStr(lst):
def get_str(lst):
return ' '.join([str(i) for i in lst])
position = getStr(position)
orientation = getStr(orientation)
color = getStr(color)
scale = getStr(scale)
position = get_str(position)
orientation = get_str(orientation)
color = get_str(color)
scale = get_str(scale)
name = meshfile.split('/')[-1][:-4]
ixx, ixy, ixz, iyy, iyz, izz = [str(i) for i in inertia]
@@ -388,8 +406,7 @@ def createURDFFromMesh(meshfile, filename, position=(0.,0.,0.), orientation=(0.,
f.write('</robot>')
def convertX3dToObj(filename, removeX3d=True):
def convert_x3d_to_obj(filename, removeX3d=True):
"""
Convert a .x3d into an .obj file.
@@ -424,7 +441,7 @@ def convertX3dToObj(filename, removeX3d=True):
raise OSError("Error while running the command `meshlabserver`: {}".format(e))
def convertMesh(fromFilename, toFilename, removeFile=True):
def convert_mesh(fromFilename, toFilename, removeFile=True):
"""
Convert the given file containing the original mesh to the other specified format.
The available formats are the ones supported by `meshlab`.
@@ -457,7 +474,7 @@ def convertMesh(fromFilename, toFilename, removeFile=True):
raise OSError("Error while running the command `meshlabserver`: {}".format(e))
def readObjFile(filename):
def read_obj_file(filename):
r"""
Read an .obj file and returns the whole file, as well as the list of vertices, and faces.
@@ -493,7 +510,7 @@ def readObjFile(filename):
return data, vertices, faces
def flipFaceNormalsInObj(filename):
def flip_face_normals_in_obj(filename):
"""
Flip all the face normals in .obj file.
@@ -516,7 +533,7 @@ def flipFaceNormalsInObj(filename):
f.writelines(data)
def flipFaceNormalsForConvexObj(filename, outward=True):
def flip_face_normals_for_convex_obj(filename, outward=True):
"""
Flip the face normals for convex objects, and rewrite the obj file
@@ -526,7 +543,7 @@ def flipFaceNormalsForConvexObj(filename, outward=True):
inward the object.
"""
# read the obj file
data, vertices, faces = readObjFile(filename)
data, vertices, faces = read_obj_file(filename)
# compute the center of the object
center = np.mean(vertices, axis=0)
@@ -568,7 +585,7 @@ def flipFaceNormalsForConvexObj(filename, outward=True):
f.writelines(data)
def flipFaceNormalsForExpandedObj(filename, expanded_filename, outward=True, remove_expanded_file=False):
def flip_face_normals_for_expanded_obj(filename, expanded_filename, outward=True, remove_expanded_file=False):
r"""
By comparing the expanded object with the original object, we can compute efficiently the normal vector to each
face such that it points outward. Then comparing the direction of these obtained normal vectors with the ones
@@ -580,10 +597,11 @@ def flipFaceNormalsForExpandedObj(filename, expanded_filename, outward=True, rem
has been expanded in every dimension.
outward (bool): if the face normals should point outward. If False, they will be flipped such that they point
inward the object.
remove_expanded_file (bool): if True, it will remove the expanded file.
"""
# read the obj files
d1, v1, f1 = readObjFile(filename)
d2, v2, f2 = readObjFile(expanded_filename)
d1, v1, f1 = read_obj_file(filename)
d2, v2, f2 = read_obj_file(expanded_filename)
# check the size of the obj files (they have to match)
if len(v1) != len(v2) or len(f1) != len(f2):
@@ -633,52 +651,50 @@ def flipFaceNormalsForExpandedObj(filename, expanded_filename, outward=True, rem
if __name__ == '__main__':
# 1. create 3D ellipsoid mesh (see `https://en.wikipedia.org/wiki/Ellipsoid` for more info)
a,b,c,n = 1., 0.5, 0.5, 50
#a,b,c,n = .5, .5, .5, 37
a, b, c, n = 1., 0.5, 0.5, 50
# a, b, c, n = .5, .5, .5, 37
theta, phi = np.meshgrid(np.linspace(-np.pi/2, np.pi/2, n), np.linspace(-np.pi, np.pi, n))
x = a * np.cos(theta) * np.cos(phi)
y = b * np.cos(theta) * np.sin(phi)
z = c * np.sin(theta)
createMesh(x, y, z, show=True)
#createMesh(x, y, z, filename='ellipsoid.obj', show=True)
create_mesh(x, y, z, show=True)
# create_mesh(x, y, z, filename='ellipsoid.obj', show=True)
# 2. create heightmap mesh
height = np.random.rand(100,100) # in meters
createSurfMesh(height, show=True)
create_surf_mesh(height, show=True)
# 3. create right triangular prism
x = np.array([[-0.5,-0.5],
x = np.array([[-0.5, -0.5],
[0.5, 0.5],
[-0.5,-0.5],
[-0.5,-0.5],
[-0.5,0.5],
[0.5,-0.5],
[-0.5, -0.5],
[-0.5, -0.5],
[-0.5, 0.5],
[0.5, -0.5],
[-0.5, 0.5],
[0.5, -0.5]])
y = np.array([[-0.5,0.5],
[-0.5,0.5],
[-0.5,0.5],
[-0.5,0.5],
[-0.5,-0.5],
[-0.5,-0.5],
y = np.array([[-0.5, 0.5],
[-0.5, 0.5],
[-0.5, 0.5],
[-0.5, 0.5],
[-0.5, -0.5],
[-0.5, -0.5],
[0.5, 0.5],
[0.5, 0.5]])
z = np.array([[0.,0.],
[0.,0.],
[1.,1.],
[0.,0.],
[0.,0.],
[0.,1.],
z = np.array([[0., 0.],
[0., 0.],
[1., 1.],
[0., 0.],
[0., 0.],
[0., 1.],
[0., 0.],
[0., 1.]])
#createMesh(x, y, z, show=True)
createMesh(x, y, z, filename='right_triangular_prism.obj', show=True)
flipFaceNormalsForConvexObj('right_triangular_prism.obj', outward=True)
exit()
# create_mesh(x, y, z, show=True)
create_mesh(x, y, z, filename='right_triangular_prism.obj', show=True)
flip_face_normals_for_convex_obj('right_triangular_prism.obj', outward=True)
# 4. create cone
radius, height, n = 0.5, 1., 50
@@ -686,17 +702,17 @@ if __name__ == '__main__':
[h, theta] = np.meshgrid((0., height), np.linspace(0, 2*np.pi, n))
x, y, z = r * np.cos(theta), r * np.sin(theta), h
# close the cone at the bottom
[r, theta] = np.meshgrid((0., radius), np.linspace(0, 2*np.pi, n))
[r, theta] = np.meshgrid((0., radius), np.linspace(0, 2*np.pi, n))
x = np.vstack((x, r * np.cos(theta)))
y = np.vstack((y, r * np.sin(theta)))
z = np.vstack((z, np.zeros(r.shape)))
createMesh(x, y, z, show=True)
#createMesh(x, y, z, filename='cone.obj', show=True)
create_mesh(x, y, z, show=True)
# create_mesh(x, y, z, filename='cone.obj', show=True)
# 5. create 3D heightmap
dx, dy, dz = 5., 5., 0.01
x,y = np.meshgrid(np.linspace(-dx, dx, int(2*dx)), np.linspace(-dy, dy, int(2*dy)))
x, y = np.meshgrid(np.linspace(-dx, dx, int(2*dx)), np.linspace(-dy, dy, int(2*dy)))
z = np.random.rand(*x.shape) + dz
# z0 = np.zeros(x.shape)
@@ -709,6 +725,6 @@ if __name__ == '__main__':
# c4 = (np.vstack((x[:,-1], x[:,-1])), np.vstack((y[:,-1], y[:,-1])), np.vstack((z0[:,-1], z[:,-1])))
# c = [c1,c2,c3,c4]
#
# createMesh([x,x]+[i[0] for i in c], [y,y]+[i[1] for i in c], [z,z0]+[i[2] for i in c], show=True)
# create_mesh([x,x]+[i[0] for i in c], [y,y]+[i[1] for i in c], [z,z0]+[i[2] for i in c], show=True)
create3DMesh(z, x, y, dz, show=True)
create_3d_mesh(z, x, y, dz, show=True)
pyrobolearn/utils/orientation.py → pyrobolearn/utils/transformation.py
+3 -3
View File
@@ -610,7 +610,7 @@ def logarithm_map(q):
Returns:
float[3]: resulting 3d vector
"""
q = quat_converter.convertTo(q)
q = quat_converter.convert_to(q)
v, u = q.w, np.array([q.x, q.y, q.z])
zero = np.zeros(3)
@@ -647,8 +647,8 @@ def angular_velocity_from_quaternion(q1, q2):
Returns:
float[3]: angular velocity (angular error in :math:`R^3`)
"""
q1 = quat_converter.convertTo(q1)
q2 = quat_converter.convertTo(q2)
q1 = quat_converter.convert_to(q1)
q2 = quat_converter.convert_to(q2)
return 2 * logarithm_map(q1 * q2)
pyrobolearn/worlds/world_camera.py
+1 -1
View File
@@ -9,7 +9,7 @@ Dependencies:
import numpy as np
from pyrobolearn.utils.orientation import get_quaternion_from_matrix, get_rpy_from_matrix, get_rpy_from_quaternion
from pyrobolearn.utils.transformation import get_quaternion_from_matrix, get_rpy_from_matrix, get_rpy_from_quaternion
from pyrobolearn.simulators import Simulator