如何从Drake的Manipulation Station提取IIWA子系统用于Toppra轨迹优化？

核心思路：直接提取IIWA关节的状态索引，避开四元数变量 #

不需要单独提取子Plant或子Diagram，直接从完整Plant中筛选出IIWA关节对应的位置/速度维度，生成仅包含IIWA的子轨迹用于Toppra优化，之后再扩展回完整维度即可。

• 步骤1：获取IIWA关节的位置索引
  从完整Plant中定位IIWA所有关节的位置变量索引，代码示例：

  // 从Plant中获取IIWA的模型实例
  const ModelInstanceIndex iiwa_model = plant.GetModelInstanceByName("iiwa");
  // 获取IIWA所有关节的索引列表
  std::vector<JointIndex> iiwa_joint_indices = plant.GetJointIndices(iiwa_model);
  
  // 收集所有IIWA关节对应的位置变量索引
  std::vector<int> iiwa_pos_indices;
  for (const auto& joint_idx : iiwa_joint_indices) {
      const auto& pos_indices = plant.GetJointPosIndices(joint_idx);
      iiwa_pos_indices.insert(iiwa_pos_indices.end(), pos_indices.begin(), pos_indices.end());
  }
  

• 步骤2：从原始轨迹中提取IIWA子轨迹
  基于上一步的索引，从18维原始轨迹中筛选出仅包含IIWA关节的7维轨迹：

  // original_trajectory是你之前构造的18维分段多项式轨迹
  PiecewisePolynomial<double> iiwa_trajectory = original_trajectory.Select(iiwa_pos_indices);
  

• 步骤3：用Toppra优化IIWA子轨迹
  此时iiwa_trajectory的所有维度都是欧几里得空间的关节变量，完全符合Toppra的要求。构造约束时，记得提取IIWA对应的速度、加速度极限：

  // 获取IIWA关节的速度极限
  Eigen::VectorXd iiwa_vel_limits = plant.GetVelocityLimits().segment(iiwa_pos_indices[0], iiwa_pos_indices.size());
  // 获取IIWA关节的加速度极限
  Eigen::VectorXd iiwa_acc_limits = plant.GetAccelerationLimits().segment(iiwa_pos_indices[0], iiwa_pos_indices.size());
  
  // 构造Toppra的约束和优化器并执行优化
  toppra::LinearConstraint vel_constraint(plant.num_positions(), iiwa_vel_limits, -iiwa_vel_limits);
  toppra::LinearConstraint acc_constraint(plant.num_positions(), iiwa_acc_limits, -iiwa_acc_limits);
  // ... 后续Toppra优化逻辑
  

• 步骤4：将优化后的IIWA轨迹扩展回完整维度
  优化完成后，把7维的IIWA轨迹扩展为18维，保持其他变量（门、夹爪、泡沫块）的轨迹不变：

  std::vector<PiecewisePolynomial<double>::Segment> full_segments;
  for (int i = 0; i < iiwa_optimized_trajectory.num_segments(); ++i) {
      const auto& iiwa_segment = iiwa_optimized_trajectory.segment(i);
      // 初始化完整维度的系数矩阵（可复用原始轨迹的非IIWA部分）
      Eigen::MatrixXd full_coeffs = original_trajectory.segment(i).coeffs();
      
      // 替换IIWA对应的位置索引的系数
      for (int j = 0; j < iiwa_pos_indices.size(); ++j) {
          full_coeffs.row(iiwa_pos_indices[j]) = iiwa_segment.coeffs().row(j);
      }
      
      full_segments.emplace_back(full_coeffs, iiwa_segment.start_time(), iiwa_segment.end_time());
  }
  
  // 构造最终的18维优化轨迹
  PiecewisePolynomial<double> full_optimized_trajectory(full_segments);
  

替代方案：构造仅含IIWA的子Plant（不推荐，更复杂） #

如果一定要用子Plant，可以新建一个MultibodyPlant仅加载IIWA模型，将原始轨迹中的IIWA部分映射到该子Plant进行优化，之后再映射回原Plant的完整状态。但这种方法需要处理模型实例和状态维度的映射，效率和复杂度都不如直接提取索引的方案。

内容的提问来源于stack exchange，提问作者Jake