Merge pull request #761 from compas-dev/robot_tutorial_update

Robot tutorial update

Author: beverlylytle

CHANGELOG.md

@@ -16,6 +16,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 * Fixed bug in parameter list of function `mesh_bounding_box` bound as method `Mesh.bounding_box`.
 * Changed exception type when subdivide scheme argument is incorrect on `mesh_subdivide`.
 * The `compas_rhino.artist.RobotModelArtist` functions `draw_visual` and `draw_collision` now return list of newly created Rhino object guids.
+* Added ability of `RobotModel.add_link` to accept primitives in addition to meshes.
+* Fixed bug regarding the computation of `Joint.current_origin`.
+* Fixed bug regarding a repeated call to `RobotModel.add_joint`.
 
 ### Removed
 

docs/tutorial/files/boxy_1.png

@@ -0,0 +1,54 @@
+from compas.robots import Joint
+from compas.robots import RobotModel
+from compas.geometry import Box
+from compas.geometry import Circle
+from compas.geometry import Cylinder
+from compas.geometry import Frame
+from compas.geometry import Plane
+from compas.geometry import Sphere
+from compas.geometry import Vector
+from compas_rhino.artists import RobotModelArtist
+
+model = RobotModel('drinking_bird')
+
+foot_1 = Box(Frame([2, .5, .25], [1, 0, 0], [0, 1, 0]), 1, 2, .5)
+foot_2 = Box(Frame([-2, .5, .25], [1, 0, 0], [0, 1, 0]), 1, 2, .5)
+leg_1 = Box(Frame([2, 0, 4], [1, 0, 0], [0, 1, 0]), .5, 1, 7)
+leg_2 = Box(Frame([-2, 0, 4], [1, 0, 0], [0, 1, 0]), .5, 1, 7)
+axis = Cylinder(Circle(Plane([0, 0, 7], [1, 0, 0]), .01), 4)
+legs_link = model.add_link('legs', visual_meshes=[foot_1, foot_2, leg_1, leg_2, axis])
+
+torso = Cylinder(Circle(Plane([0, 0, 0], [0, 0, 1]), .5), 8)
+torso_link = model.add_link('torso', visual_meshes=[torso])
+
+legs_joint_origin = Frame([0, 0, 7], [1, 0, 0], [0, 1, 0])
+joint_axis = Vector(1, 0, 0)
+model.add_joint(
+    'torso_base_joint',
+    Joint.CONTINUOUS,
+    legs_link,
+    torso_link,
+    legs_joint_origin,
+    joint_axis
+)
+
+head = Sphere([0, 0, 0], 1)
+beak = Cylinder(Circle(Plane([0, 1, -.3], [0, 1, 0]), .3), 1.5)
+head_link = model.add_link('head', visual_meshes=[head, beak])
+neck_joint_origin = Frame([0, 0, 4], [1, 0, 0], [0, 1, 0])
+model.add_joint('neck_joint', Joint.FIXED, torso_link, head_link, origin=neck_joint_origin)
+
+tail = Sphere([0, 0, 0], 1)
+tail_link = model.add_link('tail', visual_meshes=[tail])
+tail_joint_origin = Frame([0, 0, -4], [1, 0, 0], [0, 1, 0])
+model.add_joint('tail_joint', Joint.FIXED, torso_link, tail_link, origin=tail_joint_origin)
+
+hat = Cylinder(Circle(Plane([0, 0, 0], [0, 0, 1]), .8), 1.5)
+brim = Cylinder(Circle(Plane([0, 0, -1.5/2], [0, 0, 1]), 1.4), .1)
+hat_link = model.add_link('hat', visual_meshes=[hat, brim])
+hat_joint_origin = Frame([0, 0, 1 - .3 + 1.5/2], [1, 0, 0], [0, 1, 0])
+model.add_joint('hat_joint', Joint.FIXED, head_link, hat_link, origin=hat_joint_origin)
+
+artist = RobotModelArtist(model, layer='COMPAS::Example Robot')
+artist.clear_layer()
+artist.draw_visual()

docs/tutorial/files/boxy_2.png

@@ -141,6 +141,36 @@ of joints:
 * **Fixed**: Not really a joint because it cannot move, all degrees of freedom
   are locked.
 
+
+Visualizing Robots
+==================
+
+Before jumping into how to build a robot model, let's first see how to visualize
+one. This can be done with Blender, Rhino or Grasshopper using one of COMPAS's
+artists.  The basic procedure is the same in
+any of the CAD software (aside from the import statement), so for simplicity we
+will demonstrate the use of :class:`compas_rhino.artists.RobotModelArtist` in Rhino.
+Be sure to first install COMPAS for Rhino.  While the following code is incomplete,
+it can be used as a scaffolding for code to be run in a Python script editor within Rhino.
+
+.. code-block:: python
+
+    import compas
+    from compas.robots import RobotModel
+    from compas_rhino.artists import RobotModelArtist
+
+    model = RobotModel('Robby')
+
+    # Add some geometry to Robby here
+
+    artist = RobotModelArtist(model, layer='COMPAS::Example Robot')
+    artist.clear_layer()
+    artist.draw_visual()
+
+
+See below for a complete example.
+
+
 Building robots models
 ======================
 
@@ -175,35 +205,170 @@ shows how to construct one programmatically:
 
 This approach can end up being very verbose, so the methods ``add_link``
 and ``add_joint`` of :class:`compas.robots.RobotModel` offer an alternative that
-significantly reduces the amount of code required:
+significantly reduces the amount of code required.  Starting with an empty
+robot model, adding a link in the shape of a box is as easy as:
 
 ::
 
     >>> from compas.geometry import Box, Frame
-    >>> from compas.datastructures import Mesh
-    >>> from compas.robots import Joint, Link, RobotModel
+    >>> from compas.robots import RobotModel
 
 ::
 
-    >>> length, width, axis = 5, 0.4, (0, 0, 1)
-    >>> box = Box.from_diagonal([(0, width / -2, width / -2), (length, width / 2, width / 2)])
-    >>> robot = RobotModel('bender')
-    >>> link_last = robot.add_link('world')
-    >>> robot.name
-    'bender'
-
-    >>> for i in range(6):
-    ...     visual_mesh = Mesh.from_shape(box)
-    ...     origin = Frame.from_quaternion((1, 0, 0, 0), point=(length, 0, 0))
-    ...     link = robot.add_link('link_{}'.format(i), visual_mesh)
-    ...     robot.add_joint('joint_{}'.format(i), Joint.CONTINUOUS, link_last, link, origin, axis)
-    ...     link_last = link
-    ...
-    >>> robot.get_configurable_joint_names()
-    ['joint_0', 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5']
+    >>> model = RobotModel(name='Boxy')
+    >>> box = Box(Frame.worldXY(), 1, 2, .5)
+    >>> model.add_link(name='box_link', visual_meshes=[box])
+
+This code snippet can be modified and run in a Rhino python editor
+to visualize Boxy.  Throughout the rest of this tutorial, the code
+snippets will include the lines for visualization in Rhino, but be
+aware that the class :class:`compas.robots.RobotModel` can be used,
+and is useful, outside of a CAD environment.
+
+.. code-block:: python
+
+    from compas.geometry import Box, Frame
+    from compas.robots import RobotModel
+    from compas_rhino.artists import RobotModelArtist
+
+    model = RobotModel(name='Boxy')
+    box = Box(Frame.worldXY(), 1, 2, .5)
+    model.add_link(name='box_link', visual_meshes=[box])
+
+    artist = RobotModelArtist(model, layer='COMPAS::Example Robot')
+    artist.clear_layer()
+    artist.draw_visual()
+
+
+.. figure:: files/boxy_1.png
+    :figclass: figure
+    :class: figure-img img-fluid
+
+As can be seen, this has added a box of dimensions ``1`` x ``2`` x ``.5``
+whose geometric center and orientation coincides with the world XY frame.
+The ``visual_meshes`` argument can be given a list containing COMPAS
+primitives such as :class:`compas.geometry.Box` or the more complex
+COMPAS meshes :class:`compas.geometry.Mesh`. For simplicity,
+this tutorial uses only primitives.
+
+To reposition the box relative to the link's
+origin, simply change the frame of the provided box. To move the box so that it sits
+above the XY plane, the origin must be shifted in the z-direction by half the height
+of the box.  The box is also shifted slightly forward in the y-direction:
+
+.. code-block:: python
+
+    from compas.geometry import Box, Frame
+    from compas.robots import RobotModel
+    from compas_rhino.artists import RobotModelArtist
+
+    model = RobotModel(name='Boxy')
+    box = Box(Frame([0, .5, .25], [1, 0, 0], [0, 1, 0]), 1, 2, .5)
+    model.add_link(name='box_link', visual_meshes=[box])
+
+    artist = RobotModelArtist(model, layer='COMPAS::Example Robot')
+    artist.clear_layer()
+    artist.draw_visual()
 
-However, more often than not, robot models are loaded from URDF files instead of being
-defined programmatically. To load a URDF into a robot model, use the
+.. figure:: files/boxy_2.png
+    :figclass: figure
+    :class: figure-img img-fluid
+
+A link may have more than one geometric element associated to it.  Now
+there is a stack of two boxes:
+
+.. code-block:: python
+
+    from compas.geometry import Box, Frame
+    from compas.robots import RobotModel
+    from compas_rhino.artists import RobotModelArtist
+
+    model = RobotModel(name='Boxy')
+    box_1 = Box(Frame([0, .5, .25], [1, 0, 0], [0, 1, 0]), 1, 2, .5)
+    box_2 = Box(Frame([0, 0, 4], [1, 0, 0], [0, 1, 0]), .5, 1, 7)
+    model.add_link(name='box_link', visual_meshes=[box_1, box_2])
+
+    artist = RobotModelArtist(model, layer='COMPAS::Example Robot')
+    artist.clear_layer()
+    artist.draw_visual()
+
+.. figure:: files/boxy_3.png
+    :figclass: figure
+    :class: figure-img img-fluid
+
+Remember that the frame of the box is the geometric center of the box relative
+to the link's origin (which, in this case, happens to be the world XY frame).
+So to stack the boxes, the center of ``box_2`` must be placed at a height of
+``<height of box_1> + 1/2 * <height of box_2> = .5 + .5 * 7 = 4``.
+
+One link does not an interesting robot make.  The following code snippet adds
+a cylindrical second link as well as a joint connecting the two.
+
+.. code-block:: python
+
+    from compas.geometry import Box, Circle, Cylinder, Frame, Plane, Vector
+    from compas.robots import Joint, RobotModel
+    from compas_rhino.artists import RobotModelArtist
+
+    model = RobotModel(name='Jointy')
+    box_1 = Box(Frame([2, .5, .25], [1, 0, 0], [0, 1, 0]), 1, 2, .5)
+    box_2 = Box(Frame([2, 0, 4], [1, 0, 0], [0, 1, 0]), .5, 1, 7)
+    box_link = model.add_link(name='box_link', visual_meshes=[box_1, box_2])
+
+    cylinder = Cylinder(Circle(Plane([0, 0, 0], [0, 0, 1]), .5), 8)
+    cylinder_link = model.add_link(name='cylinder_link', visual_meshes=[cylinder])
+
+    origin = Frame([0, 0, 7], [1, 0, 0], [0, 1, 0])
+    axis = Vector(1, 0, 0)
+    model.add_joint(
+         name='box_cylinder_joint',
+         type=Joint.CONTINUOUS,
+         parent_link=box_link,
+         child_link=cylinder_link,
+         origin=origin,
+         axis=axis,
+    )
+
+    artist = RobotModelArtist(model, layer='COMPAS::Example Robot')
+    artist.clear_layer()
+    artist.draw_visual()
+
+
+.. figure:: files/jointy.png
+    :figclass: figure
+    :class: figure-img img-fluid
+
+There's a lot going on in this snippet, so let's break it down.  First, the link
+containing the stacked boxes is added, but the geometry has been shifted to the
+side ``2`` units in the x-direction.  Second, a cylindrical link is added.
+The cylinder has height ``8`` and radius ``.5`` and is vertically oriented.  Finally, a
+joint is added.  This is the most involved operation so far, so let's explore
+this in detail.
+
+Several parameters must be defined for the joint to be sensible.
+The required parameters for a minimally defined joint are the ``name``, ``type``,
+``parent_link`` and ``child_link``.  The location and orientation of the joint is defined
+by the ``origin``, which specifies a frame relative to the origin of the ``parent_link``.
+In this case, the location of the joint is located ``7`` units above (ie,
+in the z-direction) the origin of the ``box_link``, making it ``.5`` units below the top
+of the second box.  The origin of the joint also specifies the origin of its
+``child_link``.  Here, this means that the center of the cylinder will coincide with
+the location of the joint.  Since the geometries of the boxes have been shifted,
+the boxes and the cylinder do not overlap.  If the joint origin is not specified,
+it will default to coincide with the origin of the ``parent_link``.  Since a continuous
+joint was added, it makes sense that the axis of rotation would also need to be given.
+Here, the joint will rotate about the joint origin's x-axis.
+
+Adding a bit more geometry and a few links with fixed joints and we arrive at a rough
+model of the classic drinking bird toy (full code
+:download:`here <files/drinking_bird.py>`).
+
+.. figure:: files/drinking_bird.png
+    :figclass: figure
+    :class: figure-img img-fluid
+
+While it can be useful to programmatically create a robot, more often than not, robot
+models are loaded from URDF files. To load a URDF into a robot model, use the
 ``from_urdf_file`` method::
 
     >>> model = RobotModel.from_urdf_file('ur5.urdf')
@@ -249,33 +414,6 @@ The following snippet shows how to load the robot model currently active in ROS:
     ...    print(robot.model)
 
 
-Visualizing Robots
-==================
-
-Once a robot has been built or loaded, it can be visualized in Blender, Rhino or
-Grasshopper using one of COMPAS's artists.  The basic procedure is the same in
-any of the CAD software (aside from the import statement), so for simplicity we
-will demonstrate the use of :class:`compas_rhino.artists.RobotModelArtist` in Rhino.  Once
-COMPAS has been installed for Rhino,
-the following can be run in a Python script editor within Rhino.
-
-.. code-block:: python
-
-    import compas
-    from compas.robots import GithubPackageMeshLoader
-    from compas.robots import RobotModel
-    from compas_rhino.artists import RobotModelArtist
-
-    compas.PRECISION = '12f'
-    github = GithubPackageMeshLoader('ros-industrial/abb', 'abb_irb6600_support', 'kinetic-devel')
-    model = RobotModel.from_urdf_file(github.load_urdf('irb6640.urdf'))
-    model.load_geometry(github)
-
-    artist = RobotModelArtist(model, layer='COMPAS FAB::Example')
-    artist.clear_layer()
-    artist.draw_visual()
-
-
 FK, IK & Path Planning
 ======================
 

docs/tutorial/files/boxy_3.png

@@ -107,7 +107,7 @@ def radius(self, radius):
 
     @property
     def length(self):
-        self.line.length
+        return self.line.length
 
     @property
     def volume(self):

docs/tutorial/files/drinking_bird.png

@@ -235,6 +235,10 @@ def from_data(cls, data):
         box.data = data
         return box
 
+    @classmethod
+    def from_geometry(cls, geometry):
+        return cls(' '.join([str(geometry.xsize), str(geometry.ysize), str(geometry.zsize)]))
+
 
 class Cylinder(BaseShape):
     """3D shape primitive representing a cylinder.
@@ -295,6 +299,10 @@ def from_data(cls, data):
         cyl.data = data
         return cyl
 
+    @classmethod
+    def from_geometry(cls, geometry):
+        return cls(geometry.radius, geometry.height)
+
 
 class Sphere(BaseShape):
     """3D shape primitive representing a sphere.
@@ -346,6 +354,10 @@ def from_data(cls, data):
         sph.data = data
         return sph
 
+    @classmethod
+    def from_geometry(cls, geometry):
+        return cls(geometry.radius)
+
 
 class Capsule(BaseShape):
     """3D shape primitive representing a capsule.
@@ -397,6 +409,10 @@ def from_data(cls, data):
         cap.data = data
         return cap
 
+    @classmethod
+    def from_geometry(cls, geometry):
+        return cls(geometry.radius, geometry.length)
+
 
 class MeshDescriptor(BaseShape):
     """Description of a mesh.