0

在 2020 年 MIT Underactuated Robotics 课程中，有一个简单的 Python 反馈取消示例。因为我刚学C++ drake，所以想把这个例子转换成C++版本作为练习。我遇到了一些麻烦。(1) 将 mutibody_plant 实例传递给 Controller VectorSystem 是否有意义？(2) 为了实现控制器，我们需要 addSystem(controller(mutibody_plant)) 为builder。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.
4

1 回答 1

0

是的，传递是MultibodyPlant有道理的。有很多例子。例如：

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

于 2020-02-09T20:06:30.067 回答