Merge branch 'master' into simplify-ros-load

# Conflicts:
#	CHANGELOG.rst

Author: gonzalocasas

AUTHORS.rst

@@ -17,7 +17,8 @@ If you use COMPAS FAB in a project, please use the following citation:
             Helmreich, M. and
             Gandia, A. and
             Ma, Z. and
-            Ariza, I.
+            Ariza, I. and
+            Pacher, M.
             },
         howpublished={https://github.com/compas-dev/compas\_fab/},
         note={Gramazio Kohler Research, ETH Z\"{u}rich},
@@ -37,3 +38,4 @@ Authors
 * Augusto Gandia <[email protected]> `@augustogandia <https://github.com/augustogandia>`_
 * Zhao Ma <[email protected]> `@xarthurx <https://github.com/xarthurx>`_
 * Inés Ariza <[email protected]> `@inesariza <https://github.com/inesariza>`_
+* Matteo Pacher <[email protected]> `@matteo-pacher <https://github.com/matteo-pacher>`_

CHANGELOG.rst

@@ -13,6 +13,8 @@ Unreleased
 **Added**
 
 * Added ``load_robot`` method to ROS client to simplify loading robots from running ROS setup.
+* Added ``compas_fab.robots.Wrench``: a Wrench class representing force in free space, separated into its linear (force) and angular (torque) parts.
+* Added ``compas_fab.robots.Inertia``: a Inertia class representing spatial distribution of mass in a rigid body
 
 **Changed**
 

src/compas_fab/backends/ros/messages/geometry_msgs.py

@@ -1,15 +1,17 @@
 from __future__ import absolute_import
 
-from .std_msgs import ROSmsg
-from .std_msgs import Header
-from .std_msgs import Time
-
 from compas.geometry import Frame
 
+import compas_fab.robots
+
+from .std_msgs import Header
+from .std_msgs import ROSmsg
+
 
 class Point(ROSmsg):
-    """http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/Point.html
+    """http://docs.ros.org/api/geometry_msgs/html/msg/Point.html
     """
+
     def __init__(self, x, y, z):
         self.x = x
         self.y = y
@@ -20,10 +22,12 @@ def from_msg(cls, msg):
         x, y, z = msg['x'], msg['y'], msg['z']
         return cls(x, y, z)
 
+
 class Quaternion(ROSmsg):
-    """http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/Quaternion.html
+    """http://docs.ros.org/api/geometry_msgs/html/msg/Quaternion.html
     """
-    def __init__(self, x=0. ,y=0., z=0., w=1.):
+
+    def __init__(self, x=0., y=0., z=0., w=1.):
         self.x = x
         self.y = y
         self.z = z
@@ -34,12 +38,14 @@ def from_frame(cls, frame):
         qw, qx, qy, qz = frame.quaternion
         return cls(qx, qy, qz, qw)
 
+
 class Pose(ROSmsg):
-    """http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/Pose.html
+    """http://docs.ros.org/api/geometry_msgs/html/msg/Pose.html
     """
+
     def __init__(self, position=None, orientation=None):
-        self.position = position if position else Point(0,0,0)
-        self.orientation = orientation if orientation else Quaternion(0,0,0,1)
+        self.position = position if position else Point(0, 0, 0)
+        self.orientation = orientation if orientation else Quaternion(0, 0, 0, 1)
 
     @classmethod
     def from_frame(cls, frame):
@@ -59,37 +65,155 @@ def from_msg(cls, msg):
         orientation = Quaternion.from_msg(msg['orientation'])
         return cls(position, orientation)
 
+
 class PoseStamped(ROSmsg):
-    """http://docs.ros.org/melodic/api/geometry_msgs/html/msg/PoseStamped.html
+    """http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html
     """
-    def __init__(self, header=Header(), pose=Pose()):
-        self.header = header
-        self.pose = pose
+
+    def __init__(self, header=None, pose=None):
+        self.header = header or Header()
+        self.pose = pose or Pose()
 
     @classmethod
     def from_msg(cls, msg):
         header = Header.from_msg(msg['header'])
         pose = Pose.from_msg(msg['pose'])
         return cls(header, pose)
 
+
 class Vector3(ROSmsg):
     """http://docs.ros.org/api/geometry_msgs/html/msg/Vector3.html
     """
+
     def __init__(self, x=0., y=0., z=0.):
         self.x = x
         self.y = y
         self.z = z
 
+    @classmethod
+    def from_msg(cls, msg):
+        x, y, z = msg['x'], msg['y'], msg['z']
+        return cls(x, y, z)
+
+
 class Transform(ROSmsg):
     """http://docs.ros.org/api/geometry_msgs/html/msg/Transform.html
     """
-    def __init__(self, translation=Vector3(), rotation=Quaternion()):
-        self.translation = translation
-        self.rotation = rotation
+
+    def __init__(self, translation=None, rotation=None):
+        self.translation = translation or Vector3()
+        self.rotation = rotation or Quaternion()
+
 
 class Twist(ROSmsg):
     """http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html
     """
-    def __init__(self, linear=Vector3(), angular=Vector3()):
-        self.linear = linear
-        self.angular = angular
+
+    def __init__(self, linear=None, angular=None):
+        self.linear = linear or Vector3()
+        self.angular = angular or Vector3()
+
+
+class Wrench(ROSmsg):
+    """http://docs.ros.org/api/geometry_msgs/html/msg/Wrench.html
+
+    This represents force in free space, separated into its linear and angular parts.
+
+    Examples
+    --------
+    >>> wrench = compas_fab.robots.Wrench([0, 0, -98], [0, 0, 0])
+    >>> ros_wrench = Wrench.from_wrench(wrench)
+    >>> ros_wrench.msg
+    {'force': {'x': 0.0, 'y': 0.0, 'z': -98.0}, 'torque': {'x': 0.0, 'y': 0.0, 'z': 0.0}}
+    >>> ros_wrench.wrench
+    Wrench(Vector(0.000, 0.000, -98.000), Vector(0.000, 0.000, 0.000))
+    """
+
+    def __init__(self, force=None, torque=None):
+        self.force = force or Vector3()
+        self.torque = torque or Vector3()
+
+    @classmethod
+    def from_msg(cls, msg):
+        force = Vector3.from_msg(msg['force'])
+        torque = Vector3.from_msg(msg['torque'])
+        return cls(force, torque)
+
+    @classmethod
+    def from_wrench(cls, wrench):
+        force = wrench.force
+        torque = wrench.torque
+        return cls(Vector3(*list(force)), Vector3(*list(torque)))
+
+    @property
+    def wrench(self):
+        force = [self.force.x, self.force.y, self.force.z]
+        torque = [self.torque.x, self.torque.y, self.torque.z]
+        return compas_fab.robots.Wrench(force, torque)
+
+
+class WrenchStamped(ROSmsg):
+    """http://docs.ros.org/api/geometry_msgs/html/msg/WrenchStamped.html
+
+    A wrench with reference coordinate frame and timestamp.
+    """
+
+    def __init__(self, header=None, wrench=None):
+        self.header = header or Header()
+        self.wrench = wrench or Wrench()
+
+    @classmethod
+    def from_msg(cls, msg):
+        header = Header.from_msg(msg['header'])
+        wrench = Wrench.from_msg(msg['wrench'])
+        return cls(header, wrench)
+
+
+class Inertia(ROSmsg):
+    """http://docs.ros.org/api/geometry_msgs/html/msg/Inertia.html
+
+    Examples
+    --------
+    >>> inertia = compas_fab.robots.Inertia([[0] * 3] * 3, 1., [0.1, 3.1, 4.4])
+    >>> ros_inertia = Inertia.from_inertia(inertia)
+    >>> ros_inertia.msg
+    {'m': 1.0, 'com': {'x': 0.1, 'y': 3.1, 'z': 4.4}, 'ixx': 0.0, 'ixy': 0.0, 'ixz': 0.0, 'iyy': 0.0, 'iyz': 0.0, 'izz': 0.0}
+    >>> ros_inertia.inertia
+    Inertia([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], 1.0, Point(0.100, 3.100, 4.400))
+    """
+
+    def __init__(self, m=0.0, com=None, ixx=0., ixy=0., ixz=0., iyy=0., iyz=0., izz=0.):
+        self.m = float(m)  # Mass [kg]
+        self.com = com or Vector3()  # Center of mass [m]
+        self.ixx = float(ixx)
+        self.ixy = float(ixy)
+        self.ixz = float(ixz)
+        self.iyy = float(iyy)
+        self.iyz = float(iyz)
+        self.izz = float(izz)
+
+    @classmethod
+    def from_msg(cls, msg):
+        com = Vector3.from_msg(msg['com'])
+        return cls(msg['m'], com, msg['ixx'], msg['ixy'], msg['ixz'], msg['iyy'], msg['iyz'], msg['izz'])
+
+    @classmethod
+    def from_inertia(cls, inertia):
+        m = inertia.mass
+        com = Vector3(*list(inertia.center_of_mass))
+        ixx, ixy, ixz = inertia.inertia_tensor[0]
+        ixy, iyy, iyz = inertia.inertia_tensor[1]
+        izz = inertia.inertia_tensor[2][2]
+        return cls(m, com, ixx, ixy, ixz, iyy, iyz, izz)
+
+    @property
+    def inertia(self):
+        inertia_tensor = [[self.ixx, self.ixy, self.ixz],
+                          [self.ixy, self.iyy, self.iyz],
+                          [self.ixz, self.iyz, self.izz]]
+        return compas_fab.robots.Inertia(inertia_tensor, self.m, [self.com.x, self.com.y, self.com.z])
+
+
+if __name__ == "__main__":
+    import doctest
+    doctest.testmod()

src/compas_fab/robots/__init__.py

@@ -19,6 +19,8 @@
     RobotSemantics
     Configuration
     Duration
+    Wrench
+    Inertia
 
 Path planning
 -------------
@@ -78,5 +80,7 @@
 from .semantics import *
 from .time_ import *
 from .trajectory import *
+from .wrench import *
+from .inertia import *
 
 __all__ = [name for name in dir() if not name.startswith('_')]

src/compas_fab/robots/conftest.py

@@ -5,6 +5,7 @@
 from compas.datastructures import Mesh
 from compas.geometry import Frame
 from compas.geometry import Scale
+from compas.geometry import Rotation
 
 import compas_fab
 from compas_fab.backends import RosClient
@@ -19,6 +20,7 @@ def add_imports(doctest_namespace):
     doctest_namespace["Frame"] = Frame
     doctest_namespace["Scale"] = Scale
     doctest_namespace["compas_fab"] = compas_fab
+    doctest_namespace["Rotation"] = Rotation
 
 
 @pytest.fixture(scope="session", autouse=True)

src/compas_fab/robots/inertia.py

@@ -0,0 +1,62 @@
+from compas.geometry import Point
+
+
+class Inertia():
+    """The moments of inertia represent the spatial distribution of mass in a rigid body.
+
+    It depends on the mass, size, and shape of a rigid body with units of
+    [mass * m**2]. The moments of inertia can be expressed as the components of
+    a symmetric positive-definite 3x3 matrix, with 3 diagonal elements, and 3
+    unique off-diagonal elements. Each inertia matrix is defined relative to a
+    coordinate frame or set of axes.
+
+    Attributes
+    ----------
+    inertia_tensor : list of float
+        A symmetric positive-definite 3x3 matrix:
+        | ixx ixy ixz |
+        | ixy iyy iyz |
+        | ixz iyz izz |
+        with [ixx, iyy, izz] as the principal moments of inertia and
+        [ixy, ixz, iyz] as the products of inertia.
+    mass: float
+        The mass of the object in kg.
+    center_of_mass : :class:`Point`
+        The center of mass of the object in meters.
+
+    Examples
+    --------
+    >>> inertia = Inertia([[0] * 3] * 3, 1., Point(0.1, 3.1, 4.4))
+    >>> inertia
+    Inertia([[0, 0, 0], [0, 0, 0], [0, 0, 0]], 1.0, Point(0.100, 3.100, 4.400))
+    >>> inertia.principal_moments
+    [0, 0, 0]
+
+    Notes
+    -----
+    Assuming uniform mass density, inertial data can be obtained using the
+    free software MeshLab, refering to this great tutorial:
+    http://gazebosim.org/tutorials?tut=inertia
+    """
+
+    def __init__(self, inertia_tensor, mass, center_of_mass):
+        self.inertia_tensor = inertia_tensor
+        self.mass = mass
+        self.center_of_mass = Point(*center_of_mass)
+
+    @property
+    def principal_moments(self):
+        """Returns the diagonal elements of the inertia tensor [ixx, iyy, izz]
+        """
+        I = self.inertia_tensor
+        return [I[0][0], I[1][1], I[2][2]]
+
+    def __repr__(self):
+        return "Inertia({0}, {1}, {2})".format(self.inertia_tensor, self.mass, self.center_of_mass)
+
+    @staticmethod
+    def calculate_inertia_tensor(cls, mesh):
+        """Returns the inertia tensor.
+        """
+        raise NotImplementedError
+