Конвертировать Python Drake в C ++ Drake - PullRequest
Новая
       6

Конвертировать Python Drake в C ++ Drake

0 голосов
/ 08 февраля 2020

В курсе робототехники «Недеформируемая робототехника» 2020 года приведен простой пример отмены обратной связи на Python. Поскольку я только изучаю C ++ Drake, я хочу взять этот пример на практике, преобразовав его в версию C ++. У меня проблемы. (1) Имеет ли смысл передавать экземпляр mutibody_plant в Controller VectorSystem? (2) Чтобы реализовать контроллер, нам нужен addSystem (controller (mutibody_plant)) для компоновщика. У C ++ есть подобный способ сделать это? Большое спасибо! Я приложил код Python и C ++ (в разработке). 

double Pendulum feedback_cancellation. 
(1) Original Python version (from 2020 MIT Underactuated robotics course):    

class Controller(VectorSystem):
    def __init__(self, multibody_plant, gravity):
        # 4 inputs (double pend state), 2 torque outputs.
        VectorSystem.__init__(self, 4, 2)
        self.plant = multibody_plant
        self.plant_context = self.plant.CreateDefaultContext()
        self.g = gravity
    def DoCalcVectorOutput(self, context_, double_pend_state, unused, torque):   
        q = double_pend_state[:2]
        v = double_pend_state[-2:]
        (M, Cv, tauG, B, tauExt) = ManipulatorDynamics(self.plant, q, v)
        # Desired pendulum parameters.
        length = 2.
        b = .1
        # Control gains for stabilizing the second joint.
        kp = 100
        kd = 10
        # Cancel double pend dynamics and inject single pend dynamics.
        torque[:] = Cv - tauG - tauExt + M.dot(
        [self.g / length * sin(q[0]) - b * v[0], -kp * q[1] - kd * v[1]])

def simulate(gravity=-9.8):

    builder = DiagramBuilder()
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
    parser = Parser(plant, scene_graph)
    parser.AddModelFromFile(FindResource("models/double_pendulum.urdf"))
    plant.Finalize()

    controller = builder.AddSystem(Controller(plant, gravity))

     # The code below is removed.  



(2) C++ double pendulum version:  

// Import something, NameSpace…..which are removed. 
// How do pass mutibody_plant instance into Controller ?

class Controller : public drake::systems::VectorSystem<double> {

 public:
  Controller(): drake::systems::VectorSystem<double>(4,2){
  }

  void DoCalcVectorOutput(
      const drake::systems::Context<double>& context,
      const Eigen::VectorBlock<const Eigen::VectorXd>& input,
      const Eigen::VectorBlock<const Eigen::VectorXd>& state,
      Eigen::VectorBlock<Eigen::VectorXd>* output) const {

      // We need instance of mutibody_plant and Create_default_Context
      // We need M,Cv,touG,B,touExt from InverseDynamic
      //Do Feedback Cancellation Calculation here.
      *output = torque;
  }
};

int do_main() {
  systems::DiagramBuilder<double> builder;
  SceneGraph<double>& scene_graph = *builder.AddSystem<SceneGraph>();
  scene_graph.set_name("scene_graph");
  const double time_step = 0.0;
  const std::string relative_name =
  "drake/multibody/benchmarks/acrobot/acrobot.sdf";
  const std::string full_name = FindResourceOrThrow(relative_name);
  MultibodyPlant<double>& acrobot = *builder.AddSystem<MultibodyPlant>(time_step);
  Parser parser(&acrobot, &scene_graph);
  parser.AddModelFromFile(full_name);
  acrobot.Finalize();

  auto controller = builder.AddSystem<Controller>(acrobot);  // <--- are you sure ?

  //  .... The code below is neglected.

1 Ответ

0 голосов
/ 09 февраля 2020

Да, передача в MultibodyPlant имеет смысл. Там ряд примеров. Например:

https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1controllers_1_1_inverse_dynamics_controller.html

...