In the 2020 MIT Underactuated Robotics course, there is a simple feedback cancellation example by Python. Because I just learn C++ drake, I want to take this example as a practice by converting it to C++ version. I get some trouble. (1) Do it make sense to pass a mutibody_plant instance into Controller VectorSystem? (2) To implement controller, we need addSystem(controller(mutibody_plant)) for builder. Do C++ have similar way to do this? Thank you very much! I attached a Python and C++ (Under Construction) code.
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.
