Add files using upload-large-folder tool

external/peract_bimanual/agents/act_bc_lang/detr/LICENSE

@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright 2020 - present, Facebook, Inc
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

external/peract_bimanual/agents/act_bc_lang/detr/models/__init__.py

@@ -0,0 +1,9 @@
+# Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved
+from .detr_vae import build as build_vae
+from .detr_vae import build_cnnmlp as build_cnnmlp
+
+def build_ACT_model(args):
+    return build_vae(args)
+
+def build_CNNMLP_model(args):
+    return build_cnnmlp(args)

external/peract_bimanual/agents/act_bc_lang/detr/models/backbone.py

@@ -0,0 +1,120 @@
+# Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved
+"""
+Backbone modules.
+"""
+from collections import OrderedDict
+
+import torch
+import torch.nn.functional as F
+import torchvision
+from torch import nn
+from torchvision.models._utils import IntermediateLayerGetter
+from typing import Dict, List
+
+from agents.act_bc_lang.detr.util.misc import NestedTensor, is_main_process
+
+from .position_encoding import build_position_encoding
+
+
+class FrozenBatchNorm2d(torch.nn.Module):
+    """
+    BatchNorm2d where the batch statistics and the affine parameters are fixed.
+
+    Copy-paste from torchvision.misc.ops with added eps before rqsrt,
+    without which any other policy_models than torchvision.policy_models.resnet[18,34,50,101]
+    produce nans.
+    """
+
+    def __init__(self, n):
+        super(FrozenBatchNorm2d, self).__init__()
+        self.register_buffer("weight", torch.ones(n))
+        self.register_buffer("bias", torch.zeros(n))
+        self.register_buffer("running_mean", torch.zeros(n))
+        self.register_buffer("running_var", torch.ones(n))
+
+    def _load_from_state_dict(self, state_dict, prefix, local_metadata, strict,
+                              missing_keys, unexpected_keys, error_msgs):
+        num_batches_tracked_key = prefix + 'num_batches_tracked'
+        if num_batches_tracked_key in state_dict:
+            del state_dict[num_batches_tracked_key]
+
+        super(FrozenBatchNorm2d, self)._load_from_state_dict(
+            state_dict, prefix, local_metadata, strict,
+            missing_keys, unexpected_keys, error_msgs)
+
+    def forward(self, x):
+        # move reshapes to the beginning
+        # to make it fuser-friendly
+        w = self.weight.reshape(1, -1, 1, 1)
+        b = self.bias.reshape(1, -1, 1, 1)
+        rv = self.running_var.reshape(1, -1, 1, 1)
+        rm = self.running_mean.reshape(1, -1, 1, 1)
+        eps = 1e-5
+        scale = w * (rv + eps).rsqrt()
+        bias = b - rm * scale
+        return x * scale + bias
+
+
+class BackboneBase(nn.Module):
+
+    def __init__(self, backbone: nn.Module, train_backbone: bool, num_channels: int, return_interm_layers: bool):
+        super().__init__()
+        # for name, parameter in backbone.named_parameters(): # only train later layers # TODO do we want this?
+        #     if not train_backbone or 'layer2' not in name and 'layer3' not in name and 'layer4' not in name:
+        #         parameter.requires_grad_(False)
+        if return_interm_layers:
+            return_layers = {"layer1": "0", "layer2": "1", "layer3": "2", "layer4": "3"}
+        else:
+            return_layers = {'layer4': "0"}
+        self.body = IntermediateLayerGetter(backbone, return_layers=return_layers)
+        self.num_channels = num_channels
+
+    def forward(self, tensor):
+        xs = self.body(tensor)
+        return xs
+        # out: Dict[str, NestedTensor] = {}
+        # for name, x in xs.items():
+        #     m = tensor_list.mask
+        #     assert m is not None
+        #     mask = F.interpolate(m[None].float(), size=x.shape[-2:]).to(torch.bool)[0]
+        #     out[name] = NestedTensor(x, mask)
+        # return out
+
+
+class Backbone(BackboneBase):
+    """ResNet backbone with frozen BatchNorm."""
+    def __init__(self, name: str,
+                 train_backbone: bool,
+                 return_interm_layers: bool,
+                 dilation: bool):
+        backbone = getattr(torchvision.models, name)(
+            replace_stride_with_dilation=[False, False, dilation],
+            pretrained=is_main_process(), norm_layer=FrozenBatchNorm2d) # pretrained # TODO do we want frozen batch_norm??
+        num_channels = 512 if name in ('resnet18', 'resnet34') else 2048
+        super().__init__(backbone, train_backbone, num_channels, return_interm_layers)
+
+
+class Joiner(nn.Sequential):
+    def __init__(self, backbone, position_embedding):
+        super().__init__(backbone, position_embedding)
+
+    def forward(self, tensor_list: NestedTensor):
+        xs = self[0](tensor_list)
+        out: List[NestedTensor] = []
+        pos = []
+        for name, x in xs.items():
+            out.append(x)
+            # position encoding
+            pos.append(self[1](x).to(x.dtype))
+
+        return out, pos
+
+
+def build_backbone(args):
+    position_embedding = build_position_encoding(args)
+    train_backbone = args.lr_backbone > 0
+    return_interm_layers = args.masks
+    backbone = Backbone(args.backbone, train_backbone, return_interm_layers, args.dilation)
+    model = Joiner(backbone, position_embedding)
+    model.num_channels = backbone.num_channels
+    return model

external/peract_bimanual/agents/act_bc_lang/detr/models/detr_vae.py

@@ -0,0 +1,276 @@
+# Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved
+"""
+DETR model and criterion classes.
+"""
+import torch
+from torch import nn
+from torch.autograd import Variable
+from .backbone import build_backbone
+from .transformer import build_transformer, TransformerEncoder, TransformerEncoderLayer
+
+import numpy as np
+
+
+def reparametrize(mu, logvar):
+    std = logvar.div(2).exp()
+    eps = Variable(std.data.new(std.size()).normal_())
+    return mu + std * eps
+
+
+def get_sinusoid_encoding_table(n_position, d_hid):
+    def get_position_angle_vec(position):
+        return [position / np.power(10000, 2 * (hid_j // 2) / d_hid) for hid_j in range(d_hid)]
+
+    sinusoid_table = np.array([get_position_angle_vec(pos_i) for pos_i in range(n_position)])
+    sinusoid_table[:, 0::2] = np.sin(sinusoid_table[:, 0::2])  # dim 2i
+    sinusoid_table[:, 1::2] = np.cos(sinusoid_table[:, 1::2])  # dim 2i+1
+
+    return torch.FloatTensor(sinusoid_table).unsqueeze(0)
+
+
+class DETRVAE(nn.Module):
+    """ This is the DETR module that performs object detection """
+    def __init__(self, backbones, transformer, encoder, state_dim,
+                 num_queries, camera_names, input_dim):
+        """ Initializes the model.
+        Parameters:
+            backbones: torch module of the backbone to be used. See backbone.py
+            transformer: torch module of the transformer architecture. See transformer.py
+            state_dim: robot state dimension of the environment
+            num_queries: number of object queries, ie detection slot. This is the maximal number of objects
+                         DETR can detect in a single image. For COCO, we recommend 100 queries.
+            aux_loss: True if auxiliary decoding losses (loss at each decoder layer) are to be used.
+        """
+        super().__init__()
+        self.num_queries = num_queries
+        self.camera_names = camera_names
+        self.transformer = transformer
+        self.encoder = encoder
+        hidden_dim = transformer.d_model
+        self.input_dim = input_dim
+        self.action_head = nn.Linear(hidden_dim, state_dim)
+        self.is_pad_head = nn.Linear(hidden_dim, 1)
+        self.query_embed = nn.Embedding(num_queries, hidden_dim)
+        if backbones is not None:
+            self.input_proj = nn.Conv2d(backbones[0].num_channels, hidden_dim, kernel_size=1)
+            self.backbones = nn.ModuleList(backbones)
+            self.input_proj_robot_state = nn.Linear(self.input_dim, hidden_dim)
+        else:
+            # input_dim + 7 # robot_state + env_state
+            self.input_proj_robot_state = nn.Linear(self.input_dim, hidden_dim)
+            self.input_proj_env_state = nn.Linear(self.input_dim, hidden_dim)
+            self.pos = torch.nn.Embedding(2, hidden_dim)
+            self.backbones = None
+
+        # encoder extra parameters
+        self.latent_dim = 32 # final size of latent z # TODO tune
+        self.cls_embed = nn.Embedding(1, hidden_dim) # extra cls token embedding
+        self.encoder_proj = nn.Linear(self.input_dim, hidden_dim) # project state to embedding
+        self.latent_proj = nn.Linear(hidden_dim, self.latent_dim*2) # project hidden state to latent std, var
+        self.register_buffer('pos_table', get_sinusoid_encoding_table(num_queries+1, hidden_dim))
+
+        # decoder extra parameters
+        self.latent_out_proj = nn.Linear(self.latent_dim, hidden_dim) # project latent sample to embedding
+        self.additional_pos_embed = nn.Embedding(2, hidden_dim) # learned position embedding for proprio and latent
+
+    def forward(self, qpos, image, env_state, actions=None, is_pad=None):
+        """
+        qpos: batch, qpos_dim
+        image: batch, num_cam, channel, height, width
+        env_state: None
+        actions: batch, seq, action_dim
+        """
+        is_training = actions is not None # train or val
+        bs, _ = qpos.shape
+        ### Obtain latent z from action sequence
+        if is_training:
+            # project action sequence to embedding dim, and concat with a CLS token
+            action_embed = self.encoder_proj(actions) # (bs, seq, hidden_dim)
+            cls_embed = self.cls_embed.weight # (1, hidden_dim)
+            cls_embed = torch.unsqueeze(cls_embed, axis=0).repeat(bs, 1, 1) # (bs, 1, hidden_dim)
+            encoder_input = torch.cat([cls_embed, action_embed], axis=1) # (bs, seq+1, hidden_dim)
+            encoder_input = encoder_input.permute(1, 0, 2) # (seq+1, bs, hidden_dim)
+            # do not mask cls token
+            cls_is_pad = torch.full((bs, 1), False).to(qpos.device) # False: not a padding
+            is_pad = torch.cat([cls_is_pad, is_pad], axis=1)  # (bs, seq+1)
+            # obtain position embedding
+            pos_embed = self.pos_table.clone().detach()
+            pos_embed = pos_embed.permute(1, 0, 2)  # (seq+1, 1, hidden_dim)
+            # query model
+            encoder_output = self.encoder(encoder_input, pos=pos_embed, src_key_padding_mask=is_pad)
+            encoder_output = encoder_output[0] # take cls output only
+            latent_info = self.latent_proj(encoder_output)
+            mu = latent_info[:, :self.latent_dim]
+            logvar = latent_info[:, self.latent_dim:]
+            latent_sample = reparametrize(mu, logvar)
+            latent_input = self.latent_out_proj(latent_sample)
+        else:
+            mu = logvar = None
+            latent_sample = torch.zeros([bs, self.latent_dim], dtype=torch.float32).to(qpos.device)
+            latent_input = self.latent_out_proj(latent_sample)
+
+        if self.backbones is not None:
+            # Image observation features and position embeddings
+            all_cam_features = []
+            all_cam_pos = []
+            for cam_id, cam_name in enumerate(self.camera_names):
+                features, pos = self.backbones[cam_id](image[:, cam_id])
+                features = features[0] # take the last layer feature
+                pos = pos[0]
+                all_cam_features.append(self.input_proj(features))
+                all_cam_pos.append(pos)
+            # proprioception features
+            proprio_input = self.input_proj_robot_state(qpos)
+            # fold camera dimension into width dimension
+            src = torch.cat(all_cam_features, axis=3)
+            pos = torch.cat(all_cam_pos, axis=3)
+            hs = self.transformer(src, None, self.query_embed.weight, pos, latent_input, proprio_input, self.additional_pos_embed.weight)[0]
+        else:
+            qpos = self.input_proj_robot_state(qpos)
+            env_state = self.input_proj_env_state(env_state)
+            transformer_input = torch.cat([qpos, env_state], axis=1) # seq length = 2
+            hs = self.transformer(transformer_input, None, self.query_embed.weight, self.pos.weight)[0]
+        a_hat = self.action_head(hs)
+        is_pad_hat = self.is_pad_head(hs)
+        return a_hat, is_pad_hat, [mu, logvar]
+
+
+
+class CNNMLP(nn.Module):
+    def __init__(self, backbones, state_dim, camera_names):
+        """ Initializes the model.
+        Parameters:
+            backbones: torch module of the backbone to be used. See backbone.py
+            transformer: torch module of the transformer architecture. See transformer.py
+            state_dim: robot state dimension of the environment
+            num_queries: number of object queries, ie detection slot. This is the maximal number of objects
+                         DETR can detect in a single image. For COCO, we recommend 100 queries.
+            aux_loss: True if auxiliary decoding losses (loss at each decoder layer) are to be used.
+        """
+        super().__init__()
+        self.camera_names = camera_names
+        self.action_head = nn.Linear(1000, state_dim) # TODO add more
+        if backbones is not None:
+            self.backbones = nn.ModuleList(backbones)
+            backbone_down_projs = []
+            for backbone in backbones:
+                down_proj = nn.Sequential(
+                    nn.Conv2d(backbone.num_channels, 128, kernel_size=5),
+                    nn.Conv2d(128, 64, kernel_size=5),
+                    nn.Conv2d(64, 32, kernel_size=5)
+                )
+                backbone_down_projs.append(down_proj)
+            self.backbone_down_projs = nn.ModuleList(backbone_down_projs)
+
+            mlp_in_dim = 768 * len(backbones) + 14
+            self.mlp = mlp(input_dim=mlp_in_dim, hidden_dim=1024, output_dim=14, hidden_depth=2)
+        else:
+            raise NotImplementedError
+
+    def forward(self, qpos, image, env_state, actions=None):
+        """
+        qpos: batch, qpos_dim
+        image: batch, num_cam, channel, height, width
+        env_state: None
+        actions: batch, seq, action_dim
+        """
+        is_training = actions is not None # train or val
+        bs, _ = qpos.shape
+        # Image observation features and position embeddings
+        all_cam_features = []
+        for cam_id, cam_name in enumerate(self.camera_names):
+            features, pos = self.backbones[cam_id](image[:, cam_id])
+            features = features[0] # take the last layer feature
+            pos = pos[0] # not used
+            all_cam_features.append(self.backbone_down_projs[cam_id](features))
+        # flatten everything
+        flattened_features = []
+        for cam_feature in all_cam_features:
+            flattened_features.append(cam_feature.reshape([bs, -1]))
+        flattened_features = torch.cat(flattened_features, axis=1) # 768 each
+        features = torch.cat([flattened_features, qpos], axis=1) # qpos: 14
+        a_hat = self.mlp(features)
+        return a_hat
+
+
+def mlp(input_dim, hidden_dim, output_dim, hidden_depth):
+    if hidden_depth == 0:
+        mods = [nn.Linear(input_dim, output_dim)]
+    else:
+        mods = [nn.Linear(input_dim, hidden_dim), nn.ReLU(inplace=True)]
+        for i in range(hidden_depth - 1):
+            mods += [nn.Linear(hidden_dim, hidden_dim), nn.ReLU(inplace=True)]
+        mods.append(nn.Linear(hidden_dim, output_dim))
+    trunk = nn.Sequential(*mods)
+    return trunk
+
+
+def build_encoder(args):
+    d_model = args.hidden_dim # 256
+    dropout = args.dropout # 0.1
+    nhead = args.nheads # 8
+    dim_feedforward = args.dim_feedforward # 2048
+    num_encoder_layers = args.enc_layers # 4 # TODO shared with VAE decoder
+    normalize_before = args.pre_norm # False
+    activation = "relu"
+
+    encoder_layer = TransformerEncoderLayer(d_model, nhead, dim_feedforward,
+                                            dropout, activation, normalize_before)
+    encoder_norm = nn.LayerNorm(d_model) if normalize_before else None
+    encoder = TransformerEncoder(encoder_layer, num_encoder_layers, encoder_norm)
+
+    return encoder
+
+
+def build(args):
+    state_dim = args.input_dim
+
+    # From state
+    # backbone = None # from state for now, no need for conv nets
+    # From image
+    backbones = []
+    for _ in args.camera_names:
+        backbone = build_backbone(args)
+        backbones.append(backbone)
+
+    transformer = build_transformer(args)
+
+    encoder = build_encoder(args)
+
+    model = DETRVAE(
+        backbones,
+        transformer,
+        encoder,
+        state_dim=state_dim,
+        num_queries=args.num_queries,
+        camera_names=args.camera_names,
+        input_dim=args.input_dim,
+    )
+
+    n_parameters = sum(p.numel() for p in model.parameters() if p.requires_grad)
+    print("number of parameters: %.2fM" % (n_parameters/1e6,))
+
+    return model
+
+def build_cnnmlp(args):
+    state_dim = args.input_dim
+
+    # From state
+    # backbone = None # from state for now, no need for conv nets
+    # From image
+    backbones = []
+    for _ in args.camera_names:
+        backbone = build_backbone(args)
+        backbones.append(backbone)
+
+    model = CNNMLP(
+        backbones,
+        state_dim=state_dim,
+        camera_names=args.camera_names,
+    )
+
+    n_parameters = sum(p.numel() for p in model.parameters() if p.requires_grad)
+    print("number of parameters: %.2fM" % (n_parameters/1e6,))
+
+    return model
+

external/peract_bimanual/agents/act_bc_lang/detr/models/position_encoding.py

@@ -0,0 +1,91 @@
+# Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved
+"""
+Various positional encodings for the transformer.
+"""
+import math
+import torch
+from torch import nn
+
+from agents.act_bc_lang.detr.util.misc import NestedTensor
+
+
+class PositionEmbeddingSine(nn.Module):
+    """
+    This is a more standard version of the position embedding, very similar to the one
+    used by the Attention is all you need paper, generalized to work on images.
+    """
+    def __init__(self, num_pos_feats=64, temperature=10000, normalize=False, scale=None):
+        super().__init__()
+        self.num_pos_feats = num_pos_feats
+        self.temperature = temperature
+        self.normalize = normalize
+        if scale is not None and normalize is False:
+            raise ValueError("normalize should be True if scale is passed")
+        if scale is None:
+            scale = 2 * math.pi
+        self.scale = scale
+
+    def forward(self, tensor):
+        x = tensor
+        # mask = tensor_list.mask
+        # assert mask is not None
+        # not_mask = ~mask
+
+        not_mask = torch.ones_like(x[0, [0]])
+        y_embed = not_mask.cumsum(1, dtype=torch.float32)
+        x_embed = not_mask.cumsum(2, dtype=torch.float32)
+        if self.normalize:
+            eps = 1e-6
+            y_embed = y_embed / (y_embed[:, -1:, :] + eps) * self.scale
+            x_embed = x_embed / (x_embed[:, :, -1:] + eps) * self.scale
+
+        dim_t = torch.arange(self.num_pos_feats, dtype=torch.float32, device=x.device)
+        dim_t = self.temperature ** (2 * (dim_t // 2) / self.num_pos_feats)
+
+        pos_x = x_embed[:, :, :, None] / dim_t
+        pos_y = y_embed[:, :, :, None] / dim_t
+        pos_x = torch.stack((pos_x[:, :, :, 0::2].sin(), pos_x[:, :, :, 1::2].cos()), dim=4).flatten(3)
+        pos_y = torch.stack((pos_y[:, :, :, 0::2].sin(), pos_y[:, :, :, 1::2].cos()), dim=4).flatten(3)
+        pos = torch.cat((pos_y, pos_x), dim=3).permute(0, 3, 1, 2)
+        return pos
+
+
+class PositionEmbeddingLearned(nn.Module):
+    """
+    Absolute pos embedding, learned.
+    """
+    def __init__(self, num_pos_feats=256):
+        super().__init__()
+        self.row_embed = nn.Embedding(50, num_pos_feats)
+        self.col_embed = nn.Embedding(50, num_pos_feats)
+        self.reset_parameters()
+
+    def reset_parameters(self):
+        nn.init.uniform_(self.row_embed.weight)
+        nn.init.uniform_(self.col_embed.weight)
+
+    def forward(self, tensor_list: NestedTensor):
+        x = tensor_list.tensors
+        h, w = x.shape[-2:]
+        i = torch.arange(w, device=x.device)
+        j = torch.arange(h, device=x.device)
+        x_emb = self.col_embed(i)
+        y_emb = self.row_embed(j)
+        pos = torch.cat([
+            x_emb.unsqueeze(0).repeat(h, 1, 1),
+            y_emb.unsqueeze(1).repeat(1, w, 1),
+        ], dim=-1).permute(2, 0, 1).unsqueeze(0).repeat(x.shape[0], 1, 1, 1)
+        return pos
+
+
+def build_position_encoding(args):
+    N_steps = args.hidden_dim // 2
+    if args.position_embedding in ('v2', 'sine'):
+        # TODO find a better way of exposing other arguments
+        position_embedding = PositionEmbeddingSine(N_steps, normalize=True)
+    elif args.position_embedding in ('v3', 'learned'):
+        position_embedding = PositionEmbeddingLearned(N_steps)
+    else:
+        raise ValueError(f"not supported {args.position_embedding}")
+
+    return position_embedding

external/peract_bimanual/agents/act_bc_lang/detr/models/transformer.py

@@ -0,0 +1,312 @@
+# Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved
+"""
+DETR Transformer class.
+
+Copy-paste from torch.nn.Transformer with modifications:
+    * positional encodings are passed in MHattention
+    * extra LN at the end of encoder is removed
+    * decoder returns a stack of activations from all decoding layers
+"""
+import copy
+from typing import Optional, List
+
+import torch
+import torch.nn.functional as F
+from torch import nn, Tensor
+
+
+class Transformer(nn.Module):
+
+    def __init__(self, d_model=512, nhead=8, num_encoder_layers=6,
+                 num_decoder_layers=6, dim_feedforward=2048, dropout=0.1,
+                 activation="relu", normalize_before=False,
+                 return_intermediate_dec=False):
+        super().__init__()
+
+        encoder_layer = TransformerEncoderLayer(d_model, nhead, dim_feedforward,
+                                                dropout, activation, normalize_before)
+        encoder_norm = nn.LayerNorm(d_model) if normalize_before else None
+        self.encoder = TransformerEncoder(encoder_layer, num_encoder_layers, encoder_norm)
+
+        decoder_layer = TransformerDecoderLayer(d_model, nhead, dim_feedforward,
+                                                dropout, activation, normalize_before)
+        decoder_norm = nn.LayerNorm(d_model)
+        self.decoder = TransformerDecoder(decoder_layer, num_decoder_layers, decoder_norm,
+                                          return_intermediate=return_intermediate_dec)
+
+        self._reset_parameters()
+
+        self.d_model = d_model
+        self.nhead = nhead
+
+    def _reset_parameters(self):
+        for p in self.parameters():
+            if p.dim() > 1:
+                nn.init.xavier_uniform_(p)
+
+    def forward(self, src, mask, query_embed, pos_embed, latent_input=None, proprio_input=None, additional_pos_embed=None):
+        # TODO flatten only when input has H and W
+        if len(src.shape) == 4: # has H and W
+            # flatten NxCxHxW to HWxNxC
+            bs, c, h, w = src.shape
+            src = src.flatten(2).permute(2, 0, 1)
+            pos_embed = pos_embed.flatten(2).permute(2, 0, 1).repeat(1, bs, 1)
+            query_embed = query_embed.unsqueeze(1).repeat(1, bs, 1)
+            # mask = mask.flatten(1)
+
+            additional_pos_embed = additional_pos_embed.unsqueeze(1).repeat(1, bs, 1) # seq, bs, dim
+            pos_embed = torch.cat([additional_pos_embed, pos_embed], axis=0)
+
+            addition_input = torch.stack([latent_input, proprio_input], axis=0)
+            src = torch.cat([addition_input, src], axis=0)
+        else:
+            assert len(src.shape) == 3
+            # flatten NxHWxC to HWxNxC
+            bs, hw, c = src.shape
+            src = src.permute(1, 0, 2)
+            pos_embed = pos_embed.unsqueeze(1).repeat(1, bs, 1)
+            query_embed = query_embed.unsqueeze(1).repeat(1, bs, 1)
+
+        tgt = torch.zeros_like(query_embed)
+        memory = self.encoder(src, src_key_padding_mask=mask, pos=pos_embed)
+        hs = self.decoder(tgt, memory, memory_key_padding_mask=mask,
+                          pos=pos_embed, query_pos=query_embed)
+        hs = hs.transpose(1, 2)
+        return hs
+
+class TransformerEncoder(nn.Module):
+
+    def __init__(self, encoder_layer, num_layers, norm=None):
+        super().__init__()
+        self.layers = _get_clones(encoder_layer, num_layers)
+        self.num_layers = num_layers
+        self.norm = norm
+
+    def forward(self, src,
+                mask: Optional[Tensor] = None,
+                src_key_padding_mask: Optional[Tensor] = None,
+                pos: Optional[Tensor] = None):
+        output = src
+
+        for layer in self.layers:
+            output = layer(output, src_mask=mask,
+                           src_key_padding_mask=src_key_padding_mask, pos=pos)
+
+        if self.norm is not None:
+            output = self.norm(output)
+
+        return output
+
+
+class TransformerDecoder(nn.Module):
+
+    def __init__(self, decoder_layer, num_layers, norm=None, return_intermediate=False):
+        super().__init__()
+        self.layers = _get_clones(decoder_layer, num_layers)
+        self.num_layers = num_layers
+        self.norm = norm
+        self.return_intermediate = return_intermediate
+
+    def forward(self, tgt, memory,
+                tgt_mask: Optional[Tensor] = None,
+                memory_mask: Optional[Tensor] = None,
+                tgt_key_padding_mask: Optional[Tensor] = None,
+                memory_key_padding_mask: Optional[Tensor] = None,
+                pos: Optional[Tensor] = None,
+                query_pos: Optional[Tensor] = None):
+        output = tgt
+
+        intermediate = []
+
+        for layer in self.layers:
+            output = layer(output, memory, tgt_mask=tgt_mask,
+                           memory_mask=memory_mask,
+                           tgt_key_padding_mask=tgt_key_padding_mask,
+                           memory_key_padding_mask=memory_key_padding_mask,
+                           pos=pos, query_pos=query_pos)
+            if self.return_intermediate:
+                intermediate.append(self.norm(output))
+
+        if self.norm is not None:
+            output = self.norm(output)
+            if self.return_intermediate:
+                intermediate.pop()
+                intermediate.append(output)
+
+        if self.return_intermediate:
+            return torch.stack(intermediate)
+
+        return output.unsqueeze(0)
+
+
+class TransformerEncoderLayer(nn.Module):
+
+    def __init__(self, d_model, nhead, dim_feedforward=2048, dropout=0.1,
+                 activation="relu", normalize_before=False):
+        super().__init__()
+        self.self_attn = nn.MultiheadAttention(d_model, nhead, dropout=dropout)
+        # Implementation of Feedforward model
+        self.linear1 = nn.Linear(d_model, dim_feedforward)
+        self.dropout = nn.Dropout(dropout)
+        self.linear2 = nn.Linear(dim_feedforward, d_model)
+
+        self.norm1 = nn.LayerNorm(d_model)
+        self.norm2 = nn.LayerNorm(d_model)
+        self.dropout1 = nn.Dropout(dropout)
+        self.dropout2 = nn.Dropout(dropout)
+
+        self.activation = _get_activation_fn(activation)
+        self.normalize_before = normalize_before
+
+    def with_pos_embed(self, tensor, pos: Optional[Tensor]):
+        return tensor if pos is None else tensor + pos
+
+    def forward_post(self,
+                     src,
+                     src_mask: Optional[Tensor] = None,
+                     src_key_padding_mask: Optional[Tensor] = None,
+                     pos: Optional[Tensor] = None):
+        q = k = self.with_pos_embed(src, pos)
+        src2 = self.self_attn(q, k, value=src, attn_mask=src_mask,
+                              key_padding_mask=src_key_padding_mask)[0]
+        src = src + self.dropout1(src2)
+        src = self.norm1(src)
+        src2 = self.linear2(self.dropout(self.activation(self.linear1(src))))
+        src = src + self.dropout2(src2)
+        src = self.norm2(src)
+        return src
+
+    def forward_pre(self, src,
+                    src_mask: Optional[Tensor] = None,
+                    src_key_padding_mask: Optional[Tensor] = None,
+                    pos: Optional[Tensor] = None):
+        src2 = self.norm1(src)
+        q = k = self.with_pos_embed(src2, pos)
+        src2 = self.self_attn(q, k, value=src2, attn_mask=src_mask,
+                              key_padding_mask=src_key_padding_mask)[0]
+        src = src + self.dropout1(src2)
+        src2 = self.norm2(src)
+        src2 = self.linear2(self.dropout(self.activation(self.linear1(src2))))
+        src = src + self.dropout2(src2)
+        return src
+
+    def forward(self, src,
+                src_mask: Optional[Tensor] = None,
+                src_key_padding_mask: Optional[Tensor] = None,
+                pos: Optional[Tensor] = None):
+        if self.normalize_before:
+            return self.forward_pre(src, src_mask, src_key_padding_mask, pos)
+        return self.forward_post(src, src_mask, src_key_padding_mask, pos)
+
+
+class TransformerDecoderLayer(nn.Module):
+
+    def __init__(self, d_model, nhead, dim_feedforward=2048, dropout=0.1,
+                 activation="relu", normalize_before=False):
+        super().__init__()
+        self.self_attn = nn.MultiheadAttention(d_model, nhead, dropout=dropout)
+        self.multihead_attn = nn.MultiheadAttention(d_model, nhead, dropout=dropout)
+        # Implementation of Feedforward model
+        self.linear1 = nn.Linear(d_model, dim_feedforward)
+        self.dropout = nn.Dropout(dropout)
+        self.linear2 = nn.Linear(dim_feedforward, d_model)
+
+        self.norm1 = nn.LayerNorm(d_model)
+        self.norm2 = nn.LayerNorm(d_model)
+        self.norm3 = nn.LayerNorm(d_model)
+        self.dropout1 = nn.Dropout(dropout)
+        self.dropout2 = nn.Dropout(dropout)
+        self.dropout3 = nn.Dropout(dropout)
+
+        self.activation = _get_activation_fn(activation)
+        self.normalize_before = normalize_before
+
+    def with_pos_embed(self, tensor, pos: Optional[Tensor]):
+        return tensor if pos is None else tensor + pos
+
+    def forward_post(self, tgt, memory,
+                     tgt_mask: Optional[Tensor] = None,
+                     memory_mask: Optional[Tensor] = None,
+                     tgt_key_padding_mask: Optional[Tensor] = None,
+                     memory_key_padding_mask: Optional[Tensor] = None,
+                     pos: Optional[Tensor] = None,
+                     query_pos: Optional[Tensor] = None):
+        q = k = self.with_pos_embed(tgt, query_pos)
+        tgt2 = self.self_attn(q, k, value=tgt, attn_mask=tgt_mask,
+                              key_padding_mask=tgt_key_padding_mask)[0]
+        tgt = tgt + self.dropout1(tgt2)
+        tgt = self.norm1(tgt)
+        tgt2 = self.multihead_attn(query=self.with_pos_embed(tgt, query_pos),
+                                   key=self.with_pos_embed(memory, pos),
+                                   value=memory, attn_mask=memory_mask,
+                                   key_padding_mask=memory_key_padding_mask)[0]
+        tgt = tgt + self.dropout2(tgt2)
+        tgt = self.norm2(tgt)
+        tgt2 = self.linear2(self.dropout(self.activation(self.linear1(tgt))))
+        tgt = tgt + self.dropout3(tgt2)
+        tgt = self.norm3(tgt)
+        return tgt
+
+    def forward_pre(self, tgt, memory,
+                    tgt_mask: Optional[Tensor] = None,
+                    memory_mask: Optional[Tensor] = None,
+                    tgt_key_padding_mask: Optional[Tensor] = None,
+                    memory_key_padding_mask: Optional[Tensor] = None,
+                    pos: Optional[Tensor] = None,
+                    query_pos: Optional[Tensor] = None):
+        tgt2 = self.norm1(tgt)
+        q = k = self.with_pos_embed(tgt2, query_pos)
+        tgt2 = self.self_attn(q, k, value=tgt2, attn_mask=tgt_mask,
+                              key_padding_mask=tgt_key_padding_mask)[0]
+        tgt = tgt + self.dropout1(tgt2)
+        tgt2 = self.norm2(tgt)
+        tgt2 = self.multihead_attn(query=self.with_pos_embed(tgt2, query_pos),
+                                   key=self.with_pos_embed(memory, pos),
+                                   value=memory, attn_mask=memory_mask,
+                                   key_padding_mask=memory_key_padding_mask)[0]
+        tgt = tgt + self.dropout2(tgt2)
+        tgt2 = self.norm3(tgt)
+        tgt2 = self.linear2(self.dropout(self.activation(self.linear1(tgt2))))
+        tgt = tgt + self.dropout3(tgt2)
+        return tgt
+
+    def forward(self, tgt, memory,
+                tgt_mask: Optional[Tensor] = None,
+                memory_mask: Optional[Tensor] = None,
+                tgt_key_padding_mask: Optional[Tensor] = None,
+                memory_key_padding_mask: Optional[Tensor] = None,
+                pos: Optional[Tensor] = None,
+                query_pos: Optional[Tensor] = None):
+        if self.normalize_before:
+            return self.forward_pre(tgt, memory, tgt_mask, memory_mask,
+                                    tgt_key_padding_mask, memory_key_padding_mask, pos, query_pos)
+        return self.forward_post(tgt, memory, tgt_mask, memory_mask,
+                                 tgt_key_padding_mask, memory_key_padding_mask, pos, query_pos)
+
+
+def _get_clones(module, N):
+    return nn.ModuleList([copy.deepcopy(module) for i in range(N)])
+
+
+def build_transformer(args):
+    return Transformer(
+        d_model=args.hidden_dim,
+        dropout=args.dropout,
+        nhead=args.nheads,
+        dim_feedforward=args.dim_feedforward,
+        num_encoder_layers=args.enc_layers,
+        num_decoder_layers=args.dec_layers,
+        normalize_before=args.pre_norm,
+        return_intermediate_dec=True,
+    )
+
+
+def _get_activation_fn(activation):
+    """Return an activation function given a string"""
+    if activation == "relu":
+        return F.relu
+    if activation == "gelu":
+        return F.gelu
+    if activation == "glu":
+        return F.glu
+    raise RuntimeError(F"activation should be relu/gelu, not {activation}.")

external/peract_bimanual/agents/act_bc_lang/detr/util/misc.py

@@ -0,0 +1,468 @@
+# Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved
+"""
+Misc functions, including distributed helpers.
+
+Mostly copy-paste from torchvision references.
+"""
+import os
+import subprocess
+import time
+from collections import defaultdict, deque
+import datetime
+import pickle
+from packaging import version
+from typing import Optional, List
+
+import torch
+import torch.distributed as dist
+from torch import Tensor
+
+# needed due to empty tensor bug in pytorch and torchvision 0.5
+import torchvision
+if version.parse(torchvision.__version__) < version.parse('0.7'):
+    from torchvision.ops import _new_empty_tensor
+    from torchvision.ops.misc import _output_size
+
+
+class SmoothedValue(object):
+    """Track a series of values and provide access to smoothed values over a
+    window or the global series average.
+    """
+
+    def __init__(self, window_size=20, fmt=None):
+        if fmt is None:
+            fmt = "{median:.4f} ({global_avg:.4f})"
+        self.deque = deque(maxlen=window_size)
+        self.total = 0.0
+        self.count = 0
+        self.fmt = fmt
+
+    def update(self, value, n=1):
+        self.deque.append(value)
+        self.count += n
+        self.total += value * n
+
+    def synchronize_between_processes(self):
+        """
+        Warning: does not synchronize the deque!
+        """
+        if not is_dist_avail_and_initialized():
+            return
+        t = torch.tensor([self.count, self.total], dtype=torch.float64, device='cuda')
+        dist.barrier()
+        dist.all_reduce(t)
+        t = t.tolist()
+        self.count = int(t[0])
+        self.total = t[1]
+
+    @property
+    def median(self):
+        d = torch.tensor(list(self.deque))
+        return d.median().item()
+
+    @property
+    def avg(self):
+        d = torch.tensor(list(self.deque), dtype=torch.float32)
+        return d.mean().item()
+
+    @property
+    def global_avg(self):
+        return self.total / self.count
+
+    @property
+    def max(self):
+        return max(self.deque)
+
+    @property
+    def value(self):
+        return self.deque[-1]
+
+    def __str__(self):
+        return self.fmt.format(
+            median=self.median,
+            avg=self.avg,
+            global_avg=self.global_avg,
+            max=self.max,
+            value=self.value)
+
+
+def all_gather(data):
+    """
+    Run all_gather on arbitrary picklable data (not necessarily tensors)
+    Args:
+        data: any picklable object
+    Returns:
+        list[data]: list of data gathered from each rank
+    """
+    world_size = get_world_size()
+    if world_size == 1:
+        return [data]
+
+    # serialized to a Tensor
+    buffer = pickle.dumps(data)
+    storage = torch.ByteStorage.from_buffer(buffer)
+    tensor = torch.ByteTensor(storage).to("cuda")
+
+    # obtain Tensor size of each rank
+    local_size = torch.tensor([tensor.numel()], device="cuda")
+    size_list = [torch.tensor([0], device="cuda") for _ in range(world_size)]
+    dist.all_gather(size_list, local_size)
+    size_list = [int(size.item()) for size in size_list]
+    max_size = max(size_list)
+
+    # receiving Tensor from all ranks
+    # we pad the tensor because torch all_gather does not support
+    # gathering tensors of different shapes
+    tensor_list = []
+    for _ in size_list:
+        tensor_list.append(torch.empty((max_size,), dtype=torch.uint8, device="cuda"))
+    if local_size != max_size:
+        padding = torch.empty(size=(max_size - local_size,), dtype=torch.uint8, device="cuda")
+        tensor = torch.cat((tensor, padding), dim=0)
+    dist.all_gather(tensor_list, tensor)
+
+    data_list = []
+    for size, tensor in zip(size_list, tensor_list):
+        buffer = tensor.cpu().numpy().tobytes()[:size]
+        data_list.append(pickle.loads(buffer))
+
+    return data_list
+
+
+def reduce_dict(input_dict, average=True):
+    """
+    Args:
+        input_dict (dict): all the values will be reduced
+        average (bool): whether to do average or sum
+    Reduce the values in the dictionary from all processes so that all processes
+    have the averaged results. Returns a dict with the same fields as
+    input_dict, after reduction.
+    """
+    world_size = get_world_size()
+    if world_size < 2:
+        return input_dict
+    with torch.no_grad():
+        names = []
+        values = []
+        # sort the keys so that they are consistent across processes
+        for k in sorted(input_dict.keys()):
+            names.append(k)
+            values.append(input_dict[k])
+        values = torch.stack(values, dim=0)
+        dist.all_reduce(values)
+        if average:
+            values /= world_size
+        reduced_dict = {k: v for k, v in zip(names, values)}
+    return reduced_dict
+
+
+class MetricLogger(object):
+    def __init__(self, delimiter="\t"):
+        self.meters = defaultdict(SmoothedValue)
+        self.delimiter = delimiter
+
+    def update(self, **kwargs):
+        for k, v in kwargs.items():
+            if isinstance(v, torch.Tensor):
+                v = v.item()
+            assert isinstance(v, (float, int))
+            self.meters[k].update(v)
+
+    def __getattr__(self, attr):
+        if attr in self.meters:
+            return self.meters[attr]
+        if attr in self.__dict__:
+            return self.__dict__[attr]
+        raise AttributeError("'{}' object has no attribute '{}'".format(
+            type(self).__name__, attr))
+
+    def __str__(self):
+        loss_str = []
+        for name, meter in self.meters.items():
+            loss_str.append(
+                "{}: {}".format(name, str(meter))
+            )
+        return self.delimiter.join(loss_str)
+
+    def synchronize_between_processes(self):
+        for meter in self.meters.values():
+            meter.synchronize_between_processes()
+
+    def add_meter(self, name, meter):
+        self.meters[name] = meter
+
+    def log_every(self, iterable, print_freq, header=None):
+        i = 0
+        if not header:
+            header = ''
+        start_time = time.time()
+        end = time.time()
+        iter_time = SmoothedValue(fmt='{avg:.4f}')
+        data_time = SmoothedValue(fmt='{avg:.4f}')
+        space_fmt = ':' + str(len(str(len(iterable)))) + 'd'
+        if torch.cuda.is_available():
+            log_msg = self.delimiter.join([
+                header,
+                '[{0' + space_fmt + '}/{1}]',
+                'eta: {eta}',
+                '{meters}',
+                'time: {time}',
+                'data: {data}',
+                'max mem: {memory:.0f}'
+            ])
+        else:
+            log_msg = self.delimiter.join([
+                header,
+                '[{0' + space_fmt + '}/{1}]',
+                'eta: {eta}',
+                '{meters}',
+                'time: {time}',
+                'data: {data}'
+            ])
+        MB = 1024.0 * 1024.0
+        for obj in iterable:
+            data_time.update(time.time() - end)
+            yield obj
+            iter_time.update(time.time() - end)
+            if i % print_freq == 0 or i == len(iterable) - 1:
+                eta_seconds = iter_time.global_avg * (len(iterable) - i)
+                eta_string = str(datetime.timedelta(seconds=int(eta_seconds)))
+                if torch.cuda.is_available():
+                    print(log_msg.format(
+                        i, len(iterable), eta=eta_string,
+                        meters=str(self),
+                        time=str(iter_time), data=str(data_time),
+                        memory=torch.cuda.max_memory_allocated() / MB))
+                else:
+                    print(log_msg.format(
+                        i, len(iterable), eta=eta_string,
+                        meters=str(self),
+                        time=str(iter_time), data=str(data_time)))
+            i += 1
+            end = time.time()
+        total_time = time.time() - start_time
+        total_time_str = str(datetime.timedelta(seconds=int(total_time)))
+        print('{} Total time: {} ({:.4f} s / it)'.format(
+            header, total_time_str, total_time / len(iterable)))
+
+
+def get_sha():
+    cwd = os.path.dirname(os.path.abspath(__file__))
+
+    def _run(command):
+        return subprocess.check_output(command, cwd=cwd).decode('ascii').strip()
+    sha = 'N/A'
+    diff = "clean"
+    branch = 'N/A'
+    try:
+        sha = _run(['git', 'rev-parse', 'HEAD'])
+        subprocess.check_output(['git', 'diff'], cwd=cwd)
+        diff = _run(['git', 'diff-index', 'HEAD'])
+        diff = "has uncommited changes" if diff else "clean"
+        branch = _run(['git', 'rev-parse', '--abbrev-ref', 'HEAD'])
+    except Exception:
+        pass
+    message = f"sha: {sha}, status: {diff}, branch: {branch}"
+    return message
+
+
+def collate_fn(batch):
+    batch = list(zip(*batch))
+    batch[0] = nested_tensor_from_tensor_list(batch[0])
+    return tuple(batch)
+
+
+def _max_by_axis(the_list):
+    # type: (List[List[int]]) -> List[int]
+    maxes = the_list[0]
+    for sublist in the_list[1:]:
+        for index, item in enumerate(sublist):
+            maxes[index] = max(maxes[index], item)
+    return maxes
+
+
+class NestedTensor(object):
+    def __init__(self, tensors, mask: Optional[Tensor]):
+        self.tensors = tensors
+        self.mask = mask
+
+    def to(self, device):
+        # type: (Device) -> NestedTensor # noqa
+        cast_tensor = self.tensors.to(device)
+        mask = self.mask
+        if mask is not None:
+            assert mask is not None
+            cast_mask = mask.to(device)
+        else:
+            cast_mask = None
+        return NestedTensor(cast_tensor, cast_mask)
+
+    def decompose(self):
+        return self.tensors, self.mask
+
+    def __repr__(self):
+        return str(self.tensors)
+
+
+def nested_tensor_from_tensor_list(tensor_list: List[Tensor]):
+    # TODO make this more general
+    if tensor_list[0].ndim == 3:
+        if torchvision._is_tracing():
+            # nested_tensor_from_tensor_list() does not export well to ONNX
+            # call _onnx_nested_tensor_from_tensor_list() instead
+            return _onnx_nested_tensor_from_tensor_list(tensor_list)
+
+        # TODO make it support different-sized images
+        max_size = _max_by_axis([list(img.shape) for img in tensor_list])
+        # min_size = tuple(min(s) for s in zip(*[img.shape for img in tensor_list]))
+        batch_shape = [len(tensor_list)] + max_size
+        b, c, h, w = batch_shape
+        dtype = tensor_list[0].dtype
+        device = tensor_list[0].device
+        tensor = torch.zeros(batch_shape, dtype=dtype, device=device)
+        mask = torch.ones((b, h, w), dtype=torch.bool, device=device)
+        for img, pad_img, m in zip(tensor_list, tensor, mask):
+            pad_img[: img.shape[0], : img.shape[1], : img.shape[2]].copy_(img)
+            m[: img.shape[1], :img.shape[2]] = False
+    else:
+        raise ValueError('not supported')
+    return NestedTensor(tensor, mask)
+
+
+# _onnx_nested_tensor_from_tensor_list() is an implementation of
+# nested_tensor_from_tensor_list() that is supported by ONNX tracing.
+@torch.jit.unused
+def _onnx_nested_tensor_from_tensor_list(tensor_list: List[Tensor]) -> NestedTensor:
+    max_size = []
+    for i in range(tensor_list[0].dim()):
+        max_size_i = torch.max(torch.stack([img.shape[i] for img in tensor_list]).to(torch.float32)).to(torch.int64)
+        max_size.append(max_size_i)
+    max_size = tuple(max_size)
+
+    # work around for
+    # pad_img[: img.shape[0], : img.shape[1], : img.shape[2]].copy_(img)
+    # m[: img.shape[1], :img.shape[2]] = False
+    # which is not yet supported in onnx
+    padded_imgs = []
+    padded_masks = []
+    for img in tensor_list:
+        padding = [(s1 - s2) for s1, s2 in zip(max_size, tuple(img.shape))]
+        padded_img = torch.nn.functional.pad(img, (0, padding[2], 0, padding[1], 0, padding[0]))
+        padded_imgs.append(padded_img)
+
+        m = torch.zeros_like(img[0], dtype=torch.int, device=img.device)
+        padded_mask = torch.nn.functional.pad(m, (0, padding[2], 0, padding[1]), "constant", 1)
+        padded_masks.append(padded_mask.to(torch.bool))
+
+    tensor = torch.stack(padded_imgs)
+    mask = torch.stack(padded_masks)
+
+    return NestedTensor(tensor, mask=mask)
+
+
+def setup_for_distributed(is_master):
+    """
+    This function disables printing when not in master process
+    """
+    import builtins as __builtin__
+    builtin_print = __builtin__.print
+
+    def print(*args, **kwargs):
+        force = kwargs.pop('force', False)
+        if is_master or force:
+            builtin_print(*args, **kwargs)
+
+    __builtin__.print = print
+
+
+def is_dist_avail_and_initialized():
+    if not dist.is_available():
+        return False
+    if not dist.is_initialized():
+        return False
+    return True
+
+
+def get_world_size():
+    if not is_dist_avail_and_initialized():
+        return 1
+    return dist.get_world_size()
+
+
+def get_rank():
+    if not is_dist_avail_and_initialized():
+        return 0
+    return dist.get_rank()
+
+
+def is_main_process():
+    return get_rank() == 0
+
+
+def save_on_master(*args, **kwargs):
+    if is_main_process():
+        torch.save(*args, **kwargs)
+
+
+def init_distributed_mode(args):
+    if 'RANK' in os.environ and 'WORLD_SIZE' in os.environ:
+        args.rank = int(os.environ["RANK"])
+        args.world_size = int(os.environ['WORLD_SIZE'])
+        args.gpu = int(os.environ['LOCAL_RANK'])
+    elif 'SLURM_PROCID' in os.environ:
+        args.rank = int(os.environ['SLURM_PROCID'])
+        args.gpu = args.rank % torch.cuda.device_count()
+    else:
+        print('Not using distributed mode')
+        args.distributed = False
+        return
+
+    args.distributed = True
+
+    torch.cuda.set_device(args.gpu)
+    args.dist_backend = 'nccl'
+    print('| distributed init (rank {}): {}'.format(
+        args.rank, args.dist_url), flush=True)
+    torch.distributed.init_process_group(backend=args.dist_backend, init_method=args.dist_url,
+                                         world_size=args.world_size, rank=args.rank)
+    torch.distributed.barrier()
+    setup_for_distributed(args.rank == 0)
+
+
+@torch.no_grad()
+def accuracy(output, target, topk=(1,)):
+    """Computes the precision@k for the specified values of k"""
+    if target.numel() == 0:
+        return [torch.zeros([], device=output.device)]
+    maxk = max(topk)
+    batch_size = target.size(0)
+
+    _, pred = output.topk(maxk, 1, True, True)
+    pred = pred.t()
+    correct = pred.eq(target.view(1, -1).expand_as(pred))
+
+    res = []
+    for k in topk:
+        correct_k = correct[:k].view(-1).float().sum(0)
+        res.append(correct_k.mul_(100.0 / batch_size))
+    return res
+
+
+def interpolate(input, size=None, scale_factor=None, mode="nearest", align_corners=None):
+    # type: (Tensor, Optional[List[int]], Optional[float], str, Optional[bool]) -> Tensor
+    """
+    Equivalent to nn.functional.interpolate, but with support for empty batch sizes.
+    This will eventually be supported natively by PyTorch, and this
+    class can go away.
+    """
+    if version.parse(torchvision.__version__) < version.parse('0.7'):
+        if input.numel() > 0:
+            return torch.nn.functional.interpolate(
+                input, size, scale_factor, mode, align_corners
+            )
+
+        output_shape = _output_size(2, input, size, scale_factor)
+        output_shape = list(input.shape[:-2]) + list(output_shape)
+        return _new_empty_tensor(input, output_shape)
+    else:
+        return torchvision.ops.misc.interpolate(input, size, scale_factor, mode, align_corners)

external/peract_bimanual/agents/arm/__init__.py

@@ -0,0 +1 @@
+import agents.arm.launch_utils

external/pyrep/.gitignore

@@ -0,0 +1,102 @@
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+./idea
+.idea/
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+.hypothesis/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# pyenv
+.python-version
+
+# celery beat schedule file
+celerybeat-schedule
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+
+pyrep/backend/_sim_cffi*
+

external/pyrep/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2019 Stephen James
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

external/pyrep/README.md

@@ -0,0 +1,284 @@
+# PyRep [![Build Status](https://github.com/stepjam/PyRep/workflows/Build/badge.svg)](https://github.com/stepjam/PyRep/actions) [![Discord](https://img.shields.io/discord/694945313638842378.svg?label=&logo=discord&logoColor=ffffff&color=7389D8&labelColor=6A7EC2)](https://discord.gg/eTMsa5Y)
+
+
+__PyRep is a toolkit for robot learning research, built on top of [CoppeliaSim](http://www.coppeliarobotics.com/) (previously called V-REP).__
+
+
+- [Install](#install)
+- [Running Headless](#running-headless)
+- [Getting Started](#getting-started)
+- [Usage](#usage)
+- [Supported Robots](#supported-robots)
+- [Adding Robots](#adding-robots)
+- [Contributing](#contributing)
+- [Projects Using PyRep](#projects-using-pyrep)
+- [What Happened to V-REP?](#what-happened-to-v-rep)
+- [Citation](#citation)
+
+
+## Install
+
+PyRep requires version **4.1** of CoppeliaSim. Download: 
+- [Ubuntu 16.04](https://www.coppeliarobotics.com/files/CoppeliaSim_Edu_V4_1_0_Ubuntu16_04.tar.xz)
+- [Ubuntu 18.04](https://www.coppeliarobotics.com/files/CoppeliaSim_Edu_V4_1_0_Ubuntu18_04.tar.xz)
+- [Ubuntu 20.04](https://www.coppeliarobotics.com/files/CoppeliaSim_Edu_V4_1_0_Ubuntu20_04.tar.xz)
+
+Once you have downloaded CoppeliaSim, you can pull PyRep from git:
+
+```bash
+git clone https://github.com/stepjam/PyRep.git
+cd PyRep
+```
+
+Add the following to your *~/.bashrc* file: (__NOTE__: the 'EDIT ME' in the first line)
+
+```bash
+export COPPELIASIM_ROOT=EDIT/ME/PATH/TO/COPPELIASIM/INSTALL/DIR
+export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$COPPELIASIM_ROOT
+export QT_QPA_PLATFORM_PLUGIN_PATH=$COPPELIASIM_ROOT
+```
+
+__Remember to source your bashrc (`source ~/.bashrc`) or 
+zshrc (`source ~/.zshrc`) after this.
+
+Finally install the python library:
+
+```bash
+pip3 install -r requirements.txt
+pip3 install .
+```
+
+You should be good to go!
+Try running one of the examples in the *examples/* folder.
+
+_Although you can use CoppeliaSim on any platform, communication via PyRep is currently only supported on Linux._
+
+#### Troubleshooting
+
+Below are some problems you may encounter during installation. If none of these solve your problem, please raise an issue.
+
+- ModuleNotFoundError: No module named 'pyrep.backend._v_rep_cffi'
+  - If you are getting this error, then please check that you are not running the interpreter from the project root. If you are, then your Python interpreter will try to import those files rather the installed files.
+- error: command 'x86_64-linux-gnu-gcc' failed
+  - You may be missing packages needed for building python extensions. Try: `sudo apt-get install python3-dev`, and then re-run the installation.
+
+## Running Headless
+
+You can run PyRep/CoppeliaSim headlessly with VirtualGL. VirtualGL is an open source toolkit that gives any Unix or Linux remote display software the ability to run OpenGL applications **with full 3D hardware acceleration**.
+First insure that you have the nVidia proprietary driver installed. I.e. you should get an output when running `nvidia-smi`. Now run the following commands:
+```bash
+sudo apt-get install xorg libxcb-randr0-dev libxrender-dev libxkbcommon-dev libxkbcommon-x11-0 libavcodec-dev libavformat-dev libswscale-dev
+sudo nvidia-xconfig -a --use-display-device=None --virtual=1280x1024
+# Install VirtualGL
+wget https://sourceforge.net/projects/virtualgl/files/2.5.2/virtualgl_2.5.2_amd64.deb/download -O virtualgl_2.5.2_amd64.deb
+sudo dpkg -i virtualgl*.deb
+rm virtualgl*.deb
+```
+You will now need to reboot, and then start the X server:
+```bash
+sudo reboot
+nohup sudo X &
+```
+Now we are good to go! To render the application with the first GPU, you can do the following:
+```bash
+export DISPLAY=:0.0
+python my_pyrep_app.py
+```
+To render with the second GPU, you will insetad set display as: `export DISPLAY=:0.1`, and so on.
+
+**Acknowledgement**: Special thanks to Boyuan Chen (UC Berkeley) for bringing VirtualGL to my attention!
+
+## Getting Started
+
+1. First take a look at [Usage](#usage) and the examples in the *examples/* folder to see if PyRep might be able to accelerate your research.
+2. Take a look at the CoppeliaSim [tutorials](http://www.coppeliarobotics.com/helpFiles/en/tutorials.htm).
+
+## Usage
+
+The best way to see how PyRep can help in your research is to look at the examples in the *examples/* folder!
+
+#### Launching the simulation
+
+```python
+from pyrep import PyRep
+
+pr = PyRep()
+# Launch the application with a scene file in headless mode
+pr.launch('scene.ttt', headless=True) 
+pr.start()  # Start the simulation
+
+# Do some stuff
+
+pr.stop()  # Stop the simulation
+pr.shutdown()  # Close the application
+```
+
+
+#### Modifying the Scene
+
+```python
+from pyrep.objects.shape import Shape
+from pyrep.const import PrimitiveShape
+
+object = Shape.create(type=PrimitiveShape.CYLINDER, 
+                      color=[r,g,b], size=[w, h, d],
+                      position=[x, y, z])
+object.set_color([r, g, b])
+object.set_position([x, y, z])
+```
+
+#### Using Robots
+
+Robots are designed to be modular; arms are treated separately to grippers.
+
+Use the robot ttm files defined in robots/ttms. These have been altered slightly from the original ones shipped with CoppeliaSim to allow them to be used with motional planning out of the box. 
+The 'tip' of the robot may not be where you want it, so feel free to play around with this.
+
+```python
+from pyrep import PyRep
+from pyrep.robots.arms.panda import Panda
+from pyrep.robots.end_effectors.panda_gripper import PandaGripper
+
+pr = PyRep()
+# Launch the application with a scene file that contains a robot
+pr.launch('scene_with_panda.ttt') 
+pr.start()  # Start the simulation
+
+arm = Panda()  # Get the panda from the scene
+gripper = PandaGripper()  # Get the panda gripper from the scene
+
+velocities = [.1, .2, .3, .4, .5, .6, .7]
+arm.set_joint_target_velocities(velocities)
+pr.step()  # Step physics simulation
+
+done = False
+# Open the gripper halfway at a velocity of 0.04.
+while not done:
+    done = gripper.actuate(0.5, velocity=0.04)
+    pr.step()
+    
+pr.stop()  # Stop the simulation
+pr.shutdown()  # Close the application
+```
+
+We recommend constructing your robot in a dictionary or a small structure, e.g.
+
+
+```python
+class MyRobot(object):
+  def __init__(self, arm, gripper):
+    self.arm = arm
+    self.gripper = gripper
+
+arm = Panda()  # Get the panda from the scene
+gripper = PandaGripper()  # Get the panda gripper from the scene
+
+# Create robot structure
+my_robot_1 = MyRobot(arm, gripper)
+# OR
+my_robot_2 = {
+  'arm': arm,
+  'gripper': gripper
+}
+```
+
+#### Running Multiple PyRep Instances
+
+Each PyRep instance needs its own process. This can be achieved using Pythons [multiprocessing](https://docs.python.org/3.6/library/multiprocessing.html) module. Here is a simple example:
+
+```python
+from multiprocessing import Process
+
+PROCESSES = 10
+
+def run():
+  pr = PyRep()
+  pr.launch('my_scene.ttt', headless=True)
+  pr.start()
+  # Do stuff...
+  pr.stop()
+  pr.shutdown()
+
+processes = [Process(target=run, args=()) for i in range(PROCESSES)]
+[p.start() for p in processes]
+[p.join() for p in processes]
+```
+
+## Supported Robots
+
+Here is a list of robots currently supported by PyRep:
+
+#### Arms
+
+- Kinova Mico
+- Kinova Jaco
+- Rethink Baxter
+- Rethink Sawyer
+- Franka Emika Panda
+- Kuka LBR iiwa 7 R800
+- Kuka LBR iiwa 14 R820
+- Universal Robots UR3
+- Universal Robots UR5
+- Universal Robots UR10
+
+#### Grippers
+
+- Kinova Mico Hand
+- Kinova Jaco Hand
+- Rethink Baxter Gripper
+- Franka Emika Panda Gripper
+
+#### Mobile Robots
+
+- Kuka YouBot
+- Turtle Bot
+- Line Tracer
+
+Feel free to send pull requests for new robots!
+
+## Adding Robots
+
+If the robot you want is not currently supported, then why not add it in!
+
+[Here is a tutorial for adding robots.](tutorials/adding_robots.md)
+
+## Contributing
+
+We want to make PyRep the best tool for rapid robot learning research. If you would like to get involved, then please [get in contact](https://www.doc.ic.ac.uk/~slj12/)!
+
+Pull requests welcome for bug fixes!
+
+## Projects Using PyRep
+
+If you use PyRep in your work, then get in contact and we can add you to the list!
+
+- [RLBench: The Robot Learning Benchmark & Learning Environment, arxiv 2019](https://arxiv.org/abs/1909.12271)
+- [Learning One-Shot Imitation from Humans without Humans, arxiv 2019](https://arxiv.org/abs/1911.01103)
+- [Task-Embedded Control Networks for Few-Shot Imitation Learning, CoRL 2018](https://arxiv.org/abs/1810.03237)
+- [Transferring End-to-End Visuomotor Control from Simulation to Real World for a Multi-Stage Task
+, CoRL 2017](https://arxiv.org/abs/1707.02267)
+
+## Acknowledgements
+
+- Georges Nomicos (Imperial College London) for the addition of mobile platforms.
+- Boyuan Chen (UC Berkeley) for bringing VirtualGL to my attention.
+
+## What Happened to V-REP?
+
+Coppelia Robotics discontinued development of __V-REP__. Instead, they now focus
+their efforts on __CoppeliaSim__. CoppeliaSim is 100% compatible with V-REP.
+See more information [here](http://coppeliarobotics.com/helpFiles/en/versionInfo.htm#coppeliaSim4.0.0).
+
+PyRep is fully compatible with both V-REP and CoppeliaSim.
+
+
+## Citation
+
+```
+@article{james2019pyrep,
+  title={PyRep: Bringing V-REP to Deep Robot Learning},
+  author={James, Stephen and Freese, Marc and Davison, Andrew J.},
+  journal={arXiv preprint arXiv:1906.11176},
+  year={2019}
+}
+```

external/pyrep/pyrep/__init__.py

@@ -0,0 +1,5 @@
+__version__ = '4.1.0.3'
+
+testing = False
+
+from .pyrep import PyRep

external/pyrep/pyrep/const.py

@@ -0,0 +1,110 @@
+from pyrep.backend import sim
+from enum import Enum
+
+
+class PrimitiveShape(Enum):
+    CUBOID = 0
+    SPHERE = 1
+    CYLINDER = 2
+    CONE = 3
+
+
+class ObjectType(Enum):
+    ALL = sim.sim_handle_all
+    SHAPE = sim.sim_object_shape_type
+    JOINT = sim.sim_object_joint_type
+    DUMMY = sim.sim_object_dummy_type
+    PROXIMITY_SENSOR = sim.sim_object_proximitysensor_type
+    GRAPH = sim.sim_object_graph_type
+    CAMERA = sim.sim_object_camera_type
+    PATH = sim.sim_object_path_type
+    VISION_SENSOR = sim.sim_object_visionsensor_type
+    VOLUME = sim.sim_object_volume_type
+    MILl = sim.sim_object_mill_type
+    FORCE_SENSOR = sim.sim_object_forcesensor_type
+    LIGHT = sim.sim_object_light_type
+    MIRROR = sim.sim_object_mirror_type
+    OCTREE = sim.sim_object_octree_type
+
+
+class JointType(Enum):
+    REVOLUTE = sim.sim_joint_revolute_subtype
+    PRISMATIC = sim.sim_joint_prismatic_subtype
+    SPHERICAL = sim.sim_joint_spherical_subtype
+
+
+class JointMode(Enum):
+    PASSIVE = sim.sim_jointmode_passive
+    IK = sim.sim_jointmode_ik
+    IK_DEPENDENT = sim.sim_jointmode_ikdependent
+    DEPENDENT = sim.sim_jointmode_dependent
+    FORCE = sim.sim_jointmode_force
+
+
+class ConfigurationPathAlgorithms(Enum):
+    BiTRRT = 'BiTRRT'
+    BITstar = 'BITstar'
+    BKPIECE1 = 'BKPIECE1'
+    CForest = 'CForest'
+    EST = 'EST'
+    FMT = 'FMT'
+    KPIECE1 = 'KPIECE1'
+    LazyPRM = 'LazyPRM'
+    LazyPRMstar = 'LazyPRMstar'
+    LazyRRT = 'LazyRRT'
+    LBKPIECE1 = 'LBKPIECE1'
+    LBTRRT = 'LBTRRT'
+    PDST = 'PDST'
+    PRM = 'PRM'
+    PRMstar = 'PRMstar'
+    pRRT = 'pRRT'
+    pSBL = 'pSBL'
+    RRT = 'RRT'
+    RRTConnect = 'RRTConnect'
+    RRTstar = 'RRTstar'
+    SBL = 'SBL'
+    SPARS = 'SPARS'
+    SPARStwo = 'SPARStwo'
+    STRIDE = 'STRIDE'
+    TRRT = 'TRRT'
+
+
+class TextureMappingMode(Enum):
+    PLANE = sim.sim_texturemap_plane
+    CYLINDER = sim.sim_texturemap_cylinder
+    SPHERE = sim.sim_texturemap_sphere
+    CUBE = sim.sim_texturemap_cube
+
+
+class PerspectiveMode(Enum):
+    ORTHOGRAPHIC = 0
+    PERSPECTIVE = 1
+
+
+class RenderMode(Enum):
+    OPENGL = sim.sim_rendermode_opengl
+    OPENGL_AUXILIARY = sim.sim_rendermode_auxchannels
+    OPENGL_COLOR_CODED = sim.sim_rendermode_colorcoded
+    POV_RAY = sim.sim_rendermode_povray
+    EXTERNAL = sim.sim_rendermode_extrenderer
+    EXTERNAL_WINDOWED = sim.sim_rendermode_extrendererwindowed
+    OPENGL3 = sim.sim_rendermode_opengl3
+    OPENGL3_WINDOWED = sim.sim_rendermode_opengl3windowed
+
+
+class Verbosity(Enum):
+    NONE = 'none'
+    ERRORS = 'errors'
+    WARNINGS = 'warnings'
+    LOAD_INFOS = 'loadinfos'
+    SCRIPT_ERRORS = 'scripterrors'
+    SCRIPT_WARNINGS = 'scriptwarnings'
+    SCRIPT_INFOS = 'scriptinfos'
+    INFOS = 'infos'
+    DEBUG = 'debug'
+    TRACE = 'trace'
+    TRACE_LUA = 'tracelua'
+    TYRACE_ALL = 'traceall'
+
+
+PYREP_SCRIPT_TYPE = sim.sim_scripttype_addonscript

external/pyrep/pyrep/errors.py

@@ -0,0 +1,30 @@
+class PyRepError(Exception):
+    pass
+
+
+class WrongObjectTypeError(Exception):
+    pass
+
+
+class ObjectAlreadyRemovedError(Exception):
+    pass
+
+
+class ObjectIsNotModelError(Exception):
+    pass
+
+
+class ConfigurationError(Exception):
+    pass
+
+
+class ConfigurationPathError(Exception):
+    pass
+
+
+class GripperError(Exception):
+    pass
+
+
+class IKError(Exception):
+    pass

external/pyrep/pyrep/pyrep.py

@@ -0,0 +1,332 @@
+import numpy as np
+from contextlib import contextmanager
+from pyrep.backend import sim, utils
+from pyrep.const import Verbosity
+from pyrep.objects.object import Object
+from pyrep.objects.shape import Shape
+from pyrep.textures.texture import Texture
+from pyrep.errors import PyRepError
+from pyrep.backend import sim
+import os
+import sys
+import time
+import threading
+from typing import Tuple, List
+import warnings
+
+
+class PyRep(object):
+    """Used for interfacing with the CoppeliaSim simulation.
+
+    Can be used for starting, stopping, and stepping the simulation. As well
+    as getting, and creating scene objects and robots.
+    """
+
+    def __init__(self):
+        self.running = False
+        self._process = None
+        self._robot_to_count = {}
+        self.connected = False
+
+        self._ui_thread = None
+        self._responsive_ui_thread = None
+
+        self._init_thread_id = None
+        self._shutting_down = False
+
+        self._handles_to_objects = {}
+
+        if 'COPPELIASIM_ROOT' not in os.environ:
+            raise PyRepError(
+                'COPPELIASIM_ROOT not defined. See installation instructions.')
+        self._vrep_root = os.environ['COPPELIASIM_ROOT']
+        if not os.path.exists(self._vrep_root):
+            raise PyRepError(
+                'COPPELIASIM_ROOT was not a correct path. '
+                'See installation instructions')
+
+    def _run_ui_thread(self, scene_file: str, headless: bool,
+                       verbosity: Verbosity) -> None:
+        # Need this otherwise extensions will not be loaded
+        os.chdir(self._vrep_root)
+        options = sim.sim_gui_headless if headless else sim.sim_gui_all
+        sim.simSetStringParameter(
+            sim.sim_stringparam_verbosity, verbosity.value)
+        sim.simExtLaunchUIThread(
+            options=options, scene=scene_file, pyrep_root=self._vrep_root)
+
+    def _run_responsive_ui_thread(self) -> None:
+        while True:
+            if not self.running:
+                with utils.step_lock:
+                    if self._shutting_down or sim.simExtGetExitRequest():
+                        break
+                    sim.simExtStep(False)
+            time.sleep(0.01)
+        # If the exit request was from the UI, then call shutdown, otherwise
+        # shutdown caused this thread to terminate.
+        if not self._shutting_down:
+            self.shutdown()
+
+    def launch(self, scene_file: str = "", headless: bool = False,
+               responsive_ui: bool = False, blocking: bool = False,
+               verbosity: Verbosity = Verbosity.NONE) -> None:
+        """Launches CoppeliaSim.
+
+        Launches the UI thread, waits until the UI thread has finished, this
+        results in the current thread becoming the simulation thread.
+
+        :param scene_file: The scene file to load. Empty string for empty scene.
+        :param headless: Run CoppeliaSim in simulation mode.
+        :param responsive_ui: If True, then a separate thread will be created to
+            asynchronously step the UI of CoppeliaSim. Note, that will reduce
+            the responsiveness of the simulation thread.
+        :param blocking: Causes CoppeliaSim to launch as if running the default
+            c++ client application. This is causes the function to block.
+            For most users, this will be set to False.
+        :param verbosity: The verbosity level for CoppeliaSim.
+            Usually Verbosity.NONE or Verbosity.LOAD_INFOS.
+        """
+        abs_scene_file = os.path.abspath(scene_file)
+        if len(scene_file) > 0 and not os.path.isfile(abs_scene_file):
+            raise PyRepError('Scene file does not exist: %s' % scene_file)
+        cwd = os.getcwd()
+        self._ui_thread = threading.Thread(
+            target=self._run_ui_thread,
+            args=(abs_scene_file, headless, verbosity))
+        self._ui_thread.daemon = True
+        self._ui_thread.start()
+
+        while not sim.simExtCanInitSimThread():
+            time.sleep(0.1)
+
+        sim.simExtSimThreadInit()
+        time.sleep(0.2)  # Stops CoppeliaSim crashing if restarted too quickly.
+
+        if blocking:
+            while not sim.simExtGetExitRequest():
+                sim.simExtStep()
+            self.shutdown()
+        elif responsive_ui:
+            self._responsive_ui_thread = threading.Thread(
+                target=self._run_responsive_ui_thread)
+            self._responsive_ui_thread.daemon = True
+            try:
+                self._responsive_ui_thread.start()
+            except (KeyboardInterrupt, SystemExit):
+                if not self._shutting_down:
+                    self.shutdown()
+                sys.exit()
+            self.step()
+        else:
+            self.step()
+        os.chdir(cwd)  # Go back to the previous cwd
+
+    def script_call(self, function_name_at_script_name: str,
+                    script_handle_or_type: int,
+                    ints=(), floats=(), strings=(), bytes='') -> (
+            Tuple[List[int], List[float], List[str], str]):
+        """Calls a script function (from a plugin, the main client application,
+        or from another script). This represents a callback inside of a script.
+
+        :param function_name_at_script_name: A string representing the function
+            name and script name, e.g. myFunctionName@theScriptName. When the
+            script is not associated with an object, then just specify the
+            function name.
+        :param script_handle_or_type: The handle of the script, otherwise the
+            type of the script.
+        :param ints: The input ints to the script.
+        :param floats: The input floats to the script.
+        :param strings: The input strings to the script.
+        :param bytes: The input bytes to the script (as a string).
+        :return: Any number of return values from the called Lua function.
+        """
+        return utils.script_call(
+            function_name_at_script_name, script_handle_or_type, ints, floats,
+            strings, bytes)
+
+    def shutdown(self) -> None:
+        """Shuts down the CoppeliaSim simulation.
+        """
+        if self._ui_thread is None:
+            raise PyRepError(
+                'CoppeliaSim has not been launched. Call launch first.')
+        if self._ui_thread is not None:
+            self._shutting_down = True
+            self.stop()
+            self.step_ui()
+            sim.simExtPostExitRequest()
+            sim.simExtSimThreadDestroy()
+            self._ui_thread.join()
+            if self._responsive_ui_thread is not None:
+                self._responsive_ui_thread.join()
+            # CoppeliaSim crashes if new instance opened too quickly after shutdown.
+            # TODO: A small sleep stops this for now.
+            time.sleep(0.1)
+        self._ui_thread = None
+        self._shutting_down = False
+
+    def start(self) -> None:
+        """Starts the physics simulation if it is not already running.
+        """
+        if self._ui_thread is None:
+            raise PyRepError(
+                'CoppeliaSim has not been launched. Call launch first.')
+        if not self.running:
+            sim.simStartSimulation()
+            self.running = True
+
+    def stop(self) -> None:
+        """Stops the physics simulation if it is running.
+        """
+        if self._ui_thread is None:
+            raise PyRepError(
+                'CoppeliaSim has not been launched. Call launch first.')
+        if self.running:
+            sim.simStopSimulation()
+            self.running = False
+            # Need this so the UI updates
+            [self.step() for _ in range(5)]  # type: ignore
+
+    def step(self) -> None:
+        """Execute the next simulation step.
+
+        If the physics simulation is not running, then this will only update
+        the UI.
+        """
+        with utils.step_lock:
+            sim.simExtStep()
+
+    def step_ui(self) -> None:
+        """Update the UI.
+
+        This will not execute the next simulation step, even if the physics
+        simulation is running.
+        This is only applicable when PyRep was launched without a responsive UI.
+        """
+        with utils.step_lock:
+            sim.simExtStep(False)
+
+    def set_simulation_timestep(self, dt: float) -> None:
+        """Sets the simulation time step. Default is 0.05.
+
+        :param dt: The time step value in seconds.
+        """
+        sim.simSetFloatParameter(sim.sim_floatparam_simulation_time_step, dt)
+        if not np.allclose(self.get_simulation_timestep(), dt):
+            warnings.warn('Could not change simulation timestep. You may need '
+                          'to change it to "custom dt" using simulation '
+                          'settings dialog.')
+
+    def get_simulation_timestep(self) -> float:
+        """Gets the simulation time step.
+
+        :return: The time step value in seconds.
+        """
+        return sim.simGetSimulationTimeStep()
+
+    def set_configuration_tree(self, config_tree: bytes) -> None:
+        """Restores configuration information previously retrieved.
+
+        Configuration information (object relative positions/orientations,
+        joint/path values) can be retrieved with
+        :py:meth:`Object.get_configuration_tree`. Dynamically simulated
+        objects will implicitly be reset before the command is applied
+        (i.e. similar to calling :py:meth:`Object.reset_dynamic_object` just
+        before).
+
+        :param config_tree: The configuration tree to restore.
+        """
+        sim.simSetConfigurationTree(config_tree)
+
+    def group_objects(self, objects: List[Shape]) -> Shape:
+        """Groups several shapes into a compound shape (or simple shape).
+
+        :param objects: The list of shapes to group.
+        :return: A single grouped shape.
+        """
+        handles = [o.get_handle() for o in objects]
+        handle = sim.simGroupShapes(handles)
+        return Shape(handle)
+
+    def merge_objects(self, objects: List[Shape]) -> Shape:
+        """Merges several shapes into a compound shape (or simple shape).
+
+        :param objects: The list of shapes to group.
+        :return: A single merged shape.
+        """
+        handles = [o.get_handle() for o in objects]
+        # FIXME: sim.simGroupShapes(merge=True) won't return correct handle,
+        # so we use name to find correct handle of the merged shape.
+        name = objects[-1].get_name()
+        sim.simGroupShapes(handles, merge=True)
+        return Shape(name)
+
+    def export_scene(self, filename: str) -> None:
+        """Saves the current scene.
+
+        :param filename: scene filename. The filename extension is required
+            ("ttt").
+        """
+        sim.simSaveScene(filename)
+
+    def import_model(self, filename: str) -> Object:
+        """	Loads a previously saved model.
+
+        :param filename: model filename. The filename extension is required
+            ("ttm"). An optional "@copy" can be appended to the filename, in
+            which case the model's objects will be named/renamed as if an
+            associated script was attached to the model.
+        :return: The imported model.
+        """
+        handle = sim.simLoadModel(filename)
+        return utils.to_type(handle)
+
+    def create_texture(self, filename: str, interpolate=True, decal_mode=False,
+                       repeat_along_u=False, repeat_along_v=False
+                       ) -> Tuple[Shape, Texture]:
+        """Creates a planar shape that is textured.
+
+        :param filename: Path to the texture to load.
+        :param interpolate: Adjacent texture pixels are not interpolated.
+        :param decal_mode: Texture is applied as a decal (its appearance
+            won't be influenced by light conditions).
+        :param repeat_along_u: Texture will be repeated along the U direction.
+        :param repeat_along_v: Texture will be repeated along the V direction.
+        :return: A tuple containing the textured plane and the texture.
+        """
+        options = 0
+        if not interpolate:
+            options |= 1
+        if decal_mode:
+            options |= 2
+        if repeat_along_u:
+            options |= 3
+        if repeat_along_v:
+            options |= 4
+        handle = sim.simCreateTexture(filename, options)
+        s = Shape(handle)
+        return s, s.get_texture()
+
+    def get_objects_in_tree(self, root_object=None, *args, **kwargs
+                            ) -> List[Object]:
+        """Retrieves the objects in a given hierarchy tree.
+
+        :param root_object: The root object in the tree. Pass None to retrieve
+            all objects in the configuration tree. :py:class:`Object` or `int`.
+        :param object_type: The object type to retrieve.
+            One of :py:class:`.ObjectType`.
+        :param exclude_base: Exclude the tree base from the returned list.
+        :param first_generation_only: Include in the returned list only the
+            object's first children. Otherwise, entire hierarchy is returned.
+        :return: A list of objects in the hierarchy tree.
+        """
+        return Object._get_objects_in_tree(root_object, *args, **kwargs)
+
+    def get_collection_handle_by_name(self, collection_name: str) -> int:
+        """Retrieves the integer handle for a given collection.
+
+        :param collection_name: Name of the collection to retrieve the integer handle for
+        :return: An integer handle for the collection
+        """
+        return sim.simGetCollectionHandle(collection_name)

external/pyrep/pyrep/robots/arms/lbr_iiwa_14_r820.py

@@ -0,0 +1,7 @@
+from pyrep.robots.arms.arm import Arm
+
+
+class LBRIwaa14R820(Arm):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'LBR_iiwa_14_R820', 7)

external/pyrep/pyrep/robots/arms/lbr_iiwa_7_r800.py

@@ -0,0 +1,7 @@
+from pyrep.robots.arms.arm import Arm
+
+
+class LBRIwaa7R800(Arm):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'LBR_iiwa_7_R800', 7)

external/pyrep/pyrep/robots/arms/locobot_arm.py

@@ -0,0 +1,7 @@
+from pyrep.robots.arms.arm import Arm
+
+
+class LoCoBotArm(Arm):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'LoCoBotArm', 5)

external/pyrep/pyrep/robots/arms/mico.py

@@ -0,0 +1,7 @@
+from pyrep.robots.arms.arm import Arm
+
+
+class Mico(Arm):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'Mico', 6)

external/pyrep/pyrep/robots/arms/panda.py

@@ -0,0 +1,7 @@
+from pyrep.robots.arms.arm import Arm
+
+
+class Panda(Arm):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'Panda', 7)

external/pyrep/pyrep/robots/arms/sawyer.py

@@ -0,0 +1,7 @@
+from pyrep.robots.arms.arm import Arm
+
+
+class Sawyer(Arm):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'Sawyer', 7)

external/pyrep/pyrep/robots/arms/ur10.py

@@ -0,0 +1,7 @@
+from pyrep.robots.arms.arm import Arm
+
+
+class UR10(Arm):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'UR10', 6)

external/pyrep/pyrep/robots/arms/ur3.py

@@ -0,0 +1,7 @@
+from pyrep.robots.arms.arm import Arm
+
+
+class UR3(Arm):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'UR3', 6)

external/pyrep/pyrep/robots/arms/ur5.py

@@ -0,0 +1,7 @@
+from pyrep.robots.arms.arm import Arm
+
+
+class UR5(Arm):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'UR5', 6)

external/pyrep/pyrep/robots/arms/xarm7.py

@@ -0,0 +1,7 @@
+from pyrep.robots.arms.arm import Arm
+
+
+class XArm7(Arm):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'xarm', 7)

external/pyrep/pyrep/robots/arms/youBot.py

@@ -0,0 +1,7 @@
+from pyrep.robots.arms.arm import Arm
+
+
+class youBot(Arm):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'YouBot', 5)

external/pyrep/pyrep/robots/configuration_paths/arm_configuration_path.py

@@ -0,0 +1,199 @@
+from pyrep.backend import sim
+from pyrep.robots.configuration_paths.configuration_path import (
+    ConfigurationPath)
+import numpy as np
+from typing import List, Optional, Union
+
+
+class ArmConfigurationPath(ConfigurationPath):
+    """A path expressed in joint configuration space.
+
+    Paths are retrieved from an :py:class:`Arm`, and are associated with the
+    arm that generated the path.
+
+    This class is used for executing motion along a path via the
+    Reflexxes Motion Library type II or IV. The Reflexxes Motion Library
+    provides instantaneous trajectory generation capabilities for motion
+    control systems.
+    """
+
+    def __init__(self, arm: 'Arm',  # type: ignore
+                 path_points: Union[List[float], np.ndarray]):
+        self._arm = arm
+        self._path_points = np.asarray(path_points)
+        self._rml_handle: Optional[int] = None
+        self._drawing_handle = None
+        self._path_done = False
+        self._num_joints = arm.get_joint_count()
+        self._joint_position_action = None
+
+    def __len__(self):
+        return len(self._path_points) // self._num_joints
+
+    def __getitem__(self, i):
+        path_points = self._path_points.reshape(-1, self._num_joints)
+        path_points = path_points[i].flatten()
+        return self.__class__(arm=self._arm, path_points=path_points)
+
+    def step(self) -> bool:
+        """Makes a step along the trajectory.
+
+        This function steps forward a trajectory generation algorithm from
+        Reflexxes Motion Library.
+        NOTE: This does not step the physics engine. This is left to the user.
+
+        :return: If the end of the trajectory has been reached.
+        """
+        if self._path_done:
+            raise RuntimeError('This path has already been completed. '
+                               'If you want to re-run, then call set_to_start.')
+        if self._rml_handle is None:
+            self._rml_handle = self._get_rml_handle()
+        done = self._step_motion() == 1
+        self._path_done = done
+        return done
+
+    def set_to_start(self, disable_dynamics=False) -> None:
+        """Sets the arm to the beginning of this path.
+        """
+        start_config = self._path_points[:len(self._arm.joints)]
+        self._arm.set_joint_positions(start_config, disable_dynamics=disable_dynamics)
+        self._path_done = False
+
+    def set_to_end(self, disable_dynamics=False) -> None:
+        """Sets the arm to the end of this path.
+        """
+        final_config = self._path_points[-len(self._arm.joints):]
+        self._arm.set_joint_positions(final_config, disable_dynamics=disable_dynamics)
+
+    def visualize(self) -> None:
+        """Draws a visualization of the path in the scene.
+
+        The visualization can be removed
+        with :py:meth:`ConfigurationPath.clear_visualization`.
+        """
+        if len(self._path_points) <= 0:
+            raise RuntimeError("Can't visualise a path with no points.")
+
+        tip = self._arm.get_tip()
+        self._drawing_handle = sim.simAddDrawingObject(
+            objectType=sim.sim_drawing_lines, size=3, duplicateTolerance=0,
+            parentObjectHandle=-1, maxItemCount=99999,
+            ambient_diffuse=[1, 0, 1])
+        sim.simAddDrawingObjectItem(self._drawing_handle, None)
+        init_angles = self._arm.get_joint_positions()
+        self._arm.set_joint_positions(
+            self._path_points[0: len(self._arm.joints)])
+        prev_point = list(tip.get_position())
+
+        for i in range(len(self._arm.joints), len(self._path_points),
+                       len(self._arm.joints)):
+            points = self._path_points[i:i + len(self._arm.joints)]
+            self._arm.set_joint_positions(points)
+            p = list(tip.get_position())
+            sim.simAddDrawingObjectItem(self._drawing_handle, prev_point + p)
+            prev_point = p
+
+        # Set the arm back to the initial config
+        self._arm.set_joint_positions(init_angles)
+
+    def clear_visualization(self) -> None:
+        """Clears/removes a visualization of the path in the scene.
+        """
+        if self._drawing_handle is not None:
+            sim.simAddDrawingObjectItem(self._drawing_handle, None)
+
+    def get_executed_joint_position_action(self) -> np.ndarray:
+        return self._joint_position_action
+
+    def _get_rml_handle(self) -> int:
+        dt = sim.simGetSimulationTimeStep()
+        limits = np.array(self._arm.get_joint_upper_velocity_limits())
+        vel_correction = 1.0
+        max_vel = self._arm.max_velocity
+        max_accel = self._arm.max_acceleration
+        max_jerk = self._arm.max_jerk
+        lengths = self._get_path_point_lengths()
+        target_pos_vel = [lengths[-1],0]
+        previous_q = self._path_points[0:len(self._arm.joints)]
+
+        while True:
+            pos_vel_accel = [0, 0, 0]
+            rMax = 0
+            rml_handle = sim.simRMLPos(
+                1, 0.0001, -1, pos_vel_accel,
+                [max_vel * vel_correction, max_accel, max_jerk],
+                [1], target_pos_vel)
+            state = 0
+            while state == 0:
+                state, pos_vel_accel = sim.simRMLStep(rml_handle, dt, 1)
+                if state >= 0:
+                    pos = pos_vel_accel[0]
+                    for i in range(len(lengths)-1):
+                        if lengths[i] <= pos <= lengths[i + 1]:
+                            t = (pos - lengths[i]) / (lengths[i + 1] - lengths[i])
+                            # For each joint
+                            offset = len(self._arm.joints) * i
+                            p1 = self._path_points[
+                                 offset:offset + self._num_joints]
+                            offset = len(self._arm.joints) * (i + 1)
+                            p2 = self._path_points[
+                                 offset:offset + self._num_joints]
+                            dx = p2 - p1
+                            qs = p1 + dx * t
+                            dq = qs - previous_q
+                            previous_q = qs
+                            r = np.abs(dq / dt) / limits
+                            m = np.max(r)
+                            if m > rMax:
+                                rMax = m
+                            break
+            sim.simRMLRemove(rml_handle)
+            if rMax > 1.001:
+                vel_correction = vel_correction / rMax
+            else:
+                break
+        pos_vel_accel = [0, 0, 0]
+        rml_handle = sim.simRMLPos(
+            1, 0.0001, -1, pos_vel_accel,
+            [max_vel*vel_correction, max_accel, max_jerk], [1], target_pos_vel)
+        return rml_handle
+
+    def _step_motion(self) -> int:
+        self._joint_position_action = None
+        dt = sim.simGetSimulationTimeStep()
+        lengths = self._get_path_point_lengths()
+        state, posVelAccel = sim.simRMLStep(self._rml_handle, dt, 1)
+        if state >= 0:
+            pos = posVelAccel[0]
+            for i in range(len(lengths) - 1):
+                if lengths[i] <= pos <= lengths[i + 1]:
+                    t = (pos - lengths[i]) / (lengths[i + 1] - lengths[i])
+                    # For each joint
+                    offset = len(self._arm.joints) * i
+                    p1 = self._path_points[
+                         offset:offset + len(self._arm.joints)]
+                    offset = self._arm._num_joints * (i + 1)
+                    p2 = self._path_points[
+                         offset:offset + len(self._arm.joints)]
+                    dx = p2 - p1
+                    qs = p1 + dx * t
+                    self._joint_position_action = qs
+                    self._arm.set_joint_target_positions(qs)
+                    break
+        if state == 1:
+            sim.simRMLRemove(self._rml_handle)
+        return state
+
+    def _get_path_point_lengths(self) -> List[float]:
+        path_points = self._path_points
+        prev_points = path_points[0:len(self._arm.joints)]
+        dists = [0.]
+        d = 0
+        for i in range(len(self._arm.joints), len(self._path_points),
+                       len(self._arm.joints)):
+            points = path_points[i:i + len(self._arm.joints)]
+            d += np.sqrt(np.sum(np.square(prev_points - points)))
+            dists.append(d)
+            prev_points = points
+        return dists

external/pyrep/pyrep/robots/configuration_paths/configuration_path.py

@@ -0,0 +1,18 @@
+class ConfigurationPath(object):
+    """Base class for paths expressed in joint configuration space.
+    """
+
+    def step(self) -> bool:
+        raise NotImplementedError()
+
+    def set_to_start(self) -> None:
+        raise NotImplementedError()
+
+    def set_to_end(self) -> None:
+        raise NotImplementedError()
+
+    def visualize(self) -> None:
+        raise NotImplementedError()
+
+    def clear_visualization(self) -> None:
+        raise NotImplementedError()

external/pyrep/pyrep/robots/configuration_paths/holonomic_configuration_path.py

@@ -0,0 +1,71 @@
+from pyrep.backend import sim, utils
+from pyrep.robots.configuration_paths.mobile_configuration_path import (
+    MobileConfigurationPath)
+from pyrep.const import PYREP_SCRIPT_TYPE
+from math import sqrt
+
+
+class HolonomicConfigurationPath(MobileConfigurationPath):
+    """A path expressed in joint configuration space.
+
+    Paths are retrieved from an :py:class:`Mobile`, and are associated with the
+    mobile base that generated the path.
+
+    This class is used for executing motion along a path via the
+    _get_base_actuation function employing a proportional controller.
+    """
+
+    def step(self) -> bool:
+        """Make a step along the trajectory.
+
+        Step forward by calling _get_base_actuation to get the velocity needed
+        to be applied at the wheels.
+
+        NOTE: This does not step the physics engine. This is left to the user.
+
+        :return: If the end of the trajectory has been reached.
+
+        """
+        if self._path_done:
+            raise RuntimeError('This path has already been completed. '
+                               'If you want to re-run, then call set_to_start.')
+
+        pos_inter = self._mobile.intermediate_target_base.get_position(
+            relative_to=self._mobile)
+
+        if len(self._path_points) > 2:  # Non-linear path
+            if self.inter_done:
+                self._next_i_path()
+                self._set_inter_target(self.i_path)
+                self.inter_done = False
+
+                handleBase = self._mobile.get_handle()
+                handleInterTargetBase = self._mobile.intermediate_target_base.get_handle()
+
+                __, ret_floats, _, _ = utils.script_call(
+                    'getBoxAdjustedMatrixAndFacingAngle@PyRep',
+                    PYREP_SCRIPT_TYPE,
+                    ints=[handleBase, handleInterTargetBase])
+
+                m = ret_floats[:-1]
+                angle = ret_floats[-1]
+                self._mobile.intermediate_target_base.set_position(
+                    [m[3], m[7], self._mobile.target_z])
+                self._mobile.intermediate_target_base.set_orientation(
+                    [0, 0, angle])
+
+            if sqrt((pos_inter[0]) ** 2 + (pos_inter[1]) ** 2) < 0.1:
+                self.inter_done = True
+                actuation = [0, 0, 0]
+            else:
+                actuation, _ = self._mobile.get_base_actuation()
+
+            self._mobile.set_base_angular_velocites(actuation)
+
+            if self.i_path == len(self._path_points) - 1:
+                self._path_done = True
+        else:
+            actuation, self._path_done = self._mobile.get_base_actuation()
+            self._mobile.set_base_angular_velocites(actuation)
+
+        return self._path_done

external/pyrep/pyrep/robots/configuration_paths/mobile_configuration_path.py

@@ -0,0 +1,114 @@
+from pyrep.backend import sim, utils
+from pyrep.robots.configuration_paths.configuration_path import (
+    ConfigurationPath)
+from pyrep.robots.mobiles.mobile_base import MobileBase
+from pyrep.const import PYREP_SCRIPT_TYPE
+from math import sqrt
+from typing import List
+
+
+class MobileConfigurationPath(ConfigurationPath):
+    """A path expressed in joint configuration space.
+
+    Paths are retrieved from an :py:class:`Mobile`, and are associated with the
+    mobile base that generated the path.
+
+    This class is used for executing motion along a path via the
+    _get_base_actuation function employing a proportional controller.
+    """
+
+    def __init__(self, mobile: MobileBase, path_points: List[List[float]]):
+        self._mobile = mobile
+        self._path_points = path_points
+        self._drawing_handle = None
+        self._path_done = False
+        self.i_path = -1
+        self.inter_done = True
+        self._num_joints = mobile.get_joint_count()
+
+        self.set_to_start()
+
+        if len(self._path_points) > 2:
+            self._set_inter_target(0)
+
+    def step(self) -> bool:
+        """Make a step along the trajectory.
+
+        Step forward by calling _get_base_actuation to get the velocity needed
+        to be applied at the wheels.
+
+        NOTE: This does not step the physics engine. This is left to the user.
+
+        :return: If the end of the trajectory has been reached.
+
+        """
+        raise NotImplementedError()
+
+    def set_to_start(self) -> None:
+        """Sets the mobile base to the beginning of this path.
+
+        :param allow_force_mode: Not used.
+        """
+        start_config = self._path_points[0]
+        self._mobile.set_2d_pose(start_config[:3])
+        self._path_done = False
+
+    def set_to_end(self) -> None:
+        """Sets the mobile base to the end of this path.
+
+        :param allow_force_mode: Not used.
+        """
+        final_config = self._path_points[-1]
+        self._mobile.set_2d_pose(final_config[:3])
+
+    def visualize(self) -> None:
+        """Draws a visualization of the path in the scene.
+
+        The visualization can be removed
+        with :py:meth:`ConfigurationPath.clear_visualization`.
+        """
+        if len(self._path_points) <= 0:
+            raise RuntimeError("Can't visualise a path with no points.")
+
+        tip = self._mobile
+        self._drawing_handle = sim.simAddDrawingObject(
+            objectType=sim.sim_drawing_lines, size=3, duplicateTolerance=0,
+            parentObjectHandle=-1, maxItemCount=99999,
+            ambient_diffuse=[1, 0, 1])
+        sim.simAddDrawingObjectItem(self._drawing_handle, None)
+        init_pose = self._mobile.get_2d_pose()
+        self._mobile.set_2d_pose(self._path_points[0][:3])
+        prev_point = list(tip.get_position())
+
+        for i in range(len(self._path_points)):
+            points = self._path_points[i]
+            self._mobile.set_2d_pose(points[:3])
+            p = list(tip.get_position())
+            sim.simAddDrawingObjectItem(self._drawing_handle, prev_point + p)
+            prev_point = p
+
+        # Set the arm back to the initial config
+        self._mobile.set_2d_pose(init_pose[:3])
+
+    def clear_visualization(self) -> None:
+        """Clears/removes a visualization of the path in the scene.
+        """
+        if self._drawing_handle is not None:
+            sim.simAddDrawingObjectItem(self._drawing_handle, None)
+
+    def _next_i_path(self):
+        incr = 0.01
+        dist_to_next = 0
+        while dist_to_next < incr:
+            self.i_path += 1
+            if self.i_path == len(self._path_points) - 1:
+                self.i_path = len(self._path_points) - 1
+                break
+            dist_to_next += self._path_points[self.i_path][-1]
+
+    def _set_inter_target(self, i):
+        self._mobile.intermediate_target_base.set_position(
+            [self._path_points[i][0], self._path_points[i][1],
+             self._mobile.target_z])
+        self._mobile.intermediate_target_base.set_orientation(
+            [0, 0, self._path_points[i][2]])

external/pyrep/pyrep/robots/configuration_paths/nonholonomic_configuration_path.py

@@ -0,0 +1,55 @@
+from pyrep.robots.configuration_paths.mobile_configuration_path import (
+    MobileConfigurationPath)
+from math import sqrt
+
+
+class NonHolonomicConfigurationPath(MobileConfigurationPath):
+    """A path expressed in joint configuration space.
+
+    Paths are retrieved from an :py:class:`Mobile`, and are associated with the
+    mobile base that generated the path.
+
+    This class is used for executing motion along a path via the
+    _get_base_actuation function employing a proportional controller.
+    """
+
+    def step(self) -> bool:
+        """Make a step along the trajectory.
+
+        Step forward by calling _get_base_actuation to get the velocity needed
+        to be applied at the wheels.
+
+        NOTE: This does not step the physics engine. This is left to the user.
+
+        :return: If the end of the trajectory has been reached.
+
+        """
+        if self._path_done:
+            raise RuntimeError('This path has already been completed. '
+                               'If you want to re-run, then call set_to_start.')
+
+        pos_inter = self._mobile.intermediate_target_base.get_position(
+            relative_to=self._mobile)
+
+        if len(self._path_points) > 2:  # Non-linear path
+            if self.inter_done:
+                self._next_i_path()
+                self._set_inter_target(self.i_path)
+                self.inter_done = False
+
+            if sqrt((pos_inter[0]) ** 2 + (pos_inter[1]) ** 2) < 0.1:
+                self.inter_done = True
+                actuation, _ = self._mobile.get_base_actuation()
+            else:
+                actuation, _ = self._mobile.get_base_actuation()
+
+            self._mobile.set_joint_target_velocities(actuation)
+
+            if self.i_path == len(self._path_points) - 1:
+                self._path_done = True
+
+        else:
+            actuation, self._path_done = self._mobile.get_base_actuation()
+            self._mobile.set_joint_target_velocities(actuation)
+
+        return self._path_done

external/pyrep/pyrep/robots/end_effectors/baxter_gripper.py

@@ -0,0 +1,9 @@
+from pyrep.robots.end_effectors.gripper import Gripper
+
+
+class BaxterGripper(Gripper):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'BaxterGripper',
+                         ['BaxterGripper_leftJoint',
+                          'BaxterGripper_rightJoint'])

external/pyrep/pyrep/robots/end_effectors/baxter_suction_cup.py

@@ -0,0 +1,7 @@
+from pyrep.robots.end_effectors.suction_cup import SuctionCup
+
+
+class BaxterSuctionCup(SuctionCup):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'BaxterSuctionCup')

external/pyrep/pyrep/robots/end_effectors/dobot_suction_cup.py

@@ -0,0 +1,7 @@
+from pyrep.robots.end_effectors.suction_cup import SuctionCup
+
+
+class DobotSuctionCup(SuctionCup):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'Dobot_suctionCup')

external/pyrep/pyrep/robots/end_effectors/dual_panda_gripper.py

@@ -0,0 +1,22 @@
+from pyrep.robots.end_effectors.gripper import Gripper
+
+
+
+class PandaGripperRight(Gripper):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'Panda_rightArm_gripper',
+                         ['Panda_rightArm_gripper_joint1', 'Panda_rightArm_gripper_joint2'])
+
+
+
+class PandaGripperLeft(Gripper):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'Panda_leftArm_gripper',
+                         ['Panda_leftArm_gripper_joint1', 'Panda_leftArm_gripper_joint2'])
+
+
+
+
+

external/pyrep/pyrep/robots/end_effectors/gripper.py

@@ -0,0 +1,149 @@
+from typing import List, Union
+from pyrep.objects.object import Object
+from pyrep.objects.proximity_sensor import ProximitySensor
+from pyrep.objects.force_sensor import ForceSensor
+from pyrep.robots.robot_component import RobotComponent
+import numpy as np
+
+import logging
+
+POSITION_ERROR = 0.001
+
+
+class Gripper(RobotComponent):
+    """Represents all types of end-effectors, e.g. grippers.
+    """
+
+    def __init__(self, count: int, name: str, joint_names: List[str]):
+        super().__init__(count, name, joint_names)
+        suffix = '' if count == 0 else '#%d' % (count - 1)
+        prox_name = '%s_attachProxSensor%s' % (name, suffix)
+        attach_name = '%s_attachPoint%s' % (name, suffix)
+        self._proximity_sensor = ProximitySensor(prox_name)
+        self._attach_point = ForceSensor(attach_name)
+        self._old_parents: List[Object] = []
+        self._grasped_objects: List[Object] = []
+        self._prev_positions = [None] * len(joint_names)
+        self._prev_vels = [None] * len(joint_names)  # Used to stop oscillating
+
+        self._touch_sensors = []
+        i = 0
+        while True:
+            fname = '%s_touchSensor%d%s' % (name, i, suffix)
+            if not ForceSensor.exists(fname):
+                break
+            self._touch_sensors.append(ForceSensor(fname))
+            i += 1
+
+    def grasp(self, obj: Object) -> bool:
+        """Grasp object if it is detected.
+
+        Note: The does not actuate the gripper, but instead simply attaches the
+        detected object to the gripper to 'fake' a grasp.
+
+        :param obj: The object to grasp if detected.
+        :return: True if the object was detected/grasped.
+        """
+        detected = self._proximity_sensor.is_detected(obj)
+        # Check if detected and that we are not already grasping it.
+        if detected and obj not in self._grasped_objects:
+            self._grasped_objects.append(obj)
+            logging.debug("Grasping object with name: %s. Object is being attached to gripper.", obj.get_name())
+            self._old_parents.append(obj.get_parent())  # type: ignore
+            obj.set_parent(self._attach_point, keep_in_place=True)
+        return detected
+
+    def release(self) -> None:
+        """Release any grasped objects.
+
+        Note: The does not actuate the gripper, but instead simply detaches any
+        grasped objects.
+        """
+        for grasped_obj, old_parent in zip(
+                self._grasped_objects, self._old_parents):
+            # Check if the object still exists
+            if grasped_obj.still_exists():
+                grasped_obj.set_parent(old_parent, keep_in_place=True)
+        self._grasped_objects = []
+        self._old_parents = []
+
+    def get_grasped_objects(self) -> List[Object]:
+        """Gets the objects that are currently grasped.
+
+        :return: A list of grasped objects.
+        """
+        return self._grasped_objects
+
+    def actuate(self, amount: float, velocity: float) -> bool:
+        """Actuate the gripper, but return after each simulation step.
+
+        The functions attempts to open/close the gripper according to 'amount',
+        where 1 represents open, and 0 represents close. The user should
+        iteratively call this function until it returns True.
+
+        This is a convenience method. If you would like direct control of the
+        gripper, use the :py:class:`RobotComponent` methods instead.
+
+        For some grippers, this method will need to be overridden.
+
+        :param amount: A float between 0 and 1 representing the gripper open
+            state. 1 means open, whilst 0 means closed.
+        :param velocity: The velocity to apply to the gripper joints.
+
+
+        :raises: ValueError if 'amount' is not between 0 and 1.
+
+        :return: True if the gripper has reached its open/closed limits, or if
+            the 'max_force' has been exerted.
+        """
+        if not (0.0 <= amount <= 1.0):
+            raise ValueError("'open_amount' should be between 0 and 1.'")
+        _, joint_intervals_list = self.get_joint_intervals()
+        joint_intervals = np.array(joint_intervals_list)
+
+        # Decide on if we need to open or close
+        joint_range = joint_intervals[:, 1] - joint_intervals[:, 0]
+        target_pos = joint_intervals[:, 0] + (joint_range * amount)
+
+        current_positions = self.get_joint_positions()
+        done = True
+        for i, (j, target, cur, prev) in enumerate(zip(
+                self.joints, target_pos, current_positions,
+                self._prev_positions)):
+            # Check if the joint has moved much
+            not_moving = (prev is not None and
+                          np.fabs(cur - prev) < POSITION_ERROR)
+            reached_target = np.fabs(target - cur) < POSITION_ERROR
+            vel = -velocity if cur - target > 0 else velocity
+            oscillating = (self._prev_vels[i] is not None and
+                           vel != self._prev_vels[i])
+            if not_moving or reached_target or oscillating:
+                j.set_joint_target_velocity(0)
+                continue
+            done = False
+            self._prev_vels[i] = vel  # type: ignore
+            j.set_joint_target_velocity(vel)
+        self._prev_positions = current_positions  # type: ignore
+        if done:
+            self._prev_positions = [None] * self._num_joints
+            self._prev_vels = [None] * self._num_joints
+            self.set_joint_target_velocities([0.0] * self._num_joints)
+        return done
+
+    def get_open_amount(self) -> List[float]:
+        """Gets the gripper open state. 1 means open, whilst 0 means closed.
+
+        :return: A list of floats between 0 and 1 representing the gripper open
+            state for each joint. 1 means open, whilst 0 means closed.
+        """
+        _, joint_intervals_list = self.get_joint_intervals()
+        joint_intervals = np.array(joint_intervals_list)
+        joint_range = joint_intervals[:, 1] - joint_intervals[:, 0]
+        return list(np.clip((np.array(
+            self.get_joint_positions()) - joint_intervals[:, 0]) /
+                            joint_range, 0.0, 1.0))
+
+    def get_touch_sensor_forces(self) -> List[List[float]]:
+        if len(self._touch_sensors) == 0:
+            raise NotImplementedError('No touch sensors for this robot!')
+        return [ts.read()[0] for ts in self._touch_sensors]

external/pyrep/pyrep/robots/end_effectors/jaco_gripper.py

@@ -0,0 +1,10 @@
+from pyrep.robots.end_effectors.gripper import Gripper
+
+
+class JacoGripper(Gripper):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'JacoHand',
+                         ['JacoHand_joint1_finger1',
+                          'JacoHand_joint1_finger2',
+                          'JacoHand_joint1_finger3'])

external/pyrep/pyrep/robots/end_effectors/locobot_gripper.py

@@ -0,0 +1,8 @@
+from pyrep.robots.end_effectors.gripper import Gripper
+
+
+class LoCoBotGripper(Gripper):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'LoCoBotGripper',
+                         ['LoCoBotGripper_joint1', 'LoCoBotGripper_joint2'])

external/pyrep/pyrep/robots/end_effectors/mico_gripper.py

@@ -0,0 +1,9 @@
+from pyrep.robots.end_effectors.gripper import Gripper
+
+
+class MicoGripper(Gripper):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'MicoHand',
+                         ['MicoHand_joint1_finger1',
+                          'MicoHand_joint1_finger3'])

external/pyrep/pyrep/robots/end_effectors/panda_gripper.py

@@ -0,0 +1,8 @@
+from pyrep.robots.end_effectors.gripper import Gripper
+
+
+class PandaGripper(Gripper):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'Panda_gripper',
+                         ['Panda_gripper_joint1', 'Panda_gripper_joint2'])

external/pyrep/pyrep/robots/end_effectors/robotiq85_gripper.py

@@ -0,0 +1,58 @@
+from typing import List
+
+from pyrep.objects import Joint
+from pyrep.robots.end_effectors.gripper import Gripper, POSITION_ERROR
+import numpy as np
+
+POSITION_ERROR = 0.00001
+
+
+class Robotiq85Gripper(Gripper):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'ROBOTIQ_85', ['ROBOTIQ_85_active1',
+                                               'ROBOTIQ_85_active2'])
+        self._open_amount_query_joints = [
+            Joint(jname) for jname in [
+                'ROBOTIQ_85_Rjoint1', 'ROBOTIQ_85_Ljoint1']]
+
+    def actuate(self, amount: float, velocity: float) -> bool:
+        if amount != 0.0 and amount != 1.0:
+            raise ValueError(
+                'This gripper currently only supports fully open or closed.')
+
+        current_positions = self.get_joint_positions()
+        vel = velocity if amount == 1.0 else -velocity
+        done = True
+        for i, (j, cur, prev) in enumerate(zip(
+                self.joints, current_positions,
+                self._prev_positions)):
+            # Check if the joint has moved much
+            not_moving = (prev is not None and
+                          np.fabs(cur - prev) < POSITION_ERROR)
+            if not_moving:
+                j.set_joint_target_velocity(0)
+                continue
+            done = False
+            self._prev_vels[i] = vel  # type: ignore
+            j.set_joint_target_velocity(vel)
+        self._prev_positions = current_positions  # type: ignore
+        if done:
+            self._prev_positions = [None] * self._num_joints
+            self._prev_vels = [None] * self._num_joints
+            self.set_joint_target_velocities([0.0] * self._num_joints)
+        return done
+
+    def get_open_amount(self) -> List[float]:
+        """Gets the gripper open state. 1 means open, whilst 0 means closed.
+
+        :return: A list of floats between 0 and 1 representing the gripper open
+            state for each joint. 1 means open, whilst 0 means closed.
+        """
+        joint_intervals = np.array([j.get_joint_interval()[1]
+                     for j in self._open_amount_query_joints])
+        joint_range = joint_intervals[:, 1] - joint_intervals[:, 0]
+        # Flip open amount
+        return list(1. - np.clip((np.array(
+            [j.get_joint_position() for j in self._open_amount_query_joints]
+        ) - joint_intervals[:, 0]) / joint_range, 0.0, 1.0))

external/pyrep/pyrep/robots/end_effectors/suction_cup.py

@@ -0,0 +1,57 @@
+from typing import List
+from pyrep.objects.object import Object
+from pyrep.objects.shape import Shape
+from pyrep.objects.proximity_sensor import ProximitySensor
+from pyrep.objects.force_sensor import ForceSensor
+
+
+class SuctionCup(Shape):
+    """Represents all suction cups.
+    """
+
+    def __init__(self, count: int, name: str, base_name: str = None):
+        suffix = '' if count == 0 else '#%d' % (count - 1)
+        super().__init__(name + suffix if base_name is None else base_name + suffix)
+        self._proximity_sensor = ProximitySensor('%s_sensor%s' % (name, suffix))
+        self._attach_point = ForceSensor('%s_connection%s' % (name, suffix))
+        self._old_parents: List[Object] = []
+        self._grasped_objects: List[Object] = []
+
+    def grasp(self, obj: Object) -> bool:
+        """Attach the object to the suction cup if it is detected.
+
+        Note: The does not move the object up to the suction cup. Therefore, the
+        proximity sensor should have a short range in order for the suction
+        grasp to look realistic.
+
+        :param obj: The object to grasp if detected.
+        :return: True if the object was detected/grasped.
+        """
+        detected = self._proximity_sensor.is_detected(obj)
+        # Check if detected and that we are not already grasping it.
+        if detected and obj not in self._grasped_objects:
+            self._grasped_objects.append(obj)
+            self._old_parents.append(obj.get_parent())  # type: ignore
+            obj.set_parent(self._attach_point, keep_in_place=True)
+        return detected
+
+    def release(self) -> None:
+        """Release any objects that have been sucked.
+
+        Note: The does not actuate the gripper, but instead simply detaches any
+        grasped objects.
+        """
+        for grasped_obj, old_parent in zip(
+                self._grasped_objects, self._old_parents):
+            # Check if the object still exists
+            if grasped_obj.still_exists():
+                grasped_obj.set_parent(old_parent, keep_in_place=True)
+        self._grasped_objects = []
+        self._old_parents = []
+
+    def get_grasped_objects(self) -> List[Object]:
+        """Gets the objects that are currently in the suction cup.
+
+        :return: A list of grasped objects.
+        """
+        return self._grasped_objects

external/pyrep/pyrep/robots/end_effectors/xarm_gripper.py

@@ -0,0 +1,13 @@
+from pyrep.robots.end_effectors.gripper import Gripper
+
+
+class XArmGripper(Gripper):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'xarm_gripper',
+                         ['xarm_left_inner_knuckle_joint',
+                          'xarm_right_inner_knuckle_joint',
+                          'xarm_drive_joint',
+                          'xarm_left_finger_joint',
+                          'xarm_right_outer_knuckle_joint',
+                          'xarm_right_finger_joint'])

external/pyrep/pyrep/robots/mobiles/holonomic_base.py

@@ -0,0 +1,215 @@
+from pyrep.robots.mobiles.mobile_base import MobileBase
+from pyrep.robots.configuration_paths.holonomic_configuration_path import (
+    HolonomicConfigurationPath)
+from pyrep.backend import utils
+from pyrep.const import PYREP_SCRIPT_TYPE
+from pyrep.const import ConfigurationPathAlgorithms as Algos
+from pyrep.errors import ConfigurationPathError
+from typing import List
+from pyrep.objects.joint import Joint
+from math import pi, sqrt
+
+
+class HolonomicBase(MobileBase):
+
+    def __init__(self,
+                 count: int,
+                 num_wheels: int,
+                 distance_from_target: float,
+                 name: str,
+                 max_velocity: float = 4,
+                 max_velocity_rotation: float = 6,
+                 max_acceleration: float = 0.035):
+        """Init.
+
+        :param count: used for multiple copies of robots.
+        :param num_wheels: number of actuated wheels.
+        :param distance_from_target: offset from target.
+        :param name: string with robot name (same as base in vrep model).
+        :param max_velocity: bounds x,y velocity for motion planning.
+        :param max_velocity_rotation: bounds yaw velocity for motion planning.
+        :param max_acceleration: bounds acceleration for motion planning.
+        """
+
+        super().__init__(count, num_wheels, name)
+
+        suffix = '' if count == 0 else '#%d' % (count - 1)
+
+        self.paramP = 20
+        self.paramO = 10
+        # self.paramO = 30
+        self.previous_forw_back_vel = 0
+        self.previous_left_right_vel = 0
+        self.previous_rot_vel = 0
+        self.accelF = max_acceleration
+        self.maxV = max_velocity
+        self.max_v_rot = max_velocity_rotation
+        self.dist1 = distance_from_target
+
+        joint_slipping_names = [
+            '%s_slipping_m_joint%s%s' % (name, str(i + 1), suffix) for i in
+            range(self.num_wheels)]
+        self.joints_slipping = [Joint(jsname)
+                                for jsname in joint_slipping_names]
+
+    def set_base_angular_velocites(self, velocity: List[float]):
+        """Calls required functions to achieve desired omnidirectional effect.
+
+        :param velocity: A List with forwardBackward, leftRight and rotation
+            velocity (in radian/s)
+        """
+        # self._reset_wheel()
+        fBVel = velocity[0]
+        lRVel = velocity[1]
+        rVel = velocity[2]
+        self.set_joint_target_velocities(
+            [-fBVel - lRVel - rVel, -fBVel + lRVel - rVel,
+             -fBVel - lRVel + rVel, -fBVel + lRVel + rVel])
+
+    def get_linear_path(self, position: List[float],
+                        angle=0) -> HolonomicConfigurationPath:
+        """Initialize linear path and check for collision along it.
+
+        Must specify either rotation in euler or quaternions, but not both!
+
+        :param position: The x, y position of the target.
+        :param angle: The z orientation of the target (in radians).
+        :raises: ConfigurationPathError if no path could be created.
+
+        :return: A linear path in the 2d space.
+        """
+        position_base = self.get_position()
+        angle_base = self.get_orientation()[-1]
+
+        self.target_base.set_position(
+            [position[0], position[1], self.target_z])
+        self.target_base.set_orientation([0, 0, angle])
+
+        handle_base = self.get_handle()
+        handle_target_base = self.target_base.get_handle()
+        _, ret_floats, _, _ = utils.script_call(
+            'getBoxAdjustedMatrixAndFacingAngle@PyRep', PYREP_SCRIPT_TYPE,
+            ints=[handle_base, handle_target_base])
+
+        m = ret_floats[:-1]
+        angle = ret_floats[-1]
+        self.intermediate_target_base.set_position(
+            [m[3] - m[0] * self.dist1, m[7] - m[4] * self.dist1,
+             self.target_z])
+        self.intermediate_target_base.set_orientation([0, 0, angle])
+        self.target_base.set_orientation([0, 0, angle])
+
+        path = [[position_base[0], position_base[1], angle_base],
+                [position[0], position[1], angle]]
+
+        if self._check_collision_linear_path(path):
+            raise ConfigurationPathError(
+                'Could not create path. '
+                'An object was detected on the linear path.')
+
+        return HolonomicConfigurationPath(self, path)
+
+    def get_nonlinear_path(self, position: List[float],
+                           angle=0,
+                           boundaries=2,
+                           path_pts=600,
+                           ignore_collisions=False,
+                           algorithm=Algos.RRTConnect
+                            ) -> HolonomicConfigurationPath:
+        """Gets a non-linear (planned) configuration path given a target pose.
+
+        :param position: The x, y, z position of the target.
+        :param angle: The z orientation of the target (in radians).
+        :param boundaries: A float defining the path search in x and y direction
+            [[-boundaries,boundaries],[-boundaries,boundaries]].
+        :param path_pts: The number of sampled points returned from the
+            computed path
+        :param ignore_collisions: If collision checking should be disabled.
+        :param algorithm: Algorithm used to compute path
+        :raises: ConfigurationPathError if no path could be created.
+
+        :return: A non-linear path (x,y,angle) in the xy configuration space.
+        """
+
+        path = self._get_nonlinear_path_points(
+            position, angle, boundaries, path_pts, ignore_collisions, algorithm)
+
+        return HolonomicConfigurationPath(self, path)
+
+    def get_base_actuation(self):
+        """Proportional controller.
+
+        :return: A list with left and right joint velocity,
+            and a bool representing target is reached.
+        """
+
+        handleBase = self.get_handle()
+        handle_inter_target_base = self.intermediate_target_base.get_handle()
+        pos_v = self.target_base.get_position(relative_to=self)
+        or_v = self.target_base.get_orientation(relative_to=self)
+
+        pos_inter = self.intermediate_target_base.get_position(
+            relative_to=self)
+        or_inter = self.intermediate_target_base.get_orientation(
+            relative_to=self)
+
+        if (sqrt((pos_v[0]) ** 2 + (
+        pos_v[1]) ** 2) - self.dist1) < 0.001 and or_v[-1] < 0.1 * pi / 180:
+            return [self.previous_forw_back_vel, self.previous_left_right_vel,
+                    self.previous_rot_vel], True
+
+        forw_back_vel = pos_inter[1] * self.paramP
+        left_right_vel = pos_inter[0] * self.paramP
+        rot_vel = - or_inter[2] * self.paramO
+
+        v = sqrt(forw_back_vel * forw_back_vel +
+                 left_right_vel * left_right_vel)
+        if v > self.maxV:
+            forw_back_vel = forw_back_vel * self.maxV / v
+            left_right_vel = left_right_vel * self.maxV / v
+
+        if (abs(rot_vel) > self.max_v_rot):
+            rot_vel = self.max_v_rot * rot_vel / abs(rot_vel)
+
+        df = forw_back_vel - self.previous_forw_back_vel
+        ds = left_right_vel - self.previous_left_right_vel
+        dr = rot_vel - self.previous_rot_vel
+
+        if abs(df) > self.maxV * self.accelF:
+            df = abs(df) * (self.maxV * self.accelF) / df
+
+        if abs(ds) > self.maxV * self.accelF:
+            ds = abs(ds) * (self.maxV * self.accelF) / ds
+
+        if abs(dr) > self.max_v_rot * self.accelF:
+            dr = abs(dr) * (self.max_v_rot * self.accelF) / dr
+
+        forw_back_vel = self.previous_forw_back_vel + df
+        left_right_vel = self.previous_left_right_vel + ds
+        rot_vel = self.previous_rot_vel + dr
+
+        self.previous_forw_back_vel = forw_back_vel
+        self.previous_left_right_vel = left_right_vel
+        self.previous_rot_vel = rot_vel
+
+        return [forw_back_vel, left_right_vel, rot_vel], False
+
+    def _reset_wheel(self):
+        """Required to achieve desired omnidirectional wheel effect.
+        """
+        [j.reset_dynamic_object() for j in self.wheels]
+
+        p = [[-pi / 4, 0, 0], [pi / 4, 0, pi], [-pi / 4, 0, 0], [pi / 4, 0, pi]]
+
+        for i in range(self.num_wheels):
+            self.joints_slipping[i].set_position([0, 0, 0],
+                                                 relative_to=self.joints[i],
+                                                 reset_dynamics=False)
+            self.joints_slipping[i].set_orientation(p[i],
+                                                    relative_to=self.joints[i],
+                                                    reset_dynamics=False)
+            self.wheels[i].set_position([0, 0, 0], relative_to=self.joints[i],
+                                        reset_dynamics=False)
+            self.wheels[i].set_orientation([0, 0, 0],
+                                           relative_to=self.joints[i],
+                                           reset_dynamics=False)

external/pyrep/pyrep/robots/mobiles/line_tracer.py

@@ -0,0 +1,6 @@
+from pyrep.robots.mobiles.mobile_base import MobileBase
+
+
+class LineTracer(MobileBase):
+    def __init__(self, count: int = 0):
+        super().__init__(count, 2, 'LineTracer')

external/pyrep/pyrep/robots/mobiles/locobot.py

@@ -0,0 +1,6 @@
+from pyrep.robots.mobiles.nonholonomic_base import NonHolonomicBase
+
+
+class LoCoBot(NonHolonomicBase):
+    def __init__(self, count: int = 0):
+        super().__init__(count, 2, 'LoCoBot')

external/pyrep/pyrep/robots/mobiles/mobile_base.py

@@ -0,0 +1,202 @@
+from pyrep.backend import sim, utils
+from pyrep.objects.dummy import Dummy
+from pyrep.objects.shape import Shape
+from pyrep.robots.robot_component import RobotComponent
+from pyrep.const import ConfigurationPathAlgorithms as Algos
+from pyrep.errors import ConfigurationPathError
+from pyrep.const import PYREP_SCRIPT_TYPE
+from typing import List, Union
+from math import sqrt
+import numpy as np
+
+
+class MobileBase(RobotComponent):
+    """Base class representing a robot mobile base with path planning support.
+    """
+
+    def __init__(self, count: int, num_wheels: int, name: str):
+        """Count is used for when we have multiple copies of mobile bases.
+
+        :param count: used for multiple copies of robots
+        :param num_wheels: number of actuated wheels
+        :param name: string with robot name (same as base in vrep model).
+        """
+
+        joint_names = ['%s_m_joint%s' % (name, str(i + 1)) for i in
+                       range(num_wheels)]
+        super().__init__(count, name, joint_names)
+
+        self.num_wheels = num_wheels
+        suffix = '' if count == 0 else '#%d' % (count - 1)
+
+        wheel_names = ['%s_wheel%s%s' % (name, str(i + 1), suffix) for i in
+                       range(self.num_wheels)]
+        self.wheels = [Shape(name) for name in wheel_names]
+
+        # Motion planning handles
+        self.intermediate_target_base = Dummy(
+            '%s_intermediate_target_base%s' % (name, suffix))
+        self.target_base = Dummy('%s_target_base%s' % (name, suffix))
+
+        self._collision_collection = sim.simGetCollectionHandle(
+            '%s_base%s' % (name, suffix))
+
+        # Robot parameters and handle
+        self.z_pos = self.get_position()[2]
+        self.target_z = self.target_base.get_position()[-1]
+        self.wheel_radius = self.wheels[0].get_bounding_box()[5]  # Z
+        self.wheel_distance = np.linalg.norm(
+            np.array(self.wheels[0].get_position()) -
+            np.array(self.wheels[1].get_position()))
+
+        # Make sure dummies are orphan if loaded with ttm
+        self.intermediate_target_base.set_parent(None)
+        self.target_base.set_parent(None)
+
+    def get_2d_pose(self) -> np.ndarray:
+        """Gets the 2D (top-down) pose of the robot [x, y, yaw].
+
+        :return: A List containing the x, y, yaw (in radians).
+        """
+        return np.r_[self.get_position()[:2], self.get_orientation()[-1:]]
+
+    def set_2d_pose(self, pose: Union[List[float], np.ndarray]) -> None:
+        """Sets the 2D (top-down) pose of the robot [x, y, yaw]
+
+        :param pose: A List containing the x, y, yaw (in radians).
+        """
+        x, y, yaw = pose
+        self.set_position([x, y, self.z_pos])
+        self.set_orientation([0, 0, yaw])
+
+    def assess_collision(self):
+        """Silent detection of the robot base with all other entities present in the scene.
+
+        :return: True if collision is detected
+        """
+        return sim.simCheckCollision(self._collision_collection,
+                                     sim.sim_handle_all) == 1
+
+    def set_cartesian_position(self, position: List[float]):
+        """Set a delta target position (x,y) and rotation position
+
+        :param position: length 3 list containing x and y position, and angle position
+
+        NOTE: not supported for nonholonomic mobile bases.
+        """
+        vel = [0, 0, 0]
+        vel[-1] = position[-1]
+        for i in range(2):
+            vel[i] = position[i] / (0.05 * self.wheel_radius / 2)  # "0.05 is dt"
+
+        self.set_base_angular_velocites(vel)
+
+    def set_base_angular_velocites(self, velocity: List[float]):
+        """This function has no effect for two_wheels robot. More control is required for omnidirectional robot.
+
+        :param velocity: for two wheels robot: each wheel velocity, for omnidirectional robot forwardBackward, leftRight and rotation velocity
+        """
+        raise NotImplementedError()
+
+    def _get_nonlinear_path_points(self, position: List[float],
+                           angle=0,
+                           boundaries=2,
+                           path_pts=600,
+                           ignore_collisions=False,
+                           algorithm=Algos.RRTConnect) -> List[List[float]]:
+        """Gets a non-linear (planned) configuration path given a target pose.
+
+        :param position: The x, y, z position of the target.
+        :param angle: The z orientation of the target (in radians).
+        :param boundaries: A float defining the path search in x and y direction
+        [[-boundaries,boundaries],[-boundaries,boundaries]].
+        :param path_pts: number of sampled points returned from the computed path
+        :param ignore_collisions: If collision checking should be disabled.
+        :param algorithm: Algorithm used to compute path
+        :raises: ConfigurationPathError if no path could be created.
+
+        :return: A non-linear path (x,y,angle) in the xy configuration space.
+        """
+
+        # Base dummy required to be parent of the robot tree
+        # self.base_ref.set_parent(None)
+        # self.set_parent(self.base_ref)
+
+        # Missing the dist1 for intermediate target
+
+        self.target_base.set_position([position[0], position[1], self.target_z])
+        self.target_base.set_orientation([0, 0, angle])
+
+        handle_base = self.get_handle()
+        handle_target_base = self.target_base.get_handle()
+
+        _, ret_floats, _, _ = utils.script_call(
+            'getNonlinearPathMobile@PyRep', PYREP_SCRIPT_TYPE,
+            ints=[handle_base, handle_target_base,
+                  self._collision_collection,
+                  int(ignore_collisions), path_pts], floats=[boundaries],
+                  strings=[algorithm.value])
+
+        # self.set_parent(None)
+        # self.base_ref.set_parent(self)
+
+        if len(ret_floats) == 0:
+            raise ConfigurationPathError('Could not create path.')
+
+        path = []
+        for i in range(0, len(ret_floats) // 3):
+            inst = ret_floats[3 * i:3 * i + 3]
+            if i > 0:
+                dist_change = sqrt((inst[0] - prev_inst[0]) ** 2 + (
+                inst[1] - prev_inst[1]) ** 2)
+            else:
+                dist_change = 0
+            inst.append(dist_change)
+
+            path.append(inst)
+
+            prev_inst = inst
+
+        return path
+
+    def _check_collision_linear_path(self,path):
+        """Check for collision on a linear path from start to goal
+
+        :param path: A list containing start and goal as [x,y,yaw]
+        :return: A bool, True if collision was detected
+        """
+        start = path[0]
+        end = path[1]
+
+        m = (end[1] - start[1])/(end[0] - start[0])
+        b = start[1] - m * start[0]
+        x_range = [start[0],end[0]]
+        x_span = start[0] - end[0]
+
+        incr = round(abs(x_span)/50, 3)
+        if x_range[1] < x_range[0]:
+            incr = - incr
+
+        x = x_range[0]
+        for k in range(50):
+            x += incr
+            y = m * x + b
+            self.set_2d_pose([x,y,start[-1] if k < 46 else end[-1]])
+            status_collision = self.assess_collision()
+            if status_collision == True:
+                break
+
+        return status_collision
+
+    def get_base_actuation(self):
+        """Controller for mobile bases.
+        """
+        raise NotImplementedError()
+
+    def copy(self) -> RobotComponent:
+        self.intermediate_target_base.set_parent(self)
+        self.target_base.set_parent(self)
+        c = super().copy()
+        self.intermediate_target_base.set_parent(None)
+        self.target_base.set_parent(None)
+        return c