问题描述
我试图了解如何使用Drake通过PID控制器来控制简单的双摆。
按照此link上的示例,我创建了一个类似的程序,但我只想尝试在下部关节上使用1个PID,然后看看系统做了什么。 (我知道以这种方式无法达到稳定状态)
from django.db.models import Max
CustomUser.objects.filter(
languages__language_id=1
).annotate(
highest_level=Max('languages__level')
).order_by('highest_level')
MultibodyPlant<double>* dp = builder.AddSystem<MultibodyPlant<double>>(max_time_step);
dp->set_name("plant");
dp->RegisterasSourceForSceneGraph(&scene_graph);
Parser parser(dp);
parser.AddModelFromFile(kDoublePendulumSdfPath);
// Weld the base link to world frame with no rotation.
const auto& root_link = dp->GetBodyByName("base");
dp->AddJoint<WeldJoint>("weld_base",dp->world_body(),std::nullopt,root_link,RigidTransform<double>::Identity());
dp->AddJointActuator("a2",dp->GetJointByName("joint2"));
// Now the plant is complete.
dp->Finalize();
// Create PID Controller.
const Eigen::VectorXd Kp = Eigen::VectorXd::Ones(1) * Kp_;
const Eigen::VectorXd Ki = Eigen::VectorXd::Ones(1) * Ki_;
const Eigen::VectorXd Kd = Eigen::VectorXd::Ones(1) * Kd_;
const auto* const pid = builder.AddSystem<PidController<double>>(Kp,Ki,Kd);
builder.Connect(dp->get_state_output_port(),pid->get_input_port_estimated_state());
builder.Connect(pid->get_output_port_control(),dp->get_actuation_input_port());
//...
雄鸭会支撑不受主动控制的关节,我该如何设置呢?
解决方法
代替构造具有标量Kp,Kd,Ki增益的PID控制器,应使用大小为2的矢量作为增益。由于您只想控制下部关节,因此增益矩阵中只有与下部关节相对应的条目为非零。
我认为您可以将代码更改为
const Eigen::VectorXd Kp = Eigen::Vector2d(0,1) * Kp_;
const Eigen::VectorXd Ki = Eigen::Vector2d(0,1) * Ki_;
const Eigen::VectorXd Kd = Eigen::Vector2d(0,1) * Kd_;
const auto* const pid = builder.AddSystem<PidController<double>>(Kp,Ki,Kd);
我假设您的工厂状态的顺序为(upper_joint_position,lower_joint_position,upper_joint_velocity,lower_joint_velocity)。因此,增益为[0,1]。如果在您的工厂状态下lower_joint_position位于upper_joint_position之前,则增益应为[1,0]