Robots.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_rbdyn/Robot.h>
 
#include <mc_rtc/SignalSlot.h>
#include <mc_rtc/iterators.h>
#include <mc_rtc/shared.h>
 
namespace mc_rbdyn
{
 
struct MC_RBDYN_DLLAPI Robots : public mc_rtc::shared<Robots>
{
  friend struct Robot;
  using value_type = mc_rbdyn::Robot;
 
public:
  using iterator = mc_rtc::SharedPtrVectorIterator<Robot>;
  using const_iterator = mc_rtc::SharedPtrVectorConstIterator<Robot>;
  using reverse_iterator = std::reverse_iterator<iterator>;
  using const_reverse_iterator = std::reverse_iterator<const_iterator>;
  using size_type = std::vector<RobotPtr>::size_type;
  const std::vector<mc_rbdyn::RobotModule> & robotModules() const;
 
  std::vector<rbd::MultiBody> & mbs();
  const std::vector<rbd::MultiBody> & mbs() const;
 
  std::vector<rbd::MultiBodyConfig> & mbcs();
  const std::vector<rbd::MultiBodyConfig> & mbcs() const;
 
  bool hasRobot(const std::string & name) const;
 
  inline mc_rbdyn::Robots & robots() noexcept { return *this; }
 
  inline const mc_rbdyn::Robots & robots() const noexcept { return *this; }
 
  unsigned int robotIndex() const;
  unsigned int envIndex() const;
  unsigned int robotIndex(const std::string & name) const;
 
  Robot & load(const std::string & name, const RobotModule & module, const LoadRobotParameters & params = {});
 
  inline Robot & load(const RobotModule & module, const LoadRobotParameters & params = {})
  { return load(module.name, module, params); }
 
  void load(const std::vector<std::shared_ptr<RobotModule>> & modules);
 
  void rename(const std::string & oldName, const std::string & newName);
 
  Robot & loadFromUrdf(const std::string & name,
                       const std::string & urdf,
                       const rbd::parsers::ParserParameters & parser_params = {},
                       const LoadRobotParameters & load_params = {});
 
  void robotCopy(const Robot & robot, const std::string & copyName);
 
  void removeRobot(const std::string & name);
 
  void removeRobot(unsigned int idx);
 
  /* End of Robot(s) loading/unloading functions group */
 
  const RobotModule & robotModule(size_t idx) const;
 
  Robot & robot();
  const Robot & robot() const;
 
  Robot & env();
  const Robot & env() const;
 
  Robot & robot(size_t idx);
  const Robot & robot(size_t idx) const;
 
  Robot & robot(const std::string & name);
  const Robot & robot(const std::string & name) const;
 
  iterator begin() noexcept;
  const_iterator begin() const noexcept;
  const_iterator cbegin() const noexcept;
 
  iterator end() noexcept;
  const_iterator end() const noexcept;
  const_iterator cend() const noexcept;
 
  reverse_iterator rbegin() noexcept;
  const_reverse_iterator rbegin() const noexcept;
  const_reverse_iterator crbegin() const noexcept;
 
  reverse_iterator rend() noexcept;
  const_reverse_iterator rend() const noexcept;
  const_reverse_iterator crend() const noexcept;
  using onRobotAddedSigT = mc_rtc::Signal<std::string>;
  using onRobotAddedSlotT = onRobotAddedSigT::SlotT;
 
  inline onRobotAddedSigT::Proxy onRobotAdded() const noexcept { return onRobotAdded_; }
 
  using onRobotRemovedSigT = mc_rtc::Signal<std::string>;
  using onRobotRemovedSlotT = onRobotRemovedSigT::SlotT;
 
  inline onRobotRemovedSigT::Proxy onRobotRemoved() const noexcept { return onRobotRemoved_; }
  size_type size() const noexcept;
 
  void reserve(size_type new_cap);
 
  inline mc_rbdyn::Robot & operator[](size_t idx)
  {
    assert(idx < robots_.size());
    return *robots_[idx];
  }
 
  inline const mc_rbdyn::Robot & operator[](size_t idx) const
  {
    assert(idx < robots_.size());
    return *robots_[idx];
  }
 
  inline mc_rbdyn::Robot & at(size_t idx)
  {
    assert(idx < robots_.size());
    return *robots_.at(idx);
  }
 
  inline const mc_rbdyn::Robot & at(size_t idx) const
  {
    assert(idx < robots_.size());
    return *robots_.at(idx);
  }
 
  inline static RobotsPtr make() noexcept { return std::make_shared<Robots>(NewRobotsToken{}); }
 
  template<class Deleter>
  inline static RobotsPtr make(Deleter deleter) noexcept
  { return std::shared_ptr<Robots>(new Robots{NewRobotsToken{}}, deleter); }
 
  void copy(mc_rbdyn::Robots & out) const;
 
protected:
  struct NewRobotsToken
  {
  };
 
public:
  Robots(NewRobotsToken);
 
protected:
  Robots(const Robots & rhs) = delete;
  Robots & operator=(const Robots & rhs) = delete;
  Robots(Robots && robots) = delete;
  Robots & operator=(Robots && robots) = delete;
 
  RobotModuleVector robot_modules_;
  std::vector<RobotPtr> robots_;
  std::vector<rbd::MultiBody> mbs_;
  std::vector<rbd::MultiBodyConfig> mbcs_;
  std::vector<rbd::MultiBodyGraph> mbgs_;
  unsigned int robotIndex_;
  unsigned int envIndex_;
  void updateIndexes();
  std::unordered_map<std::string, unsigned int> robotNameToIndex_; 
  mutable onRobotAddedSigT onRobotAdded_;
  mutable onRobotRemovedSigT onRobotRemoved_;
};
 
/* Static pendant of the loader functions to create Robots directly */
MC_RBDYN_DLLAPI RobotsPtr loadRobot(const RobotModule & module, const LoadRobotParameters & params = {});
 
MC_RBDYN_DLLAPI RobotsPtr loadRobot(const std::string & name,
                                    const RobotModule & module,
                                    const LoadRobotParameters & params = {});
 
MC_RBDYN_DLLAPI RobotsPtr loadRobots(const std::vector<std::shared_ptr<RobotModule>> & modules);
 
MC_RBDYN_DLLAPI RobotsPtr loadRobotAndEnv(const RobotModule & module, const RobotModule & envModule);
 
MC_RBDYN_DLLAPI RobotsPtr loadRobotFromUrdf(const std::string & name,
                                            const std::string & urdf,
                                            const rbd::parsers::ParserParameters & parser_params = {},
                                            const LoadRobotParameters & load_params = {});
 
} // namespace mc_rbdyn